Binocular vision inertial odometer method using landmark point bias pose correction

Abstract

This invention discloses a binocular visual inertial odometry method using landmark offset pose correction. First, the input binocular image undergoes data preprocessing, including feature point extraction, optical flow tracing, IMU pre-integration, and keyframe filtering, resulting in filtered keyframes. Then, a loosely coupled visual and IMU approach is used for rapid visual inertial odometry initialization. Based on the BA optimization objective function and the Jacobian matrix function of pose change, the correlation between landmark offset and pose estimation error is derived. The inverse depth offset of landmarks during optical flow tracing is calculated, and this correlation is used to correct the pose estimation. Finally, based on a tightly coupled back-end nonlinear optimization method, an optimization error function is constructed and iteratively solved to eliminate depth information errors and pose estimation errors, thereby reducing trajectory errors.

Description

Technical Field

[0001] This invention belongs to the field of computer technology, specifically the field of SLAM (Simultaneous Localization and Mapping) technology, and mainly relates to a binocular visual inertial odometry method that utilizes landmark offset pose correction. Background Technology

[0002] For cooperative SLAM systems, loop closure detection includes not only intra-loop closures between non-adjacent frames of a single robot, but also relative pose transformations between frames of different robots, referred to as inter-loop closures. It is precisely the introduction of loop closure detection that eliminates the cumulative error introduced by odometry, ensuring the accuracy and stability of the trajectory and map over a long period. However, the presence of abnormal loops can make it difficult for the optimizer's solution to converge. Therefore, reducing or eliminating the impact of false positive loops during the back-end optimization process can lower the probability of optimization failure and is crucial for improving the robustness of the SLAM system.

[0003] In summary, most methods for eliminating false positive loop closures in a single robot are unsuitable for incremental online elimination. Furthermore, research on eliminating false positive loop closures across multiple robots is limited, and both methods are sensitive to manually set parameters. To address these issues, it is necessary to consider how to first eliminate false positive loop closure measurements from a single robot within a collaborative SLAM system, thus providing reliable support for relative pose transformations among multiple robots. Summary of the Invention

[0004] This invention addresses the problem of insufficient pose estimation accuracy in existing visual inertial odometry (VIO) technologies by providing a binocular VIOMA method that utilizes landmark offset pose correction. First, the input binocular image undergoes data preprocessing, including feature point extraction, optical flow tracing, IMU pre-integration, and keyframe filtering, resulting in filtered keyframes. Next, a loosely coupled visual and IMU approach is used for rapid VIOMA initialization. Based on the BA optimization objective function and the Jacobian matrix function of pose change, the correlation between landmark offset and pose estimation error is derived. The inverse depth offset of landmarks during optical flow tracing is calculated, and this correlation is used to correct the pose estimation. Finally, based on a tightly coupled back-end nonlinear optimization method, an optimization error function is constructed and iteratively solved to eliminate depth information errors and pose estimation errors, thereby reducing trajectory errors.

[0005] To achieve the above objectives, the technical solution adopted by this invention is: a binocular visual inertial odometry method using landmark offset pose correction, comprising the following steps:

[0006] S1, Data preprocessing: The input binocular image is preprocessed to obtain the filtered keyframes; the data preprocessing includes at least feature point extraction and optical flow tracing, IMU pre-integration and keyframe filtering;

[0007] S2, Fast Initialization of Visual Inertial Odometry: Fast initialization of visual inertial odometry is performed using loose coupling between vision and IMU. The stereo camera recovers the feature point depth through triangulation, and the PnP algorithm is used for pure vision-based camera pose estimation. This is then aligned with the IMU pre-integration results to recover the scale factor, gravity, gyroscope bias, and velocity for each frame. The initialization includes at least gyroscope bias correction, initialization of velocity, gravity vector, and scale factor.

[0008] S3, Pose Estimation Correction: Based on the BA optimization objective function and the binocular stereo vision camera observation equation, the correlation between landmark offset and pose estimation error is derived. The inverse depth offset of landmarks during optical flow tracking of feature points is derived, and the correlation is used to correct the pose estimation.

[0009] S4, Back-end Nonlinear Optimization: Based on a tightly coupled back-end nonlinear optimization method, an optimization error function is constructed and iteratively solved to achieve tightly coupled nonlinear optimization of the system state variables to obtain more accurate positioning data; the system state variables include camera pose, IMU accelerometer bias, gyroscope bias, extrinsic parameters from camera to IMU, and inverse depth of 3D landmark points.

[0010] As an improvement of the present invention, step S1 specifically includes:

[0011] S11: The camera pose is estimated using the feature point method, feature points are extracted, and the feature points are matched using the optical flow tracking method;

[0012] S12: The IMU pre-integration method is used to process the relative state quantities between the measurements, and the covariance matrix and Jacobian matrix of the pre-integrated state are given to achieve decoupling from the absolute pose. An error function is constructed to iteratively optimize the pose of the key frame. The relative state quantities include at least the current attitude, velocity and displacement relative to the previous moment.

[0013] S13: Keyframe Filtering: A frame is considered a keyframe if it meets one of the following two conditions:

[0014] (a) Based on disparity judgment, key frames are selected by calculating the disparity of the Shi-Tomasi corner points of optical flow tracking. If the disparity is greater than the set threshold, it is determined to be a key frame.

[0015] (b) Based on the number of feature points, if the number of feature points tracked in an image frame does not exceed the set threshold, it is set as a key frame.

[0016] As an improvement of the present invention, in step S12, the covariance matrix and Jacobian matrix of the pre-integration state are given by the IMU pre-integration method, that is, the Jacobian matrix J at time t+δt is derived. t+δt Iterative relationship:

[0017]

[0018] Where J represents the Jacobian matrix of the error of the IMU measurement, I represents the identity matrix, and F represents the linearity error coefficient.

[0019] As an improvement of the present invention, in step S2, the gyroscope bias is corrected to obtain the gyroscope bias δb. ω :

[0020]

[0021] in, It is b ω It corresponds to the Jacobian matrix. right The partial derivative, It is the transpose of the former. Representing Kronecker, This represents the rotation matrix from frame k-1 to frame k+1. This represents the rotation matrix from the IMU coordinate system corresponding to the k-th frame to the previous frame. vec represents the rotation matrix from the IMU coordinate system to the frame corresponding to the (k+1)th frame, and vec represents matrix vectorization;

[0022] Solve the following equation using Cholesky decomposition to recover the variable to be optimized. This will give us the initialized velocity, gravity vector, and scale factor:

[0023]

[0024] Where I represents the identity matrix, This represents the gravity vector in the reference frame. This represents the speed corresponding to k frames. The speed represents the velocity corresponding to frame k+1, and s represents the scale factor. and Represents the pre-integral term of two adjacent IMUs. This represents the rotation matrix from the reference frame to the k-th frame IMU. Δt represents the rotation matrix from the camera to the reference frame in the k-th frame. k Represents the interval time. This represents the rotation matrix from the IMU to the camera.

[0025] As another improvement of the present invention, the derivation of the correlation between landmark point offset and pose estimation error in step S3 is specifically as follows: The camera pose and 3D points are simultaneously used as optimization variables. The nonlinear least squares problem is modeled as minimizing the reprojection error, i.e., Bundle Adjustment (BA). The objective function for BA optimization is:

[0026]

[0027] Where x represents the camera pose, and p represents the n 3D landmark points in the world coordinate system. i The transformation relationship of feature points zi in pixel coordinates is represented as h(x,p) i Based on the binocular stereo vision camera model, the observation equation is h(x,p). i Since h(x,p) is a nonlinear equation, the above least squares optimization problem needs to be solved using an iterative optimization algorithm. i In the initial value x (k) A first-order Taylor expansion at this point yields:

[0028] h(x,p i )=h(x (k) ,p i )+J x (xx (k) )

[0029] J x h(x,p) i The Jacobian matrix of h(x) with respect to pose x; (k) ,p i In the initial value Perform a first-order Taylor expansion at this point:

[0030]

[0031] h(x) (k) ,p i Regarding the three-dimensional landmark point p i The Jacobian matrix for h(x,p) i The pose update of ) is solved using Gauss-Newton iteration, and is expressed as:

[0032]

[0033] In the formula For J x By combining the pseudo-inverse matrix and the first-order Taylor expansion above, we can obtain:

[0034]

[0035] Pose estimation error x during iteration (k+1) -x (k) Represented as ∈ x , This represents the measurement error in the pixel coordinate system. This indicates the error of the three-dimensional landmark point;

[0036] The expected value of the pose estimation error determined by the offsets of n 3D landmark points can be expressed as:

[0037]

[0038] Where matrix I 3n×3n The format is as follows:

[0039]

[0040] Among them, J x h(x) (k) ,p i The Jacobian matrix for pose x, For J x The inverse matrix, J p h(x) (k) ,p i The Jacobian matrix of a three-dimensional landmark point p, ∈ p It is the error of three-digit landmark points, μ p It is the measurement error bias.

[0041] As another improvement of the present invention, in step S3, the derivation of the inverse depth bias of the landmark point extraction by binocular vision, the average inverse depth bias μ(d) L ) is represented as:

[0042]

[0043]

[0044] in, It is the inverse depth value. It is the average inverse depth bias. This refers to the depth offset of a specific frame. As another improvement of the present invention, step S3 further includes landmark point filtering, using the following strategy based on the camera's angular velocity:

[0045]

[0046] Where, ||ω t ||2 represents the l2 norm of the camera motion angular velocity given by the IMU. When the calculated landmark offset is greater than the threshold set for the corresponding situation, it is discarded.

[0047] As another improvement of the present invention, step S4 improves the tightly coupled back-end nonlinear optimization of the binocular vision reprojection error term. The objective function of the back-end is constructed to include three errors: monocular vision reprojection error, IMU measurement error and marginalization information. The Levenberg-Marquardt (LM) method is used to iteratively optimize and solve the objective function.

[0048] The reprojection error of the binocular camera is represented by a three-dimensional vector:

[0049]

[0050] in, and f and c represent the 3D coordinates of the points in the camera coordinate system after the external parameter transformation in the world coordinate system. x c y These are the internal references for the left and right eye cameras.

[0051] Compared with existing technologies, this invention proposes a visual inertial odometry (VIO) method for pose correction using landmark offsets. By analyzing the correlation between the expected pose estimation error and the 3D landmark offset, the derived inverse depth offset of the 3D landmarks in optical flow tracking is used to eliminate both depth information errors and pose estimation errors. This method can be easily combined with existing VIO methods that offer superior overall performance. The algorithm effectively reduces the RMSE (Real-Time Error Sequence) and MAE (Modular Error Estimate) of the absolute trajectory error, and significantly improves the positioning accuracy calculated based on both metrics. Furthermore, experiments on the average computation time per image frame demonstrate that although adding the pose correction step slightly increases the computation time, it still meets the real-time computation requirements of embedded devices. Attached Figure Description

[0052] Figure 1 This is a flowchart of the steps of the method of the present invention;

[0053] Figure 2 This is a flowchart of the binocular image feature extraction and point pair matching process in step S1 of the method of the present invention;

[0054] Figure 3 This is a schematic diagram illustrating the loosely coupled visual-inertial initialization effect in step S2 of the method of the present invention;

[0055] Figure 4 This is a schematic diagram of a pinhole camera model.

[0056] Figure 5 This is a schematic diagram of a binocular camera model.

[0057] Figure 6 This is a comparison diagram of the method of the present invention and the VINS-Mono trajectory, and a comparison diagram of the trajectories in the x, y, and z directions, wherein:

[0058] Figure 6 (a) is a comparison chart of the MH_02_easy sequence trajectory;

[0059] Figure 6 (b) is a comparison diagram of the trajectory of the V2_03_difficult sequence;

[0060] Figure 6 (c) is a comparison chart of the three-directional trajectories of MH_02_easy;

[0061] Figure 6 (d) is a comparison chart of the three-directional trajectories of V2_03_difficu lt. Detailed Implementation

[0062] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the invention.

[0063] Example 1

[0064] A binocular visual inertial odometry method utilizing landmark offset pose correction, such as Figure 1 As shown, it includes the following steps:

[0065] Step S1: Data preprocessing includes extracting feature points from the input binocular images and performing optical flow tracing, IMU pre-integration, and keyframe filtering;

[0066] (11) Extract feature points and match feature points using optical flow tracing method

[0067] The camera pose is estimated using the feature point method, and then extracted and optically tracked to achieve a balance between the speed and accuracy of the visual inertial odometry front end. The process of binocular image feature extraction and optical flow tracking is as follows: Figure 2 As shown, a binocular camera is used. First, feature extraction is performed, followed by optical flow tracking. The optical flow tracking of feature points between the left-eye images is used to solve for inter-frame correspondences, while the optical flow tracking of feature points between the left and right-eye images, along with reverse optical flow tracking, is used for triangulation to prepare for motion estimation and pose correction using the PnP algorithm.

[0068] Visual localization employs the Shi-Tomasi feature point extraction algorithm, which is an improvement on Harris feature point extraction. It can calculate good features to track, resulting in a more uniform feature distribution and achieving sub-pixel level detection accuracy.

[0069] (12) Error analysis of IMU pre-integration and pre-integration term

[0070] To address the issue of repeated integration of state variables in backend optimization algorithms, IMU pre-integration technology processes the relative state variables between measurements, achieving decoupling from absolute pose, and constructing an error function for iterative optimization of keyframe pose. The continuous form and the discrete form based on the median method of the IMU pre-integration term are derived. Furthermore, to apply IMU pre-integration to nonlinear optimization, the error term is analyzed, and the covariance matrix and Jacobian matrix of the pre-integrated state are given.

[0071] Consider two adjacent image frames c k and c k+1 The IMU coordinate system corresponds to b respectively k and b k+1 Since the IMU's acquisition frequency is much higher than the camera's image acquisition frequency, then b k+1 The three state variables of frame position, velocity, and direction need to be determined according to [t]. k ,t k+1 All IMU measurements within the time interval are transferred in the world coordinate system by integrating the measurements, as follows:

[0072]

[0073] In the above formula The quaternion right-multiplication symbol on the right side of the equation is represented in the following matrix form, [ω]. × It is an antisymmetric matrix of the following form:

[0074]

[0075] In formula (1), Δt k For [t] k ,t k+1 The time interval between ] This indicates a transformation from the IMU coordinate system to the world coordinate system. The rotation of the IMU coordinate system to the frame at the current moment is represented by a quaternion.

[0076] Error analysis of the pre-integral term and derivation of its Jacobian and covariance matrix representing the weights of the error term: The error of the IMU measurement at the next time step is linearly related to the error of the measurement at the previous time step. Based on the current value, the mean and covariance at the next time step are predicted. According to the error propagation formula, the covariance matrix at time t+δt can be derived from the covariance matrix at time t.

[0077]

[0078] In the above formula, Q represents b k The initial covariance of the noise term, the initial values ​​of the covariance matrix of the error term, and the matrix Q at frame time are as follows:

[0079]

[0080] At the same time, the Jacobian matrix J at time t+δt can be calculated according to formula (3). t+δt There is an iterative relationship:

[0081]

[0082] The above are the derived covariance matrix and Jacobian matrix of the pre-integral term.

[0083] (13) Keyframe Filtering

[0084] In the preprocessing module, key frame (KF) filtering improves system efficiency by eliminating image frames with high information redundancy. Initialization is considered complete when the number of key frames in the camera input image is not less than the set sliding window size (set to 10). Key frame filtering eliminates image frames with high information redundancy based on the following two conditions: (a) By calculating the disparity of the Shi-Tomasi corner points in optical flow tracking, if the disparity is greater than a set threshold, the tracking quality of this image frame is good, and it is identified as a key frame; (b) If the number of feature points tracked in an image frame does not exceed a set threshold, the image frame has low information redundancy, and it is also identified as a key frame. Considering the possibility of mismatched feature points during optical flow tracking, the RANSAC algorithm is used to improve matching accuracy.

[0085] Step S2: Loose coupling between vision and IMU enables rapid initialization of visual inertial odometry;

[0086] A fast initialization scheme using loose coupling between vision and IMU is employed. The binocular camera first recovers the depth of feature points through triangulation, then uses the PnP algorithm for pure vision-based camera pose estimation, i.e., the pose of all frames within the sliding window and the positions of all landmarks in 3D space. This is then aligned with the IMU pre-integration results to recover the scale factor, gravity, gyroscope bias, and velocity for each frame. The alignment effect between camera motion and IMU pre-integration is shown below. Figure 3 As shown:

[0087] (21) Gyroscope bias correction

[0088] The accelerometer bias value and gravitational acceleration are coupled together. Since the accelerometer bias is generally small during initialization and difficult to calculate directly, its influence is ignored. During initialization, it is not calculated again, and only the gyroscope bias is corrected. For adjacent image frames, the rotation between adjacent frames given by visual motion estimation should be equal to the rotation value pre-integrated by the IMU. The objective function is constructed as follows:

[0089]

[0090] in:

[0091]

[0092] b represents the small perturbation that increases the pre-integral rotation value. Let bω be the Jacobian matrix corresponding to equation (5). right The partial derivative of , the minimum value of the objective function of equation (6) is a unit quaternion, which can be further written as:

[0093]

[0094] by() vec The symbol represents the imaginary part of a quaternion, from which we can derive:

[0095]

[0096] By converting the left side of equation (9) into a positive definite matrix, the gyroscope bias δbω can be directly solved using Cholesky decomposition:

[0097]

[0098] (22) Initialize velocity, gravity vector and scale factor

[0099] The variables to be optimized, initially defined as velocity, gravity vector, and scale factor, are:

[0100]

[0101] in Let be the velocity of the n+1 image frames in the sliding window, and s be the scale factor. Let the gravity vector be the reference frame of the camera. The residual is defined as the pre-integral term of two adjacent IMU data points. and The error between the sum and the predicted value and

[0102]

[0103] Where Δt is the time between two adjacent image frames, substituting the frame pose transformation formula into the above equation... From the item, we can obtain:

[0104]

[0105] To utilize Cholesky decomposition, the above expression is transformed into the form Hx = b:

[0106]

[0107] Represented in matrix form as follows:

[0108]

[0109] Similarly, Converting to matrix form, the comprehensive formula (15) can be written as:

[0110]

[0111] By using Cholesky decomposition to solve formula (16), the variable to be optimized can be recovered. This will give you the initialized velocity, gravity vector, and scale factor.

[0112] Step S3: Using the derived correlation between landmark offset and pose estimation error, calculate the inverse depth offset of landmark during optical flow tracking of feature points, and use the correlation to correct the pose estimation.

[0113] (31) Correlation analysis between pose estimation error and 3D landmark offset

[0114] When both camera pose and 3D points are used as optimization variables, the nonlinear least squares problem is modeled as minimizing the reprojection error. This type of problem is collectively called Bundle Adjustment (BA), and the objective function for BA optimization is:

[0115]

[0116] In the formula, x represents the camera pose, and p represents the three-dimensional landmark points in the n world coordinate system. i The transformation relationship of feature points zi in pixel coordinates is represented as h(x,p) i h(x,p) i Since h(x,p) is a nonlinear equation, the above least squares optimization problem needs to be solved using an iterative optimization algorithm. i In the initial value x (k) A first-order Taylor expansion at this point yields:

[0117] h(x,p i )=h(x (k) ,p i )+J x (xx (k) (18)

[0118] J x h(x,p) i The Jacobian matrix of h(x) with respect to pose x. Similarly, for h(x) (k) ,p i In the initial value Perform a first-order Taylor expansion at this point:

[0119]

[0120] J pi h(x) (k) ,p i Regarding the three-dimensional landmark point p i The Jacobian matrix is ​​used to solve the pose update of formula (18) using Gauss-Newton iteration, and is expressed as:

[0121] x (k+1) =x (k) -J x + (h(x (k) ,p i )-z i (20)

[0122] In the formula J x + For J x By combining the pseudo-inverse matrix and equation (19), we can obtain:

[0123]

[0124] Pose estimation error x during iteration (k+1) -x (k) Represented as ∈ x , This represents the measurement error in the pixel coordinate system. This indicates the error of the 3D landmark points. It can be seen that the iterative results of the Gauss-Newton method for pose estimation are mainly affected by... and The influence of measurement error. Assume that the measurement error follows a zero-mean Gaussian distribution. Assume that the landmark points follow a Gaussian distribution with non-zero mean. Its bias Due to the influence of image matching accuracy and disparity angle, it is also possible to minimize the reprojection error through BA optimization. Based on the above conditions, the expected value of the pose estimation error determined by the offsets of n 3D landmark points can be expressed as:

[0125]

[0126] Where matrix I 3n×3n The format is as follows:

[0127]

[0128] Thus, the relationship between the expected pose estimation error and the offset of the 3D landmark points is derived, and the pose error can be corrected based on the offset of the landmark points.

[0129] (32) Derivation of inverse depth bias for binocular vision extraction of landmark points

[0130] Use v t =[v x ,v y ,v z ] T and ω t =[ω x ,ω y ,ω z ] T Let z represent the linear velocity and angular velocity of the camera at time t, respectively. Feature point tracking is performed using optical flow, and z is used as the angular velocity. i (x i ,y i () represents the pixel coordinates of the extracted feature point in the pixel coordinate system, and its horizontal and vertical velocities are respectively:

[0131]

[0132] In the formula, f is the camera focal length, (x FOE ,y FOE ) represents the coordinates of the focus of expansion (FOE) in the optical flow field, d(x) i ,y i ) represents the scaling inverse depth v z / Z iLet the number of optical flow tracking feature points be n, and normalize the focal length. For ease of derivation, the symbols and their meanings are defined in this embodiment as shown in Table 1:

[0133] Table 1. Symbols for the derivation of inverse depth offset of landmarks.

[0134]

[0135]

[0136] Note: The diag symbol represents a diagonal matrix.

[0137] Based on formula (24), the matrix form of n feature points for optical flow tracking is obtained as follows:

[0138]

[0139] Analyzing formula (25), the angular velocity [ω] at each moment t ] 3n×1 This can be directly provided by the IMU sensor in a visual inertial odometry system, and the above formula can be further written as:

[0140] Ad = b (26)

[0141] in:

[0142]

[0143] Solving the above overpositive definite equations using the least squares method yields:

[0144]

[0145] in Calculated z i (x i ,y i Inverse depth estimation bias for:

[0146]

[0147] in This represents the noise covariance of feature points in the pixel coordinate system. The noise covariance is related to the reprojection error when calibrating the camera intrinsic parameters, which is a deviation of several pixel values. In the implementation, it is fixed at 1.5 pixel values. ix and s ix They represent r respectively i and s i For x i Partial derivative, r iy and s iy Then it means r i and si For y i Partial derivative. Using a stereo camera to estimate the feature point depth, suppose there are L frames of stereo images used to estimate the depth of the n optical flow tracking points. The inverse depth value d calculated from the L stereo frames is then used... L And the average inverse depth offset μ(d) calculated by formula (29) L ) is represented as:

[0148]

[0149] use The symbol represents the true inverse depth of a 3D point. The expected inverse depth calculated by the stereo frame then satisfies... If d u =d L -μ(d L ), then satisfy Therefore, d is called u To correct for the inverse depth. Thus, based on μ(d) L By combining the relationship between pose estimation error and landmark offset in formula (22) and calculating the reciprocal of the inverse depth, the pose estimation can be corrected.

[0150] Regarding the use of binocular cameras to estimate feature point depth, analysis of the binocular camera imaging model allows for a better study of the impact of images on pose estimation and point cloud data extraction.

[0151] The pinhole camera model, a widely used fundamental model, is used to explain the imaging process from a 3D point onto the camera's imaging plane. The pinhole camera model is as follows: Figure 4 As shown, O c -Z c The axis points in front of the camera, and the camera's focal length is f. Based on the pinhole imaging principle and the similar triangle theorem, the following transformation relationship exists between a spatial point P(X,Y,Z) and a point P'(x',y') on the imaging plane:

[0152]

[0153] It should be noted that the negative sign in formula (31) indicates image reversal. In practice, the camera will flip the image on the imaging plane. Therefore, the imaging plane can be equivalently placed symmetrically in front of the camera with the camera plane as the center. Formula (31) becomes:

[0154]

[0155] After simplification, the coordinates of the spatial point in the image coordinate system are:

[0156]

[0157] The image coordinate system is transformed into pixel coordinates Ouv, with the origin defined at the top left corner of the image, through translation and scaling. Let α be the scaling factor from the origin to the pixel coordinate Ouv, β be the scaling factor, and [C] be the translation amount. x C y ] T Then the pixel coordinates can be expressed as

[0158]

[0159] Combining this with formula (33), we get:

[0160]

[0161] In the formula, f is the camera focal length in meters; α is the unit dimension of the Ox axis in pixels per meter; β is the unit dimension of the Oy axis in pixels per meter; f x and f y The unit is pixels. After rearranging it into a matrix, we get:

[0162]

[0163] The left side of the equation represents the pixel coordinates in homogeneous coordinates. The coordinates of point P in the equation are its coordinates in the camera coordinate system. Considering the camera's motion in space, the coordinates of point P in the camera coordinate system can be obtained by transforming its coordinates in the world coordinate system according to the camera pose. Let the camera's position and pose in the world coordinate system be respectively represented by the translation vector t. cam With rotation matrix R cam The description includes:

[0164]

[0165] Where P w The translation vector and rotation matrix represent the position in the world coordinate system, which are the values ​​to be estimated for pose estimation using the visual-inertial odometry method. The camera trajectory is also represented. Two monocular cameras are placed horizontally along a horizontal line to form a stereo camera system. Figure 5 As shown, the distance between the optical centers of the left and right cameras is the baseline of the binocular cameras. By simultaneously acquiring two images (left and right) and knowing the baseline length of the binocular cameras, the feature information of three-dimensional points in space can be calculated.

[0166] Figure 5 In the middle, O L O R These are the optical centers of the left and right cameras, respectively; f is the focal length; u... L u R These are the pixel coordinates on the u-axis, and b is the optical center O. L To O RThe baseline length between them. Based on the above binocular camera model, the distance from 3D point P to the camera can be calculated as:

[0167]

[0168] Where d is the difference in pixel coordinates between the left and right images, called parallax.

[0169] (33) Road sign selection strategy

[0170] Incorrect feature point matching will cause the 3D landmark point to be biased by μ. p The inverse depth offset becomes very large, so directly using the estimated inverse depth offset to correct the pose may reduce the stability of the SLAM algorithm. To avoid this problem, it is necessary to determine which landmark offsets can be used for pose correction. This embodiment uses the following strategy for selection based on camera motion angular velocity:

[0171]

[0172] Where ||ω t ||2 represents the l2 norm of the camera motion angular velocity given by the IMU. The basic principle is that the faster the camera moves, the greater the parallax of the feature points, and the more accurate the depth estimation of the feature points. As shown in formula (39), when the calculated landmark point offset is greater than the threshold set in the corresponding case, it is discarded to avoid the impact of feature point mismatch and improve the stability of the visual inertial odometry system.

[0173] Step S4: Based on the tightly coupled back-end nonlinear optimization method, construct the optimization error function and solve iteratively.

[0174] Following the visual-inertial odometry initialization and pose correction steps, a tightly coupled nonlinear optimization is performed on the system state variable X in the backend to obtain more accurate positioning data. The system-defined state variables include: n+1 camera poses within a sliding window after pose correction. IMU accelerometer bias b a and gyroscope bias b ω And external parameters from the camera to the IMU (including position vectors) and rotation The inverse depth d of m+1 three-dimensional landmarks j (0≤j≤m), therefore it can be expressed as:

[0175]

[0176] To solve for the optimal state variable X * The objective function for building the backend is:

[0177]

[0178] In formula (41), the objective function includes three errors: monocular visual reprojection error, IMU measurement error, and marginalization information. The Levenberg-Marquardt (LM) method is used to iteratively optimize and solve the objective function. The construction of IMU measurement error and marginalization information is consistent with VINS-Mono. Since the camera input sensor type is binocular, the visual reprojection error needs to be improved to reduce the construction complexity of binocular visual error. Based on the camera projection model, any 3D landmark point in the world coordinate system is projected into the pixel coordinate system. The reprojection error of the left camera can be expressed as:

[0179]

[0180] in, Let the pixel coordinates in the left image be... and Let x and y coordinates be the coordinates. This represents the projection relationship from the world coordinate system to the left camera's pixel coordinate system. If reprojection errors are constructed for both left and right cameras, a four-degree-of-freedom error vector will be generated. To simplify, using a binocular camera vision model, we assume the ordinates in the corrected binocular camera images are the same. Therefore, we only need to calculate the abscissa of the landmark point projection in the right image. The reprojection error of the binocular camera is represented by a three-dimensional vector:

[0181]

[0182] in and f and c represent the 3D coordinates of the points in the camera coordinate system after the external parameter transformation in the world coordinate system. x c y For the intrinsic parameters of the left and right cameras, use Equation (43) instead of the visual reprojection error in Equation (42) to reduce the dimension of the error vector, reduce the computational complexity of the back-end optimization, and improve the computational speed.

[0183] Experimental test case

[0184] This design is an improvement on the original VINS-Mono visual inertial odometry. By fully considering and deriving the offset information during the extraction of 3D landmarks by a binocular camera, and analyzing the correlation between pose estimation error and 3D landmark offset, the pose localization results of the visual inertial odometry are corrected. To verify the computational speed and localization accuracy of the proposed method, a comparative experiment on localization accuracy and average processing time per image frame was conducted on an embedded device using the EuRoC dataset for visual inertial SLAM algorithms.

[0185] The embedded hardware device used to run the standalone client visual inertial odometry in the experiment was the NVIDIA Jetson Xavier NX, which is suitable for drones, small commercial robots and other IoT embedded systems. Comparative experiments were conducted on a public dataset, using the EuRoc aircraft dataset.

[0186] Two metrics from the Absolute Trajectory Error (ATE) methodology—Root Mean Square Error (RMSE) and Mean Absolute Error (MAE)—are used to measure the pose estimation accuracy of the SLAM system. The absolute trajectory error measures the degree of consistency between the estimated trajectory and the real trajectory, i.e., trajectory accuracy. It calculates the position difference between the real and estimated trajectories after registration, and can be used to evaluate the pose estimation results of the SLAM algorithm. Both the mean absolute error and the root mean square error can reduce the impact of noise or randomness on the evaluation results. Therefore, this experiment records RMSE and MAE to comprehensively analyze the algorithm performance. Let the real motion trajectory of the entity be x = {x1, x2, ... x...} n The system estimates the camera trajectory as x′={x′1,x′2,…x′}. n}, then we have:

[0187]

[0188]

[0189] Where *tran* represents the translation vector of the pose. To more intuitively demonstrate the improvement in positioning accuracy, the comparative experiment also involved calculating the performance improvement:

[0190]

[0191] Where θ represents the improvement in positioning accuracy, β is the trajectory error of the improved algorithm, and α is the trajectory error of the basic algorithm. Furthermore, to evaluate the impact of the improved method on time, the average processing time of the improved algorithm for each image frame was calculated on different dataset sequences, demonstrating that the algorithm can meet the requirements of real-time computing.

[0192] Once the visual-inertial odometry (VIO) completes its loosely coupled initialization, a pose correction step is performed. The corrected camera pose and inverse depth data within the sliding window replace the variables to be optimized in the original VIN-Mono system for nonlinear optimization. This algorithm can improve the odometry's localization accuracy by providing better initial values ​​for the backend algorithm. Note that in the comparative experiments, in addition to using a landmark selection strategy, this algorithm also limits the number of landmarks involved in pose correction in each keyframe to less than 50. This parameter is based on the fact that, in both the original VINS-Mono system and the improved system proposed in this chapter, the maximum number of feature points extracted from each frame of the visual odometry front-end does not exceed 150. Extensive practical testing has demonstrated that limiting the number of landmarks involved in pose correction to one-third effectively balances the accuracy after pose correction with the computational efficiency of the odometry system.

[0193] In order to make the test results more accurate and reliable, 10 experiments were conducted on each data sequence, and the average RMSE and MAE of the 10 experimental results were calculated accordingly. The results of the original visual inertial odometry in VINS-Mono and the method were compared on the Jetson Xavier NX device as shown in the table below (RMSE and MAE accuracy are retained to three decimal places).

[0194] Comparison of RMSE index in table positioning accuracy test

[0195]

[0196]

[0197] Comparison of MAE indicators in table positioning accuracy test

[0198]

[0199] The results above show that, in the ten test sequences, the proposed method outperforms the original visual-inertial odometry method in both accuracy metrics. The best RMSE and MAE improvements in positioning accuracy were achieved in the test sequence V2_03_difficult, at 34.22% and 30.93%, respectively. On the EuRoc aircraft dataset with an average trajectory length of 82.045m, the algorithm achieved an average RMSE improvement of 12.22% and an average MAE improvement of 13.63%. To more intuitively represent the improvement in positioning accuracy, we have plotted the trajectory comparison between the proposed algorithm and the original VINS-Mono in two different scenarios: data sequences MH_02_easy and V2_03_difficult, as well as the comparison results with the ground truth trajectory in three directions over time. (See attached figures.) Figure 6 As shown, where Figure 6 (a) is a comparison chart of the MH_02_easy sequence trajectory; Figure 6 (b) is a comparison diagram of the trajectory of the V2_03_difficult sequence; Figure 6 (c) is a comparison chart of the three-directional trajectories of MH_02_easy; Figure 6 (d) is a comparison of the three-directional trajectories of V2_03_difficult. The dashed lines in the figure represent the true trajectory values ​​given by the Euroc dataset. The two solid lines marked at the points represent the trajectories calculated by the original VINS-Mono system and the trajectories calculated by this method, respectively. As can be seen from the boxed part in the figure below, in each data sequence, the solid lines marked by this method are closer to the true trajectory values, which proves that this algorithm can improve the positioning accuracy of visual inertial odometry after correcting the pose.

[0200] Meanwhile, to calculate the impact of the pose correction step introduced in this method on the computation time, the average computation time of a single image frame in the vi_ns_estimator node of the ROS framework was compared with that of the original VINS-Mono system. The computation time includes both pose correction and backend optimization. This is because the feature_tracker node of both algorithms only performs feature extraction and optical flow tracking, and publishes feature point messages for vi_ns_estimator to subscribe to and output keyframe poses. However, this method changes the front-end monocular camera sensor input to binocular. After actual testing and comparison on embedded devices (using the GPU on Jetson Xavier NX for acceleration), the time difference is negligible. The time test results are shown in the table below, where the computation difference is the average computation time of this algorithm minus the average computation time of the VINS-Mono algorithm. Similarly, to make the results more reliable, 10 experiments were conducted on each data sequence, and the average computation time of the 10 experiments was calculated.

[0201] Average computation time per image frame in the vins_estimator node

[0202]

[0203] The results in the table above show that the time differences in the four data sequences V1_02_medium, V1_03_difficult, V2_01_easy, and V2_03_difficult are significantly lower than those in other sequences, indicating that the addition of the error correction step has a relatively low impact on the computation time of the original visual inertial odometry algorithm. This is because the calculation of landmark offsets in this algorithm accounts for most of the increased time consumption. Furthermore, the four datasets mentioned above are all Vicon Room scenes, mostly consisting of white walls, and the texture information in these scenes is not as rich as in the Machine Hall scenes of MH_01–05. Therefore, the number of landmark offsets that need to be calculated is relatively small. Figure 6 (b) It can be seen that, due to the rapid change in the aircraft's pose in the indoor Vicon Room scene, and the limited number of landmarks involved in pose correction, the time impact of the pose correction step is relatively low. Calculations show that the average increment time across the ten dataset sequences is 8.829 ms, representing 19.26% of the average increment time of the original visual-inertial odometry. Time consumption experiments verify that this method can achieve real-time calculations on embedded devices.

[0204] In summary, this algorithm eliminates depth information errors and pose estimation errors, thereby improving the positioning accuracy of visual inertial odometry.

[0205] It should be noted that the above content merely illustrates the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. For those skilled in the art, various improvements and modifications can be made without departing from the principle of the present invention, and all such improvements and modifications fall within the scope of protection of the claims of the present invention.

Claims

1. A binocular vision inertial odometry method using landmark point bias pose correction, characterized in that, The method comprises the following steps: S1, data preprocessing: data preprocessing is performed on the input binocular image to obtain a screened key frame; the data preprocessing at least comprises extracting feature points and performing optical flow tracking, IMU pre-integration and key frame screening; S2, visual inertial odometry rapid initialization: loose coupling of vision and IMU is adopted for visual inertial odometry rapid initialization, a feature point depth is recovered by triangulation of a binocular camera, camera pose estimation is performed by a PnP algorithm, and then the camera pose estimation is aligned with an IMU pre-integration result to recover a scale factor, a gravity, a gyroscope bias and a speed of each frame; the initialization at least comprises gyroscope bias correction, initialization speed, gravity vector and scale factor; S3, pose estimation correction: according to a target function of BA optimization and a binocular stereo vision camera observation equation, a relationship between a landmark point bias and a pose estimation error is derived, an inverse depth bias of a landmark point in a feature point tracking process is derived, and the pose estimation is corrected by using the relationship; S4, rear-end nonlinear optimization: a rear-end nonlinear optimization method based on tight coupling is adopted to construct an optimization error function for iterative solving, and a system state quantity is subjected to tight coupling nonlinear optimization to obtain more accurate positioning data; the system state quantity comprises a camera pose, an IMU accelerometer bias, a gyroscope bias, a camera-to-IMU external parameter and an inverse depth of a three-dimensional landmark point.

2. The binocular vision inertial odometry method using landmark point bias pose correction of claim 1, wherein: The step S1 specifically comprises: S11: a camera pose is estimated by using a feature point method, feature points are extracted, and the feature points are matched by using an optical flow tracking method; S12: a relative state quantity between measurement values is processed by using an IMU pre-integration method, a covariance matrix and a Jacobian matrix of a pre-integrated state are given, decoupling with an absolute pose is realized, and an error function is constructed for iterative optimization of a key frame pose; the relative state quantity at least comprises a current attitude and speed, displacement relative to a previous time; S13: key frame screening: one of the following two conditions is met to determine a key frame: (a) according to a parallax judgment, a parallax of a Shi-Tomasi corner point tracked by an optical flow is calculated to screen a key frame, and if the parallax is greater than a set threshold, the key frame is determined; (b) according to a feature point quantity judgment, if a feature point quantity tracked by an image frame does not exceed a set threshold, the image frame is set as a key frame.

3. The binocular vision inertial odometry method using landmark point bias pose correction of claim 2, wherein: In the step S12, the covariance matrix of the pre-integrated state and the Jacobian matrix are given by the IMU pre-integration method, that is, the following iterative relationship is derived the Jacobian matrix at time ​ ； Wherein, J represents a Jacobian matrix of an error of an IMU measurement value; I represents a unit matrix; F represents a linear error coefficient.

4. The binocular vision inertial odometry method using landmark point bias pose correction of claim 2, wherein: The step S2, the gyroscope bias correction, the gyroscope bias : ； wherein is the Jacobian matrix in the corresponding Jacobian matrix the partial derivative of is the transpose matrix of the former, denotes the Kronecker product; denotes the rotation matrix from k-1 frame to k+1 frame; denotes the rotation matrix from the k-th frame corresponding IMU coordinate system to the last frame; denotes the rotation matrix from the k+1-th frame corresponding IMU coordinate system to the frame, and vec denotes the matrix vectorization;​ The initialized velocity, gravity vector and scale factor are obtained by solving the following equation using Cholesky decomposition i.e. ； where I represents an identity matrix; represents the gravity vector in the reference frame; represents the velocity corresponding to the kth frame represents the velocity corresponding to the k+1th frame; S represents a scale factor; and represents the pre-integration term of the adjacent two IMUs; represents the rotation matrix of the reference frame to the kth frame IMU represents the rotation matrix of the kth frame camera to the reference frame; represents the interval time; represents the rotation matrix of the IMU to the camera.

5. The binocular vision inertial odometry method using landmark point bias pose correction according to claim 3 or 4, characterized in that: The step S3 of deriving the relationship between the landmark point bias and the pose estimation error specifically comprises: The camera pose and the three-dimensional point are simultaneously taken as optimization variables, a nonlinear least square problem is modeled as minimizing a re-projection error, that is, bundle adjustment (BA), and a target function of BA optimization is: ； where x is the camera pose, and n 3D landmarks in the world coordinate system The transformation of the feature points into the pixel coordinate system is denoted as According to the binocular stereo vision camera model, the observation equation is is a nonlinear equation, the above least squares optimization problem needs to be solved by an iterative optimization algorithm, and the Jacobian matrix is calculated at the initial value The first-order Taylor expansion is obtained as​ ； For The Jacobian matrix with respect to the poses x; for A first order Taylor expansion is performed at the initial value x0: ； For Regarding the Jacobian matrix of the three-dimensional landmark points The pose update of the is solved using a Gauss-Newton iteration, denoted as: ； where is the pseudo-inverse matrix of The joint derivation of the above first-order Taylor expansion and the pseudo-inverse matrix gives ； pose estimation error x in iteration (k+1) - x (k) is denoted as , denotes the measurement error in pixel coordinate system, denotes the three-dimensional landmark point error; the expectation of pose estimation error determined by one three-dimensional landmark point bias can be represented as: ； where the matrix is of the form: ； wherein is the Jacobian matrix with respect to the pose x, is the inverse matrix of is the Jacobian matrix with respect to the three-dimensional landmark point p, is the three-dimensional landmark point error, is the measurement error bias.

6. The binocular vision inertial odometry method using landmark point bias pose correction of claim 5, wherein: The step S3 derives binocular vision extraction road sign point inverse depth bias, average inverse depth bias is expressed as: ； wherein, is an inverse depth value, is an average inverse depth bias, is a depth bias for a certain frame.

7. The binocular vision inertial odometry method using landmark point bias pose correction of claim 5, wherein: The step S3 further comprises landmark point screening, and a screening strategy according to a camera motion angular velocity is as follows: ； wherein, represents the angular velocity of the camera motion given by the IMU norm, when the calculated landmark point bias is greater than a threshold set for the corresponding case, it is discarded.

Images