A moving platform relative pose estimation method based on a multi-camera system

Abstract

The application relates to the technical field of visual navigation, and provides a moving platform relative position estimation method based on a multi-camera system, which comprises the following steps: synchronously collecting images at three continuous time points by using a multi-camera system in a moving platform, and forming image pairs by using the images at the three continuous time points; tracking and matching point pairs by combining a FAST operator and a pyramid LK optical flow method; removing mismatched point pairs based on a forward-backward error strategy; randomly selecting a matching point pair, solving the relative positions of the moving platform at the three continuous time points based on an Ackermann motion model, judging inliers by using the space points corresponding to each matching point pair to project and calculate the re-projection error of each solution, taking the inlier with the maximum total number as a better solution, taking the inlier with the larger total number from the better solution of the present iteration and the optimal solution of the last iteration as the optimal solution of the present iteration, until the iteration is completed, and taking the optimal solution as the relative position. The application can improve the precision and efficiency of the moving platform relative position estimation, reduce the cost, and is suitable for scenes with a lack of textures.

Description

Technical Field

[0001] This invention relates to the field of visual navigation technology, and in particular to a method for relative pose estimation of a moving platform based on a multi-camera system. Background Technology

[0002] Currently, relative pose estimation of moving platforms largely relies on sensors such as radar, laser, and GPS. These sensors are large and expensive, while small and inexpensive cameras are generally only used as auxiliary sensors. Due to factors such as size and cost, cameras are commonly installed on moving platforms such as ships, aircraft, automobiles, especially drones, unmanned vehicles, wheeled robots, and mobile phones, making cameras a promising candidate to become the primary sensor for moving platform pose estimation tasks.

[0003] To fully utilize the advantages of cameras in relative pose estimation of moving platforms, many pure vision relative pose estimation methods have emerged in the existing technology. Although these pure vision relative pose estimation methods have many advantages, they face various problems in practical engineering applications: (1) Some methods cannot meet the requirements of high real-time performance and low computational cost in practical engineering applications due to offline processing, high computational cost, or slow operation speed, especially the high real-time performance requirements in fields such as autonomous driving; (2) Some methods have high requirements for processors, which increases the cost of moving platform pose estimation systems; (3) Some methods are only applicable to some special image and video acquisition devices, and their applicability and portability are not high; (4) Pose estimation methods usually require a large number of feature matching point pairs, and it is difficult to complete the pose estimation task independently in scenes lacking texture. The above problems have led to the fact that existing pure vision relative pose estimation methods are still rarely used in relative pose estimation of moving platforms, especially in the field of autonomous driving. Therefore, there is an urgent need for more efficient pure vision relative pose estimation methods. Summary of the Invention

[0004] To address the problems existing in the prior art, this invention provides a method for relative pose estimation of a moving platform based on a multi-camera system, which can improve the accuracy and efficiency of relative pose estimation of a moving platform, reduce costs, and is applicable to scenes lacking texture.

[0005] The technical solution of this invention is as follows:

[0006] A method for relative pose estimation of a moving platform based on a multi-camera system includes the following steps:

[0007] Step 1: Obtain image pairs

[0008] The system utilizes all cameras in the multi-camera system of the moving platform to synchronously acquire images of the surrounding environment at three consecutive time points. At each time point, one image is randomly selected from the images acquired by all cameras. The images selected at three consecutive time points form an image pair. Multiple selections are used to obtain a set of image pairs.

[0009] Step 2: Track matching point pairs

[0010] For each image pair in the image pair set, the FAST operator is used to detect the feature points of the first frame image. The pyramid LK optical flow method is used to forward track the feature points extracted by the FAST operator in the second and third frames image respectively. The three feature points obtained constitute a matching point pair. The matching point pairs of all image pairs constitute the initial set of matching point pairs.

[0011] Step 3: Remove mismatched pairs

[0012] Based on the forward and backward error strategy, mismatched point pairs in the initial set of matching point pairs are removed to obtain the set of matching point pairs;

[0013] Step 4: Estimate relative pose based on Ackerman motion constraints and RANSAC framework

[0014] Step 4.1: Randomly select a matching point pair from the set of matching point pairs;

[0015] Step 4.2: Based on the Ackerman motion model of the moving platform, the relative pose of the moving platform between three consecutive time points is solved using the selected matching point pairs to obtain multiple solutions;

[0016] Step 4.3: For each solution, using the relative pose corresponding to the solution, reproject the spatial 3D points corresponding to each matching point pair in the matching point pair set onto the image plane, calculate the reprojection error of each matching point pair, and determine that the matching point pair is an interior point when the reprojection error is less than or equal to the preset error threshold, and determine that the matching point pair is an exterior point when the reprojection error is greater than the preset error threshold. Record the total number of interior points under the solution, and take the solution with the largest total number of interior points among all solutions of the selected matching point pairs as the better solution. Take the solution with the larger total number of interior points between the better solution of this iteration and the best solution of the previous iteration as the best solution of this iteration.

[0017] Step 4.4: Repeat steps 4.1 to 4.3 until the iteration ends, and use the pose corresponding to the optimal solution of the last iteration as the relative pose of the moving platform for three consecutive time moments.

[0018] Furthermore, step 3 specifically includes the following steps:

[0019] Step 3.1: For each matching point pair in the initial set of matching point pairs, using the feature points tracked in the third frame image as a reference, use the pyramid LK optical flow method to reverse track the corresponding feature points in the second frame image and the first frame image.

[0020] Step 3.2: Calculate the coordinate difference between the feature points extracted forward by the FAST operator and the feature points traced backward by the pyramid LK optical flow method in the first frame image;

[0021] Step 3.3: Remove matching point pairs whose coordinate difference exceeds the set coordinate difference threshold, and the remaining matching point pairs form a matching point pair set.

[0022] Furthermore, in step 4.2, the Ackerman motion model of the moving platform is as follows: the multi-camera system in the moving platform continuously performs uniform Ackerman motion, performs circular motion around the instantaneous rotation center ICR when turning, and performs circular motion with an infinite radius when moving in a straight line. The polar coordinates of the multi-camera system are (ρ, δ), the x-axis of the multi-camera system is perpendicular to the radius of the circular motion, and the y-axis points to the instantaneous rotation center ICR. θ is the yaw angle of the multi-camera system at the previous and next time steps, and ρ is the translation vector scale of the multi-camera system at the previous and next time steps.

[0023] Furthermore, in step 4.2, the relative pose of the moving platform at three consecutive time points is solved using the selected matching point pairs, specifically including:

[0024] In the selected matching point pair (p1, p2, p3), p1, p2, and p3 are the image points of spatial point P at three consecutive times k, k+1, and k+2, respectively, and their Plücker line coordinates are respectively Assuming the moving platform has the same velocity within a short time interval, the relative pose parameters (ρ, θ) are the same at consecutive times k, k+1, and k+2. The relative rotation matrix R and relative translation vector t of two adjacent frames are respectively...

[0025]

[0026] Substituting equation (1) into the generalized camera epipolar constraint... In the process, the new generalized camera epipolar constraints between time k and k+1, and between k+1 and k+2, are obtained as follows:

[0027]

[0028]

[0029] in,

[0030] Using the half-angle formula of trigonometric functions

[0031]

[0032]

[0033]

[0034] make Combining equations (4) to (6), and using equations (2) and (3), we obtain two expressions for the translation vector scale ρ:

[0035]

[0036]

[0037] therefore

[0038]

[0039] From equation (9), we get

[0040] f(n) = En 2 +Fn+G=0 (10)

[0041] Where E, F, and G are all functions of m,

[0042]

[0043] Let H = m 2 ,but It can be written as g(n) = n 2 +H-1=0;

[0044] The necessary and sufficient condition for f(n) and g(n) to have a common root is that the resultant is...

[0045]

[0046] This yields a 6th-order equation for m.

[0047] Am 6 +Bm 4 +Cm 2 +D=0 (13)

[0048] Among them, A, B, C and D are composed of coefficients (a1, b1, c1, d1, e1) and (a2, b2, c2, d2, e2);

[0049] When the matching points are from three frames of images from the same camera, a1 = -e1, a2 = -e2, then

[0050]

[0051] Let q = m2 Then equation (13) simplifies to

[0052] q(Aq 2 +Bq 1 +C)=0 (15)

[0053] Thus, we obtain three solutions for q.

[0054]

[0055] Since q = m 2 Thus, we obtain 5 solutions for m, one of which is the zero solution;

[0056] According to the formula From equation (7), we obtain the yaw angle θ = 2arcsin(m) and the translation vector scale. Substituting into equation (1), we obtain multiple values ​​of the relative pose (R,t) of the moving platform between three consecutive moments.

[0057] Furthermore, in step 4.3, for each solution, using the relative pose corresponding to that solution, the spatial 3D points corresponding to each matching point pair in the matching point pair set are reprojected onto the image plane, and the reprojection error of each matching point pair is calculated, specifically including:

[0058] Using the relative pose corresponding to the solution, the feature points in the first frame image corresponding to each matching point pair are projected onto the second frame image. The viewing direction vector of the reprojected image point is calculated based on the image point coordinates and relative pose of the feature points in the first frame image. Calculate the line-of-sight vector of the measured image point based on the image point coordinates of the feature points in the second frame image. Calculate the reprojection error between the first frame and the second frame.

[0059] Using the relative pose corresponding to the solution, the feature points in the second frame image corresponding to each matching point pair are projected onto the third frame image. The viewing direction vector of the reprojected image point is calculated based on the image point coordinates and relative pose of the feature points in the second frame image. Calculate the line-of-sight vector of the measured image point based on the image point coordinates of the feature points in the third frame image. Calculate the reprojection error between the second and third frame images as follows:

[0060] Calculate the reprojection error of this matching point pair as ε. multi =ε 12 +ε 23 .

[0061] The beneficial effects of this invention are as follows:

[0062] (1) This invention utilizes a multi-camera system in a moving platform to synchronously acquire images at three consecutive time points. Using image pairs at three consecutive time points as image sequence processing units, it tracks matching point pairs and eliminates mismatched point pairs. In the RANSAC framework estimation, the Ackerman motion model based on the moving platform uses a randomly selected matching point pair to solve the relative pose of the moving platform at three consecutive time points. Each solution is used to reproject the corresponding 3D spatial points of each matching point pair and calculate the reprojection error. Based on the reprojection error, inliers are determined, and the solution with the largest total number of inliers is selected as the better solution. Through continuous iteration, the solution with the larger total number of inliers between the better solution of each iteration and the optimal solution of the previous iteration is selected as the optimal solution of each iteration. The final optimal solution is used as the final estimated relative pose. On the one hand, it realizes that the relative pose of the moving platform can be solved using only one matching point pair at three consecutive time points. The relative pose between moments is applicable to texture-deficient scenarios, solving the technical problem that existing pure visual relative pose estimation methods, which require a large number of feature matching point pairs, struggle to independently complete pose estimation tasks in texture-deficient scenarios. Furthermore, it cleverly integrates pose solving based on the Ackerman motion model, reprojection based on the solution, and error calculation into the point judgment within the RANSAC framework. The resulting optimal solution accurately reflects the relative pose of the moving platform at three consecutive moments, improving the accuracy of relative pose estimation. The entire process is simple, and the ability to estimate pose at a single point significantly reduces algorithm complexity, computational cost, and computational speed. It can be processed quickly online with low processor requirements, greatly improving the efficiency and reducing the cost of relative pose estimation for moving platforms.

[0063] (2) This invention detects feature points of the first frame image using the FAST operator, tracks feature points of the next two frames image forward using the pyramid LK optical flow method, and tracks feature points backward using the pyramid LK optical flow method. Based on the forward and backward error strategy, it removes mismatched point pairs from the initial set of matching point pairs. This can improve the accuracy of matching point pair tracking and mismatched point pair removal, lay a good foundation for pose solving, reprojection, and interior point judgment in subsequent RANSAC, reduce the burden of subsequent relative pose optimization algorithms, and further improve the accuracy and efficiency of relative pose estimation of the moving platform. Attached Figure Description

[0064] Figure 1 This is a flowchart of the relative pose estimation method for a moving platform based on a multi-camera system according to the present invention.

[0065] Figure 2 This is a flowchart of the three-view feature point detection and tracking method for relative pose estimation of a moving platform based on a multi-camera system according to the present invention.

[0066] Figure 3 This is a schematic diagram of the Ackermann motion model.

[0067] Figure 4 This is a schematic diagram of the uniform Ackerman motion of the moving platform in consecutive frames in the relative pose estimation method of the moving platform based on a multi-camera system of the present invention.

[0068] Figure 5 This is a schematic diagram of the multi-camera system reprojection in the dynamic platform relative pose estimation method based on a multi-camera system of the present invention. Detailed Implementation

[0069] The present invention will now be further described in conjunction with the accompanying drawings and specific embodiments.

[0070] like Figure 1 As shown, the relative pose estimation method for a moving platform based on a multi-camera system of the present invention includes the following steps:

[0071] Step 1: Obtain image pairs

[0072] The system utilizes all cameras in the multi-camera system of the moving platform to synchronously acquire images of the surrounding environment at three consecutive time points. At each time point, one image is randomly selected from the images acquired by all cameras. The images selected at three consecutive time points form an image pair. Multiple selections are used to obtain a set of image pairs.

[0073] Step 2: Track matching point pairs

[0074] For each image pair in the image pair set, the FAST operator is used to detect the feature points of the first frame image. The pyramid LK optical flow method is used to forward track the feature points extracted by the FAST operator in the second and third frames image respectively. The three feature points obtained constitute a matching point pair. The matching point pairs of all image pairs constitute the initial set of matching point pairs.

[0075] Step 3: Remove mismatched pairs

[0076] Based on the forward and backward error strategy, mismatched point pairs in the initial set of matching point pairs are removed to obtain the set of matching point pairs;

[0077] Step 4: Estimate relative pose based on Ackerman motion constraints and RANSAC framework

[0078] Step 4.1: Randomly select a matching point pair from the set of matching point pairs;

[0079] Step 4.2: Based on the Ackerman motion model of the moving platform, the relative pose of the moving platform between three consecutive time points is solved using the selected matching point pairs to obtain multiple solutions;

[0080] Step 4.3: For each solution, using the relative pose corresponding to the solution, reproject the spatial 3D points corresponding to each matching point pair in the matching point pair set onto the image plane, calculate the reprojection error of each matching point pair, and determine that the matching point pair is an interior point when the reprojection error is less than or equal to the preset error threshold, and determine that the matching point pair is an exterior point when the reprojection error is greater than the preset error threshold. Record the total number of interior points under the solution, and take the solution with the largest total number of interior points among all solutions of the selected matching point pairs as the better solution. Take the solution with the larger total number of interior points between the better solution of this iteration and the best solution of the previous iteration as the best solution of this iteration.

[0081] Step 4.4: Repeat steps 4.1 to 4.3 until the iteration ends, and use the pose corresponding to the optimal solution of the last iteration as the relative pose of the moving platform for three consecutive time moments.

[0082] The moving platform described in this invention can be a ship, aircraft, automobile, wheeled robot, or other moving platform. In this embodiment, an unmanned vehicle is used as an example to illustrate the method of this invention. This invention first utilizes a multi-camera system in the unmanned vehicle to simultaneously acquire images at three consecutive time points. The image pairs (referred to as three views) at the three consecutive time points are used as image sequence processing units, employing methods such as... Figure 2 The method shown tracks matching point pairs and removes false matching point pairs. Specifically, the steps for removing false matching point pairs in step 3 are as follows:

[0083] Step 3.1: For each matching point pair in the initial set of matching point pairs, using the feature points tracked in the third frame image as a reference, use the pyramid LK optical flow method to reverse track the corresponding feature points in the second frame image and the first frame image.

[0084] Step 3.2: Calculate the coordinate difference between the feature points extracted forward by the FAST operator and the feature points traced backward by the pyramid LK optical flow method in the first frame image; where, (x,y) are the coordinates of the feature points extracted by the FAST operator in the first frame image, and (x',y') are the coordinates of the feature points tracked in the reverse direction by the pyramid LK optical flow method in the first frame image.

[0085] Step 3.3: Remove matching point pairs whose coordinate difference exceeds the set coordinate difference threshold, and the remaining matching point pairs form a matching point pair set.

[0086] This invention detects feature points in the first frame of an image pair using the FAST operator, forward tracks feature points in the next two frames using the pyramid LK optical flow method, and backward tracks feature points using the pyramid LK optical flow method. Based on a forward and backward error strategy, it removes mismatched point pairs from the initial set of matching point pairs. This improves the accuracy of matching point pair tracking and mismatched point pair removal, laying a good foundation for pose solving, reprojection, and interior point determination in subsequent RANSAC operations. It also reduces the burden on subsequent relative pose optimization algorithms and further improves the accuracy and efficiency of relative pose estimation for the moving platform.

[0087] After tracing matching point pairs and initially eliminating erroneous matching point pairs, this invention uses matching point pairs to estimate the pose of the moving platform, and introduces the RANSAC framework for further filtering of matching point pairs, ultimately obtaining the optimal solution.

[0088] To address the technical problem that existing pure visual relative pose estimation methods require a large number of feature matching point pairs, making it difficult to independently complete the pose estimation task in scenarios lacking texture, this invention utilizes Ackerman motion constraints and a single matching point pair to solve for the relative pose of the moving platform between three consecutive time points.

[0089] Specifically, when designing wheeled motion platform systems such as aircraft landing gear, automobiles, unmanned vehicles, and wheeled robots, Ackermann steering geometry theory is typically used to address the issue of the different centers of the inner and outer steering wheel paths during turning. This ensures that the centers of the four wheel paths intersect at the instantaneous center of rotation (ICR) on the extended rear axle, thus preventing sideslip during turning. This invention utilizes the nonholonomic constraint condition of the wheeled motion platform—that is, the instantaneous center of rotation during its motion—to construct an Ackermann motion model of the platform. Intuitively, the driver changes the rotation angle of the two front wheels by turning the steering wheel, making them slightly different, thus causing the vehicle to instantly perform a circular motion around the ICR. Therefore, the wheeled motion platform can be considered to perform local circular motion in a plane. For linear motion, it can be viewed as the platform moving in a circle around an infinite radius. This circular motion model is called the Ackermann motion model, such as... Figure 3 As shown. Clearly, this motion model can effectively reduce the degrees of freedom in the relative pose estimation problem.

[0090] The Ackerman motion model of the moving platform in this invention is as follows: the multi-camera system in the moving platform continuously performs uniform Ackerman motion. During turns, it performs circular motion around the instantaneous rotation center ICR, and during straight-line motion, it performs circular motion with an infinite radius. The polar coordinates of the multi-camera system are (ρ, δ). The x-axis of the multi-camera system points forward perpendicular to the radius of the circular motion, and the y-axis points towards the instantaneous rotation center ICR. θ represents the yaw angle of the multi-camera system at the preceding and following time points, and ρ represents the translation vector scale of the multi-camera system at the preceding and following time points. When the moving platform performs Ackerman motion, the relative pose estimation problem has only three unknown parameters: the yaw angle θ, the polar coordinates (ρ, δ), and since the x-axis of the multi-camera system is perpendicular to the radius... Thus, the degrees of freedom for the multi-camera relative pose estimation problem based on the Ackerman motion model are reduced to 2, requiring only two parameters: the translation vector scale ρ and the yaw angle θ, to perform relative pose estimation. Figure 4 This indicates that the multi-camera system is moving in a circle around the ICR, satisfying the Ackerman motion model. The linear motion of the multi-camera system can be regarded as circular motion with an infinite radius, satisfying the yaw angle θ = 0 and the polar coordinates δ = 0.

[0091] This invention is based on the Ackerman motion model of a moving platform, and uses selected matching point pairs to solve for the relative pose of the moving platform at three consecutive time points, specifically including:

[0092] In the selected matching point pair (p1, p2, p3), p1, p2, and p3 are the image points of spatial point P at three consecutive times k, k+1, and k+2, respectively, and their Plücker line coordinates are respectively Assuming the moving platform has the same velocity within a short time interval, then the relative pose parameters (ρ, θ) at consecutive times k, k+1, and k+2 are the same. According to... Figure 4 In a multi-camera system coordinate system, the x-axis points forward, the y-axis points to the instantaneous rotation center ICR, and the z-axis points to the plane of the paper. Then, the relative rotation matrix R and the relative translation vector t between two adjacent frames are respectively...

[0093]

[0094] Substituting equation (1) into the generalized camera epipolar constraint In this process, a new generalized camera polar constraint can be obtained. Wherein, coefficients a, b, c, d, and e are the coefficients formed by combining the elements of the Plücker linear coordinates of feature points in two adjacent frames. From the new generalized camera epipolar constraint formula, it can be seen that a pair of matched points in two consecutive frames can yield a constraint equation. However, the relative pose estimation problem of a multi-camera system has two unknowns; therefore, estimating the relative pose between two consecutive time points requires two pairs of matched points. Based on practical applications, a reasonable assumption is made: the moving platform has the same speed (including rotational speed) within a short time interval, that is, the motion parameters are the same between consecutive time points, such as... Figure 4 As shown. Based on the above assumptions, this invention proposes a multi-camera relative pose estimation algorithm based on a single point in a three-view drawing. It can simultaneously estimate the relative pose parameters of the multi-camera system at three consecutive time points using only the imaging of a single spatial point at three consecutive time points. Therefore, substituting equation (1) into the generalized camera epipolar constraint... In the process, the new generalized camera epipolar constraints between time k and k+1, and between k+1 and k+2, are obtained as follows:

[0095]

[0096]

[0097] in,

[0098] Using the half-angle formula of trigonometric functions

[0099]

[0100]

[0101]

[0102] make Combining equations (4) to (6), and using equations (2) and (3), we obtain two expressions for the translation vector scale ρ:

[0103]

[0104]

[0105] therefore

[0106]

[0107] From equation (9), we get

[0108] f(n) = En 2 +Fn+G=0 (10)

[0109] Where E, F, and G are all functions of m,

[0110]

[0111] Let H = m 2 ,but It can be written as g(n) = n 2 +H-1=0;

[0112] The necessary and sufficient condition for f(n) and g(n) to have a common root is that the resultant is...

[0113]

[0114] This yields a 6th-order equation for m.

[0115] Am 6 +Bm 4 +Cm 2 +D=0 (13)

[0116] Among them, A, B, C and D are composed of coefficients (a1, b1, c1, d1, e1) and (a2, b2, c2, d2, e2);

[0117] When the matching points are from three frames of images from the same camera, a1 = -e1, a2 = -e2, then

[0118]

[0119] Let q = m 2 Then equation (13) simplifies to

[0120] q(Aq 2 +Bq 1 +C)=0 (15)

[0121] Thus, we obtain three solutions for q.

[0122]

[0123] Since q = m 2 Thus, we obtain 5 solutions for m, one of which is the zero solution;

[0124] According to the formula From equation (7), we obtain the yaw angle θ = 2arcsin(m) and the translation vector scale. Substituting into equation (1), we obtain multiple values ​​of the relative pose (R,t) of the moving platform between three consecutive moments.

[0125] This invention enables the calculation of the relative pose of a moving platform across three consecutive time points using only one matching point pair. It is applicable to scenarios lacking texture and solves the technical problem that existing pure visual relative pose estimation methods, which require a large number of feature matching point pairs, struggle to independently complete pose estimation tasks in scenarios lacking texture.

[0126] After obtaining multiple solutions by using selected matching point pairs to solve for the relative pose of the moving platform over three consecutive time points, it is necessary to filter the multiple solutions obtained from a single matching point pair to obtain a better solution. Simultaneously, it is also necessary to filter the better solutions from all matching point pairs to obtain the optimal solution. For example... Figure 5 As shown, for each solution, using the relative pose corresponding to that solution, the spatial 3D points corresponding to each matching point pair in the matching point pair set are reprojected onto the image plane, and the reprojection error of each matching point pair is calculated, specifically including:

[0127] Using the relative pose corresponding to the solution, the feature points in the first frame image corresponding to each matching point pair are projected onto the second frame image. The viewing direction vector of the reprojected image point is calculated based on the image point coordinates and relative pose of the feature points in the first frame image. Calculate the line-of-sight vector of the measured image point based on the image point coordinates of the feature points in the second frame image. Calculate the reprojection error between the first frame and the second frame. in, The physical meaning of α is the cosine of the angle α between two vectors, cosα, which is between -1 and 1. Theoretically, if the estimated pose has no error, the two line-of-sight vectors coincide, i.e. It equals 1;

[0128] Using the relative pose corresponding to the solution, the feature points in the second frame image corresponding to each matching point pair are projected onto the third frame image. The viewing direction vector of the reprojected image point is calculated based on the image point coordinates and relative pose of the feature points in the second frame image. Calculate the line-of-sight vector of the measured image point based on the image point coordinates of the feature points in the third frame image. Calculate the reprojection error between the second and third frame images as follows:

[0129] Calculate the reprojection error of this matching point pair as ε. multi =ε 12 +ε 23 .

[0130] This invention calculates the reprojection error, determines the interior points based on the reprojection error, selects the solution with the largest total number of interior points as the better solution, and iteratively selects the solution with the larger total number of interior points between the better solution of each iteration and the optimal solution of the previous iteration as the optimal solution for each iteration. The final optimal solution is used as the final estimated relative pose. This invention cleverly integrates pose solving based on the Ackerman motion model, reprojection based on the solution, and error calculation into the determination of interior points within the RANSAC framework. The final optimal solution can accurately reflect the relative pose of the moving platform at three consecutive moments, improving the accuracy of relative pose estimation for the moving platform. The entire process is simple, and the ability to estimate pose from a single point greatly reduces the complexity of the algorithm, reduces computational costs, and improves the computing speed. It can be processed quickly online with low processor requirements, greatly improving the efficiency and reducing the cost of relative pose estimation for the moving platform.

[0131] Obviously, the above embodiments are merely some, not all, of the embodiments of the present invention. The above embodiments are only used to explain the present invention and do not constitute a limitation on the scope of protection of the present invention. Based on the above embodiments, all other embodiments obtained by those skilled in the art without inventive effort, that is, all modifications, equivalent substitutions, and improvements made within the spirit and principle of this application, fall within the scope of protection claimed by the present invention.

Claims

1. A method for relative pose estimation of a moving platform based on a multi-camera system, characterized in that, Includes the following steps: Step 1: Obtain image pairs The system utilizes all cameras in the multi-camera system of the moving platform to synchronously acquire images of the surrounding environment at three consecutive time points. At each time point, one image is randomly selected from the images acquired by all cameras. The images selected at three consecutive time points form an image pair. Multiple selections are used to obtain a set of image pairs. Step 2: Track matching point pairs For each image pair in the image pair set, the FAST operator is used to detect the feature points of the first frame image. The pyramid LK optical flow method is used to forward track the feature points extracted by the FAST operator in the second and third frames image respectively. The three feature points obtained constitute a matching point pair. The matching point pairs of all image pairs constitute the initial set of matching point pairs. Step 3: Remove mismatched pairs Based on the forward and backward error strategy, mismatched point pairs in the initial set of matching point pairs are removed to obtain the set of matching point pairs; Step 4: Estimate relative pose based on Ackerman motion constraints and RANSAC framework Step 4.1: Randomly select a matching point pair from the set of matching point pairs; Step 4.2: Based on the Ackerman motion model of the moving platform, the relative pose of the moving platform between three consecutive time points is solved using the selected matching point pairs to obtain multiple solutions; Step 4.3: For each solution, using the relative pose corresponding to the solution, reproject the spatial 3D points corresponding to each matching point pair in the matching point pair set onto the image plane, calculate the reprojection error of each matching point pair, and determine that the matching point pair is an interior point when the reprojection error is less than or equal to the preset error threshold, and determine that the matching point pair is an exterior point when the reprojection error is greater than the preset error threshold. Record the total number of interior points under the solution, and take the solution with the largest total number of interior points among all solutions of the selected matching point pairs as the better solution. Take the solution with the larger total number of interior points between the better solution of this iteration and the best solution of the previous iteration as the best solution of this iteration. Step 4.4: Repeat steps 4.1 to 4.3 until the iteration ends, and use the pose corresponding to the optimal solution of the last iteration as the relative pose of the moving platform for three consecutive time moments.

2. The method for relative pose estimation of a moving platform based on a multi-camera system according to claim 1, characterized in that, Step 3 specifically includes the following steps: Step 3.1: For each matching point pair in the initial set of matching point pairs, using the feature points tracked in the third frame image as a reference, use the pyramid LK optical flow method to reverse track the corresponding feature points in the second frame image and the first frame image. Step 3.2: Calculate the coordinate difference between the feature points extracted forward by the FAST operator and the feature points traced backward by the pyramid LK optical flow method in the first frame image; Step 3.3: Remove matching point pairs whose coordinate difference exceeds the set coordinate difference threshold, and the remaining matching point pairs form a matching point pair set.

3. The method for relative pose estimation of a moving platform based on a multi-camera system according to claim 1, characterized in that, In step 4.2, the Ackerman motion model of the moving platform is as follows: the multi-camera system in the moving platform continuously performs uniform Ackerman motion, performs circular motion around the instantaneous rotation center ICR when turning, and performs circular motion with an infinite radius when moving in a straight line. The polar coordinates of the multi-camera system are (ρ, δ). The x-axis of the multi-camera system is perpendicular to the radius of the circular motion, and the y-axis points to the instantaneous rotation center ICR. θ is the yaw angle of the multi-camera system at the previous and next time steps, and ρ is the translation vector scale of the multi-camera system at the previous and next time steps.

4. The method for relative pose estimation of a moving platform based on a multi-camera system according to claim 3, characterized in that, In step 4.2, the relative pose of the moving platform at three consecutive time points is solved using the selected matching points, specifically including: In the selected matching point pair (p1, p2, p3), p1, p2, and p3 are the image points of spatial point P at three consecutive times k, k+1, and k+2, respectively, and their Plücker line coordinates are respectively Assuming the moving platform has the same velocity within a short time interval, the relative pose parameters (ρ, θ) are the same at consecutive times k, k+1, and k+2. The relative rotation matrix R and relative translation vector t of two adjacent frames are respectively... Substituting equation (1) into the generalized camera epipolar constraint... In the process, the new generalized camera epipolar constraints between time k and k+1, and between k+1 and k+2, are obtained as follows: in, Using the half-angle formula of trigonometric functions make Combining equations (4) to (6), and using equations (2) and (3), we obtain two expressions for the translation vector scale ρ: therefore From equation (9), we get f(n)=A 2 +Fn+G=0 (10) Where E, F, and G are all functions of m, Let H = m 2 ,but It can be written as g(n) = n 2 +H-1=0; The necessary and sufficient condition for f(n) and g(n) to have a common root is that the resultant is... This yields a 6th-order equation for m. Am 6 +Bm 4 +Cm 2 +D=0 (13) Among them, A, B, C and D are composed of coefficients (a1, b1, c1, d1, e1) and (a2, b2, c2, d2, e2); When the matching points are from three frames of images from the same camera, a1 = -e1, a2 = -e2, then Let q = m 2 Then equation (13) simplifies to q(Aq 2 +Bq 1 +C)=0 (15) Thus, we obtain three solutions for q. Since q = m 2 Thus, we obtain 5 solutions for m, one of which is the zero solution; According to the formula From equation (7), we obtain the yaw angle θ = 2arcsin(m) and the translation vector scale. Substituting into equation (1), we obtain multiple values ​​of the relative pose (R,t) of the moving platform between three consecutive moments.

5. The method for relative pose estimation of a moving platform based on a multi-camera system according to claim 4, characterized in that, In step 4.3, for each solution, using the relative pose corresponding to that solution, the spatial 3D points corresponding to each matching point pair in the matching point pair set are reprojected onto the image plane, and the reprojection error of each matching point pair is calculated, specifically including: Using the relative pose corresponding to the solution, the feature points in the first frame image corresponding to each matching point pair are projected onto the second frame image. The viewing direction vector of the reprojected image point is calculated based on the image point coordinates and relative pose of the feature points in the first frame image. Calculate the line-of-sight vector of the measured image point based on the image point coordinates of the feature points in the second frame image. Calculate the reprojection error between the first frame and the second frame. Using the relative pose corresponding to the solution, the feature points in the second frame image corresponding to each matching point pair are projected onto the third frame image. The viewing direction vector of the reprojected image point is calculated based on the image point coordinates and relative pose of the feature points in the second frame image. Calculate the line-of-sight vector of the measured image point based on the image point coordinates of the feature points in the third frame image. Calculate the reprojection error between the second and third frames. Calculate the reprojection error of this matching point pair as ε. multi =ε 12 +ε 23 .

Images