Initialization method suitable for monocular VIO system under low motion excitation

Abstract

The invention belongs to the technical field of navigation positioning, and discloses an initialization method suitable for a monocular VIO system under low motion excitation, and the initialization method comprises the following steps: screening N key frames from a video stream shot by a visual camera; calculating relative pose transformation and three-dimensional space point coordinates between two adjacent key frames one by one to obtain SfM of a fuzzy scale; updating the SfM of the fuzzy scale to the SfM of the absolute scale by using the scale factor; and adjusting and optimizing the SfM of the absolute scale by using an improved bundle method, combining the optimized SfM with IMU pre-integration in the monocular VIO system, constructing a maximum posterior probability estimation function, and solving the maximum posterior probability estimation function to obtain initialization parameters of the monocular VIO system, the initialization parameters at least comprise a rotation matrix in the gravity direction, zero offset of an accelerometer, zero offset of a gyroscope and the speed of each frame in the N key frame sets. According to the method, parameter initialization of the VIO system can still be completed under the condition that motion excitation is insufficient, and good robustness is achieved.

Description

Technical Field

[0001] The present invention belongs to the technical field of navigation and positioning, and in particular relates to an initialization method for a monocular VIO system under low motion excitation. Background Art

[0002] Monocular vision-inertial odometry (VIO) is an important method in the field of navigation. Its purpose is to combine the visual information captured by the monocular camera and the spatial attitude information collected by the inertial measurement unit (IMU) to infer the pose information of the carrier itself and construct the three-dimensional spatial structure of the environment. Since the monocular camera and IMU can complement each other's shortcomings and both are low-priced and cost-effective, this technology is widely used in the fields of drone positioning, autonomous driving, virtual reality and augmented reality. Among them, initialization technology is an important part of monocular VIO. In the initial stage of system operation, the initial pose of the carrier and the initial map of the environment are determined, and the initial parameters of the IMU are determined. A good initialization method can provide good initial values ​​for subsequent processes, improve the calculation convergence speed, and enhance the robustness of the system; conversely, a poor initialization method will affect the operation effect of the monocular VIO algorithm.

[0003] At present, the commonly used monocular VIO initialization method is to first use the visual information captured by the camera to construct a "Structure-from-Motion" (SfM), that is, to use multi-view geometry techniques such as epipolar constraints to solve the pose transformation relationship of the first few key frames of the camera and the spatial structure of the environment, and use it as a reference value to build a constraint relationship with the IMU pre-integration result, and solve the absolute scale of the VIO system, gyroscope zero bias, accelerometer zero bias, gravity direction, key frame speed and other information (such as the initialization schemes of ORB-SLAM3 and VINS-Mono).

[0004] But the problem with this method is that when the carrier's motion excitation is insufficient during the initialization process (such as a drone moving at a constant speed in the air), the information obtained by the IMU does not have scale observability, which will lead to inaccurate calculation of the initialization parameters or even initialization failure. Earlier related studies have shown that monocular VIO cannot observe scale under constant acceleration, resulting in significant initial errors in insufficiently excited scenes. However, in practical applications, especially VR / AR devices and drones cruising at a constant speed in the air, insufficient motion excitation is a very common scenario.

[0005] Therefore, it is urgent to propose a truly robust initialization method that can ensure that the system can complete initialization in any scenario. Summary of the invention

[0006] The purpose of the present invention is to provide an initialization method for a monocular VIO system under low motion excitation, which can enable the monocular VIO system to be initialized in any scenario and has good robustness.

[0007] In order to achieve the above object, the present invention provides the following technical solutions:

[0008] In a first aspect, the present invention provides an initialization method for a monocular VIO system under low motion excitation, wherein a single-point rangefinder is integrated in the monocular VIO system, and the single-point rangefinder has the same orientation as a visual camera in the monocular VIO system; the initialization method comprises the following steps:

[0009] S1. Filter N key frames from the video stream captured by the visual camera;

[0010] S2. Calculate the relative pose transformation and 3D space point coordinates between two adjacent key frames one by one to obtain the SfM of the blur scale;

[0011] S3. Using the scale factor, the fuzzy scale SfM is updated to the absolute scale SfM, which specifically includes the following sub-steps:

[0012] S30. Divide the N key frame images into triangular meshes, where each node in the triangular mesh is a feature point;

[0013] S31. Obtain the absolute depth value d of the pixel at a fixed position in the key frame image using a single-point rangefinder;

[0014] S32. Assume that the spatial position corresponding to the pixel point at the fixed position of the key frame image of the single-point rangefinder is coplanar with the three spatial feature points related to the triangular mesh where it is located, and assume that the visual camera coordinate system and the single-point rangefinder coordinate system coincide, and deduce the distance depth.

[0015] S33. Get the scale factor s, And the scale factor is used to update the fuzzy scale SfM to the absolute scale SfM;

[0016] S4. Optimize the SfM of the absolute scale using the improved bundle adjustment to obtain the optimized SfM;

[0017] S5. Combine the optimized SfM with the IMU pre-integration in the monocular VIO system, construct and solve the maximum a posteriori probability estimation function to obtain the initialization parameters of the monocular VIO system. The initialization parameters include at least the rotation matrix in the direction of gravity, the accelerometer bias, the gyroscope bias, and the speed of each frame in the set of N key frames.

[0018] As a possible implementation, distance depth It is derived as follows:

[0019]

[0020] n is the normal vector of the plane formed by three coplanar spatial feature points, expressed as:

[0021]

[0022] in, is the direction vector of the light beam emitted by the single-point rangefinder, C is the visual camera coordinate system, F1, F2, and F3 are the coordinates of the three coplanar feature points in the world coordinate system, is the vector pointing from the optical center of the visual camera to coordinate F1 in the world coordinate system, is the vector pointing from the optical center of the visual camera to coordinate F2 in the world coordinate system, is the vector pointing from the optical center of the visual camera to coordinate F3 in the world coordinate system, It is the vector from the origin of the world coordinate system to the optical center of the visual camera.

[0023] As a possible implementation method, the improved bundle adjustment adds the ranging error on the basis of the visual error, which is used to optimize the visual camera pose and 3D space point coordinates related to the ranging information in the fuzzy scale SfM.

[0024] As a possible implementation, the absolute scale SfM is optimized as follows:

[0025] The configuration cost function is:

[0026] in, i F (j) represents the jth three-dimensional space coordinate under the i-th key frame, i P represents the reprojection matrix of the i-th key frame, which is used to project the j-th three-dimensional space coordinate under the i-th key frame onto the key frame image. i f (j) is the 2D pixel coordinate of the jth feature point observed in the i-th key frame, f(·) represents the error function of the reprojection process, Represents the error of the ranging information, which includes the visual camera pose and 3D space coordinates to be optimized;

[0027] Minimize the cost function to obtain the optimized SfM.

[0028] As a possible implementation, S5 includes the following sub-steps:

[0029] S50. Determine the initialization parameter set ξ of the monocular VIO systemk :ξ k = {R wg ,b a ,b g ,v 0:k}; where R wg is the rotation matrix of the gravity direction, b a is the accelerometer bias, b g is the gyro bias, v 0:k is the speed of each frame in the set of N key frames;

[0030] S51. Construct the maximum a posteriori probability estimation function: p(ξ k |I 0:k )∝p(I 0:k |ξ k )p(ξ k ); where p(I 0:k |ξ k ) represents the likelihood distribution of the IMU pre-integrated observations, p(ξ k ) represents the prior of IMU state, I 0:k Represents the set of pre-integrated observations between consecutive keyframes;

[0031] S52. Solving the maximum a posteriori probability estimation function, specifically including:

[0032] S520. Rewrite p(I based on the independence of the IMU pre-integrated observations 0:k |ξ k );

[0033] S521. Minimize the negative logarithm of the initialization parameters of the monocular VIO system;

[0034] S522. Assuming that the IMU pre-integration follows a Gaussian distribution, the negative logarithmic function of the initialization parameters is converted into a least squares function;

[0035] S53. Use a nonlinear optimization algorithm to solve the least squares function and obtain initialization parameters of the monocular VIO system.

[0036] As a possible implementation method, in S520, the rewritten p(I 0:k |ξ k ) is expressed as:

[0037]

[0038] In S521, the negative logarithm of the initialization parameter of the monocular VIO system is minimized as:

[0039]

[0040] In S522, the least squares function is expressed as:

[0041]

[0042] Among them, r p represents the residual of the IMU state prior, ∑ p Represents the covariance matrix of the IMU state prior residual, I i,j represents the IMU pre-integrated observation value between the i-th key frame and the j-th key frame, represents the residual of IMU pre-integration between two consecutive key frames, and ∑Ii-1,i represents the covariance matrix of the IMU pre-integration residual between two consecutive key frames.

[0043] As a possible implementation, screening N key frames includes screening the first key frame and screening the remaining N-1 key frames;

[0044] The specific steps of selecting the first key frame are as follows: configuring a feature point quantity threshold, extracting ORB feature points frame by frame from the video stream, and the frame whose first ORB feature point meets the feature point quantity threshold is the first key frame;

[0045] The specific steps for screening the remaining N-1 key frames are as follows: configure the feature point matching number threshold and the average disparity threshold. Starting from the second key frame, each current frame needs to perform feature matching with the nearest key frame. When the number of matched ORB feature points exceeds the feature point matching number threshold, calculate the average disparity of the matched ORB feature points between the two frames. When the average disparity exceeds the average disparity threshold, the current frame is determined to be a key frame.

[0046] As a possible implementation manner, the threshold range of the number of feature points is 500 to 1200; and / or the threshold range of the number of feature point matches is 100 to 300; and / or the average disparity threshold range is 20 to 40 pixels.

[0047] As a possible implementation, S2 includes the following sub-steps:

[0048] S20. Extracting ORB feature points from the first key frame and the second key frame and performing feature matching to obtain matching feature point pairs, constructing and solving a set of simultaneous equations based on the matching feature point pairs to obtain an essential matrix;

[0049] S21. Calculate all parameters of the essential matrix, then perform singular value decomposition on the essential matrix to obtain the rotation matrix and displacement vector between the first key frame and the second key frame, that is, the relative posture transformation between the first key frame and the second key frame;

[0050] S22. triangulate each feature point in the matched feature point pair into a plurality of three-dimensional space points, and obtain the coordinates of the plurality of three-dimensional space points based on the relative position transformation between the first key frame and the second key frame;

[0051] S23. Based on the coordinates of multiple three-dimensional space points, the relative pose transformation between the first key frame and the second key frame, the relative pose transformation between the remaining N-2 key frames is calculated using an n-point perspective algorithm;

[0052] S24. Obtain the SfM of the blur scale based on the relative pose transformation between multiple 3D space point coordinates and N key frames.

[0053] In a second aspect, the present invention provides a monocular VIO system, which executes the initialization method of the monocular VIO system under low motion excitation provided in the first aspect, wherein the monocular VIO system is a monocular VIO system integrated with a single-point rangefinder; the single-point rangefinder has the same orientation as a visual camera in the monocular VIO system; the single-point rangefinder is used to obtain the true depth of the pixel point corresponding to the single-point rangefinder on the image captured by the visual camera.

[0054] Compared with the prior art, the present invention has the following effects:

[0055] 1. The present invention provides an initialization method for a monocular VIO system under low motion excitation, which integrates a single-point rangefinder into a traditional monocular VIO system. The single-point rangefinder has the advantages of low price, high ranging accuracy, light weight, etc., and can be installed on a variety of lightweight devices; installing a single-point rangefinder can effectively avoid problems such as inaccurate initialization parameter calculation or even initialization failure due to insufficient motion excitation.

[0056] 2. The present invention provides an initialization method for a monocular VIO system under low motion excitation, which integrates a single-point rangefinder into a traditional monocular VIO system. After the integration of the single-point rangefinder, the SfM can be jointly optimized by combining the ranging information and the visual information, which can greatly improve the accuracy of the visual SfM, can be applied to any scenario, and has high robustness.

[0057] 3. The initialization method for the monocular VIO system under low motion excitation provided by the present invention combines the optimized SfM with the IMU pre-integration in the monocular VIO system to construct a maximum a posteriori probability estimation function. By solving the maximum a posteriori probability estimation function, the initialization parameters of the monocular VIO system can be accurately determined, providing good initial values ​​for subsequent processes. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] The drawings described herein are used to provide further understanding of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0059] Figure 1 A flow chart of an initialization method for a monocular VIO system under low motion excitation provided by an embodiment of the present invention;

[0060] Figure 2 It is a flow chart of a method for updating the SfM of fuzzy scale to the SfM of absolute scale by using the scale factor in an embodiment of the present invention;

[0061] Figure 3 A schematic diagram of performing visual-laser cross-modal matching during operation of an initialization method for a monocular VIO system under low motion excitation provided by an embodiment of the present invention;

[0062] Figure 4 It is a schematic diagram of modeling distance measurement through visual information in an embodiment of the present invention. DETAILED DESCRIPTION

[0063] In order to clearly describe the technical solutions of the embodiments of the present invention, in the embodiments of the present invention, words such as "first" and "second" are used to distinguish the same items or similar items with basically the same functions and effects. For example, the first threshold and the second threshold are only used to distinguish different thresholds, and their order is not limited. Those skilled in the art can understand that words such as "first" and "second" do not limit the quantity and execution order, and words such as "first" and "second" do not necessarily limit them to be different.

[0064] It should be noted that, in the present invention, words such as "exemplary" or "for example" are used to indicate examples, illustrations or descriptions. Any embodiment or design described as "exemplary" or "for example" in the present invention should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of words such as "exemplary" or "for example" is intended to present related concepts in a specific way.

[0065] In the present invention, "at least one" means one or more, and "plurality" means two or more. "And / or" describes the association relationship of associated objects, indicating that three relationships may exist. For example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone, where A and B can be singular or plural. The character " / " generally indicates that the associated objects before and after are in an "or" relationship. "At least one of the following" or similar expressions refers to any combination of these items, including any combination of single or plural items. For example, at least one of a, b or c can mean: a, b, c, the combination of a and b, the combination of a and c, the combination of b and c, or the combination of a, b and c, where a, b, c can be single or multiple.

[0066] The present invention aims at the defect that the scale information of the existing initialization method of the monocular VIO system is not observable in the low motion excitation scenario, resulting in inaccurate initialization parameters or even initialization failure, and proposes an initialization method suitable for a monocular VIO system under low motion excitation. The present invention integrates a single-point rangefinder into the traditional monocular VIO system and makes the single-point rangefinder and the visual camera in the monocular VIO system face the same direction. The single-point rangefinder has the advantages of low price, high ranging accuracy, light weight, etc., and is convenient for integrated installation on light drones and VR / AR devices.

[0067] In the first aspect, the present invention provides an initialization method for a monocular VIO system under low motion excitation, see Figure 1 , the initialization method includes the following steps:

[0068] S1. Filter N key frames from the video stream captured by the visual camera;

[0069] As a possible implementation, screening N key frames includes screening the first key frame and screening the remaining N-1 key frames;

[0070] Filter the first key frame, specifically: configure the feature point quantity threshold, extract ORB feature points from the video stream frame by frame, and the first frame that exceeds the ORB feature point quantity threshold is the first key frame;

[0071] Filter the remaining N-1 key frames, specifically: configure the feature point matching number threshold and the average disparity threshold. Starting from the second key frame, each current frame needs to perform feature matching with the nearest key frame. When the number of matched ORB feature points exceeds the feature point matching number threshold, calculate the average disparity of the matched ORB feature points between the two frames. When the average disparity exceeds the average disparity threshold, the current frame is determined to be a key frame.

[0072] Exemplarily, feature matching is determined by calculating the distance between the ORB feature point descriptors in the two frames, that is, if the distance between a pair of ORB feature point descriptors in the two frames is the closest to each other, then the pair of feature points is determined to be a matching ORB feature point; on the contrary, the ORB feature points whose descriptors cannot be the closest to each other are not matching ORB feature points. The average disparity is calculated by calculating the pixel position difference between the two frames of images for each pair of matching ORB feature points, and the pixel position difference reflects the movement of the matching ORB feature points between the two frames. The pixel position difference of each pair of matching ORB feature points is accumulated to obtain the total disparity of all matching ORB feature points, and the total disparity is divided by the number of matching ORB feature points to obtain the average disparity. The average disparity reflects the average degree of change in the content of the two frames. If the average disparity between the current frame and the closest key frame exceeds the average disparity threshold, the current frame is determined to be a key frame.

[0073] As an example, the threshold range for the number of feature points is 500 to 1200, for example, 500, 600, 700, 800, 900, 1000, 1100, 1200; and / or, the threshold range for the number of feature point matches is 100 to 300, for example, 100, 150, 200, 250, 300; and / or, the average disparity threshold range is 20 to 40 pixels, for example, 20 pixels, 25 pixels, 30 pixels, 35 pixels, 40 pixels.

[0074] S2. Calculate the relative pose transformation and 3D space point coordinates between two adjacent key frames one by one to obtain the SfM of the blur scale;

[0075] As a possible implementation method, S2 specifically includes the following sub-steps:

[0076] S20. Extracting ORB feature points from the first key frame and the second key frame and performing feature matching to obtain matching feature point pairs, constructing and solving a set of simultaneous equations based on the matching feature point pairs to obtain an essential matrix;

[0077] As an example, the essential matrix is ​​solved by constructing a system of simultaneous equations based on the epipolar constraint relationship between the matched feature point pairs, where the epipolar constraint relationship between each pair of matched feature point pairs can be derived from the following equation:

[0078] s1p1=KP

[0079] s2p2=K(RP+t)

[0080] K -1 s2p2=RK -1 s1p1+t

[0081] p2 T K-T EK -1 p1=0

[0082] Among them, p1 is the pixel coordinate of the matched feature point in the first key frame image, p2 is the pixel coordinate of the matched feature point in the second key frame image, K is the intrinsic parameter matrix of the visual camera, R is the rotation transformation matrix between the two key frames, t is the displacement vector between the two key frames, P is the three-dimensional space coordinate of the three-dimensional space point corresponding to the matched feature point, s1 is the depth of the three-dimensional space point corresponding to the matched feature point in the first key frame, s2 is the depth of the three-dimensional space point corresponding to the matched feature point in the second key frame, the symbol T represents the matrix transpose operation, and E is the essential matrix.

[0083] S21. Calculate all parameters of the essential matrix, then perform singular value decomposition on the essential matrix to obtain the rotation matrix and displacement vector between the first key frame and the second key frame, that is, the relative posture transformation between the first key frame and the second key frame;

[0084] Exemplarily, each pair of matching feature point pairs can constitute an epipolar constraint equation. Since the essential matrix has a total of 8 degrees of freedom, 8 pairs of points are found in the set of matching feature point pairs. By combining them into a set of equations with 8 conditions, all parameters of the essential matrix can be solved. After that, the essential matrix is ​​subjected to singular value decomposition to obtain the rotation matrix and displacement vector between the first frame key frame and the second frame key frame, that is, the relative pose transformation between the first frame key frame and the second frame key frame.

[0085] S22. triangulate each feature point in the matched feature point pair into a plurality of three-dimensional space points, and obtain the coordinates of the plurality of three-dimensional space points based on the relative pose transformation between the first key frame and the second key frame;

[0086] As an example, the depth s1 of a feature point pair corresponding to the three-dimensional space point in the first frame key frame coordinate system has the following relationship:

[0087]

[0088] in, Represents the homogeneous pixel coordinates of the feature point in the first key frame, Represents the homogeneous pixel coordinates of the feature point in the second keyframe, × stands for The antisymmetric matrix of .

[0089] Solving the above overdetermined equation, we can obtain the least squares solution for s1. Substituting s1 into the following equation, we can obtain the three-dimensional spatial coordinates P of the feature point in the key frame coordinate system of the first frame:

[0090]

[0091] Among them, K -1 Represents the inverse of the intrinsic parameter matrix.

[0092] The above method is used to triangulate each feature point in all matching feature point pairs into a three-dimensional space point in turn, and then based on the relative posture transformation relationship between the first frame key frame and the second frame key frame, multiple three-dimensional space point coordinates are obtained.

[0093] S23. Based on the coordinates of multiple three-dimensional space points, the relative pose transformation between the first key frame and the second key frame, the relative pose transformation between the remaining N-2 key frames is calculated using an n-point perspective algorithm;

[0094] As an example, the perspective-n-point (PnP) algorithm is a method for calculating the relative pose transformation of feature points from 3D to 2D matching. When there are only the first key frame and the second key frame, only the 3D to 2D matching feature points can be obtained. After processing in step S22, the first key frame and the second key frame are triangulated to obtain the three-dimensional space point coordinates corresponding to the matching feature points. Therefore, the relative pose transformation between the remaining N-2 key frames is calculated using PnP.

[0095] Two keyframes that have completed feature matching. The first keyframe has completed triangulation and obtained the 3D coordinates of the matching feature points, while the second keyframe only has the 2D coordinates of the matching feature points. Consider a spatial point P corresponding to a feature point, whose homogeneous coordinates are P = (X, Y, Z, 1) T , the normalized plane homogeneous coordinates of the feature point in the second key frame are x = (u, v, 1) T , let the rotation matrix between the two frames be R, the displacement vector be t, and define a 3×4 augmented matrix [R|t], which contains the rotation information and displacement information between the two frames.

[0096] The relationship between the three-dimensional space point coordinates and the 2D coordinates is written as follows:

[0097]

[0098] Among them, t1~t 12 To expand the elements in the matrix [R|t], we use the last row to eliminate s, and we can get two relations:

[0099]

[0100] To simplify the representation, the row vectors in the augmented matrix [R|t] are represented as follows:

[0101] a1=(t1,t2,t3,t4) T ,a2=(t5,t6,t7,t8) T ,a3=(t9,t 10 ,t 11 ,t 12 ) T ;

[0102] So we can get:

[0103]

[0104] That is, each 3D to 2D matching feature point can provide two constraint equations, and the augmented matrix [R|t] has a total of 12 parameters: t1~t 12 Therefore, at least 6 pairs of matching feature points can solve all the parameters of the matrix. This method is used to calculate the relative pose transformation between the remaining N-2 key frames in sequence, and then the relative pose transformation between the N key frames is obtained.

[0105] S24. Obtain the SfM of the blur scale based on the relative pose transformation between multiple 3D space point coordinates and N key frames.

[0106] S3. Use the scale factor to update the SfM of the fuzzy scale to the SfM of the absolute scale, see Figure 2 , specifically including the following sub-steps:

[0107] S30. Divide the N key frame images into triangular meshes, where each node in the triangular mesh is a feature point;

[0108] Exemplarily, the Delaunay triangulation algorithm is applied to N frames of key frame images to perform triangular mesh division. The Delaunay triangulation algorithm uses the triangulated feature points in the image as vertices and divides the image plane into small triangles, turning the image into a triangular "mesh". Each node in the mesh is a feature point. The advantage of the Delaunay triangulation algorithm is that the generated triangles have uniform side lengths, avoiding the generation of "long and thin" triangles.

[0109] S31. Obtain the absolute depth value d of the pixel at a fixed position in the key frame image using a single-point rangefinder;

[0110] For example, the single-point rangefinder and the visual camera are integrated at a certain angle on a drone or other device, and are calibrated in advance. When the device starts running, the single-point distance measured by the single-point rangefinder will constantly aim at a pixel point at a fixed position in the image taken by the visual camera, see Figure 3, realize visual-laser cross-modal matching, and thus obtain the absolute depth value of the pixel. However, there is only a small probability that the ORB features extracted from the image taken by the visual camera can coincide with the single-point ranging point, and most of the time it is difficult to match the two. Therefore, it is necessary to derive the distance depth, calculate the scale factor, and use the scale factor to update the fuzzy scale SfM to the absolute scale SfM to make the SfM information more accurate.

[0111] S32. Assume that the spatial position corresponding to the pixel point at the fixed position of the key frame image of the single-point rangefinder is coplanar with the three spatial feature points related to the triangular mesh where it is located, and assume that the visual camera coordinate system and the single-point rangefinder coordinate system coincide with each other. Figure 4 , derive the distance depth

[0112] As a possible implementation, distance depth It is derived as follows:

[0113]

[0114] n is the normal vector of the plane formed by three coplanar spatial feature points, expressed as:

[0115]

[0116] in, is the direction vector of the light beam emitted by the single-point rangefinder, C is the visual camera coordinate system, F1, F2, and F3 are the coordinates of the three coplanar feature points in the world coordinate system, is the vector pointing from the optical center of the visual camera to coordinate F1 in the world coordinate system, is the vector pointing from the optical center of the visual camera to coordinate F2 in the world coordinate system, is the vector pointing from the optical center of the visual camera to coordinate F3 in the world coordinate system, It is the vector from the origin of the world coordinate system to the optical center of the visual camera.

[0117] S33. Get the scale factor s, And the scale factor is used to update the fuzzy scale SfM to the absolute scale SfM;

[0118] S4. Optimize the SfM of the absolute scale using the improved bundle adjustment to obtain the optimized SfM;

[0119] As a possible implementation method, the improved bundle adjustment adds the ranging error on the basis of the visual error, which is used to optimize the visual camera pose and 3D space point coordinates related to the ranging information in the fuzzy scale SfM. Optimizing the absolute scale SfM can make the visual camera pose and 3D space point coordinates in the SfM more accurate, which can improve the accuracy of solving the initialization parameters in the subsequent steps.

[0120] Exemplarily, the absolute scale SfM is optimized as follows:

[0121] The configuration cost function is:

[0122] in, i F (j) represents the coordinates of the jth 3D space point under the i-th key frame, i P represents the reprojection matrix of the i-th key frame, which is used to project the j-th three-dimensional space point coordinates under the i-th key frame onto the key frame image. i f (j) is the 2D pixel coordinate of the jth feature point observed in the i-th key frame, f(·) represents the error function of the reprojection process, Represents the error of the ranging information, which includes the visual camera pose to be optimized and the coordinates of the 3D space point;

[0123] Minimize the cost function to obtain the optimized SfM.

[0124] For example, minimizing the cost function can be accomplished through general graph optimization libraries such as g2o or ceres, where the nodes are the visual camera poses and three-dimensional space point coordinates of the key frames, the edges are the reprojection errors and ranging errors, and a robust kernel function is added during the optimization process to reduce the influence of small outliers to enhance the robustness of the optimizer.

[0125] S5. Combine the optimized SfM with the IMU pre-integration in the monocular VIO system, construct and solve the maximum a posteriori probability estimation function to obtain the initialization parameters of the monocular VIO system, which at least include the rotation matrix in the direction of gravity, the accelerometer bias, the gyroscope bias, and the speed of each frame in the set of N key frames;

[0126] As a possible implementation, S5 includes the following sub-steps:

[0127] S50. Determine the initialization parameter set ξ of the monocular VIO system k :ξ k = {R wg ,b a ,b g ,v 0:k}; where R wgis the rotation matrix of the gravity direction, b a is the accelerometer bias, b g is the gyro bias, v 0:k is the speed of each frame in the set of N key frames;

[0128] S51. Construct the maximum a posteriori probability estimation function: p(ξ k |I 0:k )∝p(I 0:k |ξ k )p(ξ k ); where p(I 0:k |ξ k ) represents the likelihood distribution of the IMU pre-integrated observations, p(ξ k ) represents the prior of IMU state, I 0:k Represents the set of pre-integrated observations between consecutive keyframes;

[0129] S52. Solving the maximum a posteriori probability estimation function, specifically including:

[0130] S520. Rewrite p(I based on the independence of the IMU pre-integrated observations 0:k |ξ k );

[0131] As an example, the rewritten p(I 0:k |ξ k ) is expressed as:

[0132]

[0133] S521. Minimize the negative logarithm of the initialization parameters of the monocular VIO system;

[0134] As an example, minimizing the negative logarithm of the initialization parameters of a monocular VIO system is expressed as:

[0135]

[0136] S522. Assuming that the IMU pre-integration and the IMU state prior both obey Gaussian distribution, the negative logarithmic function of the initialization parameters is converted into a least squares function;

[0137] As an example, the least squares function is expressed as:

[0138]

[0139] Among them, r p represents the residual of the IMU state prior, ∑ p Represents the covariance matrix of the IMU state prior residual, I i,j represents the IMU pre-integrated observation value between the i-th key frame and the j-th key frame, represents the residual of IMU pre-integration between two consecutive key frames, and ∑Ii-1,i represents the covariance matrix of the IMU pre-integration residual between two consecutive key frames.

[0140] Exemplarily, the IMU pre-integration residual consists of an ideal value and an observed value. The ideal value is obtained by the optimized SfM, which is mainly to calculate the ideal value increment of displacement, rotation and speed at one time based on the displacement and rotation in the optimized SfM; the observed value is obtained by the IMU pre-integration model based on the sensor data to obtain the observed increment of displacement, rotation and speed. The initialization parameter ξ to be solved k It is included in the IMU pre-integration, and the initialization parameters with the minimum error can be obtained by minimizing the residual between the ideal value and the observed value through nonlinear optimization method.

[0141] S53. Use a nonlinear optimization algorithm to solve the least squares function and obtain the initialization parameters of the monocular VIO system.

[0142] As an example, solving the least squares function can be done through graph optimization libraries such as g2o or ceres. Nonlinear optimization algorithms include Gauss-Newton method, Dog-leg algorithm, and Levenberg-Marquardt algorithm. For example, if the Levenberg-Marquardt algorithm is selected, all the parameters required for VIO initialization are obtained after successful solution, providing good initial values ​​for the subsequent VIO operation process.

[0143] In a second aspect, the present invention provides a monocular VIO system, which executes the initialization method of the monocular VIO system under low motion excitation provided in the first aspect, and the monocular VIO system is a monocular VIO system integrated with a single-point rangefinder; the single-point rangefinder has the same orientation as a visual camera in the monocular VIO system; the single-point rangefinder is used to obtain the real depth of the pixel point corresponding to the single-point rangefinder on the image captured by the visual camera.

[0144] Although the present invention is described herein in conjunction with various embodiments, in the process of implementing the claimed invention, those skilled in the art may understand and implement other variations of the disclosed embodiments by viewing the drawings, the disclosure, and the drawings, etc. In the specification, the word "comprising" does not exclude other components or steps, and "one" or "an" does not exclude multiple situations. A single processor or other unit can implement several functions listed in the specification. Certain measures are recorded in different embodiments, but this does not mean that these measures cannot be combined to produce good results.

[0145] Although the present invention has been described in conjunction with specific features and embodiments thereof, it is apparent that various modifications and combinations may be made thereto without departing from the spirit and scope of the present invention. Accordingly, this specification and the accompanying drawings are merely exemplary illustrations of the present invention and are deemed to cover any and all modifications, variations, combinations or equivalents within the scope of the present invention. Obviously, those skilled in the art may make various modifications and variations to the present invention without departing from the spirit and scope of the present invention. Thus, the present invention is intended to include such modifications and variations if they fall within the scope of the present invention and its equivalents.

Claims

1. A method for initializing a monocular VIO system under low motion excitation, characterized in that: The monocular VIO system is integrated with a single-point rangefinder, and the single-point rangefinder has the same orientation as the visual camera in the monocular VIO system; the initialization method comprises the following steps: S1. Filter N key frames from the video stream captured by the visual camera; S2. Calculate the relative pose transformation and three-dimensional space point coordinates between two adjacent key frames one by one to obtain the fuzzy scale SfM; S3. Using the scale factor, the fuzzy scale SfM is updated to the absolute scale SfM, which specifically includes the following sub-steps: S30. Divide the N key frame images into triangular meshes, where each node in the triangular mesh is a feature point; S31. Obtain the absolute depth value d of the pixel at a fixed position in the key frame image using a single-point rangefinder; S32. Assume that the spatial position corresponding to the pixel point at the fixed position of the key frame image of the single-point rangefinder is coplanar with the three spatial feature points related to the triangular mesh where it is located, and assume that the visual camera coordinate system and the single-point rangefinder coordinate system coincide, and deduce the distance depth. S33. Get the scale factor s, And the scale factor is used to update the fuzzy scale SfM to the absolute scale SfM; S4. Optimize the SfM of the absolute scale using the improved bundle adjustment to obtain the optimized SfM; S5. Combine the optimized SfM with the IMU pre-integration in the monocular VIO system, construct and solve the maximum a posteriori probability estimation function to obtain the initialization parameters of the monocular VIO system, where the initialization parameters include at least the rotation matrix in the direction of gravity, the accelerometer bias, the gyroscope bias, and the speed of each frame in the set of N key frames.

2. The initialization method for a monocular VIO system under low motion excitation according to claim 1, characterized in that: Distance Depth It is derived as follows: n is the normal vector of the plane formed by three coplanar spatial feature points, expressed as: in, is the direction vector of the light beam emitted by the single-point rangefinder, C is the visual camera coordinate system, F1, F2, and F3 are the coordinates of the three coplanar feature points in the world coordinate system, is the vector pointing from the optical center of the visual camera to coordinate F1 in the world coordinate system, is the vector pointing from the optical center of the visual camera to coordinate F2 in the world coordinate system, is the vector pointing from the optical center of the visual camera to coordinate F3 in the world coordinate system, It is the vector from the origin of the world coordinate system to the optical center of the visual camera.

3. The initialization method for a monocular VIO system under low motion excitation according to 1, characterized in that: The improved bundle adjustment adds the ranging error on the basis of the visual error, and is used to optimize the visual camera pose and three-dimensional space point coordinates related to the ranging information in the SfM of the fuzzy scale.

4. The initialization method for a monocular VIO system under low motion excitation according to claim 3, characterized in that: The absolute scale SfM is optimized as follows: The configuration cost function is: in, i F (j) represents the jth three-dimensional space coordinate under the i-th key frame, i P represents the reprojection matrix of the i-th key frame, which is used to project the j-th three-dimensional space coordinate under the i-th key frame onto the key frame image. i f (j) is the 2D pixel coordinate of the jth feature point observed in the i-th key frame, f(·) represents the error function of the reprojection process, Represents the error of the ranging information, which includes the visual camera pose and 3D space coordinates to be optimized; Minimize the cost function to obtain the optimized SfM.

5. The initialization method for a monocular VIO system under low motion excitation according to claim 1, characterized in that: The S5 comprises the following sub-steps: S50. Determine the initialization parameter set ξ of the monocular VIO system k :ξ k = {R wg ,b a ,b g ,v 0:k }; where R wg is the rotation matrix of the gravity direction, b a is the accelerometer bias, b g is the gyro bias, v 0:k is the speed of each frame in the set of N key frames; S51. Construct the maximum a posteriori probability estimation function: p(ξ k |I 0:k )∝p(I 0:k |ξ k )p(ξ k ); where p(I 0:k |ξ k ) represents the likelihood distribution of the IMU pre-integrated observations, p(ξ k ) represents the prior of IMU state, I 0:k Represents the set of pre-integrated observations between consecutive keyframes; S52. Solving the maximum a posteriori probability estimation function specifically includes: S520. Rewrite p(I based on the independence of the IMU pre-integrated observations 0:k |ξ k ); S521. Minimize the negative logarithm of the initialization parameters of the monocular VIO system; S522. Assuming that the IMU pre-integration and the IMU state prior both obey Gaussian distribution, the negative logarithmic function of the initialization parameters is converted into a least squares function; S53. Use a nonlinear optimization algorithm (list two or three other specific algorithms) to solve the least squares function and obtain the initialization parameters of the monocular VIO system.

6. The initialization method for a monocular VIO system under low motion excitation according to claim 5, characterized in that: In S520, the rewritten p(I 0:k |ξ k ) is expressed as: In S521, the negative logarithm of the initialization parameter of the monocular VIO system is minimized as: In S522, the least squares function is expressed as: Among them, r p represents the residual of the IMU state prior, ∑ p Represents the covariance matrix of the IMU state prior residual, I i,j represents the IMU pre-integrated observation value between the i-th key frame and the j-th key frame, represents the residual of IMU pre-integration between two consecutive keyframes, ∑ Ii-1,i Represents the covariance matrix of the IMU pre-integration residuals between two consecutive keyframes.

7. The initialization method for a monocular VIO system under low motion excitation according to claim 1, characterized in that: The screening of N key frames includes screening the first key frame and screening the remaining N-1 key frames; The screening of the first key frame specifically includes: configuring a feature point quantity threshold, extracting ORB feature points frame by frame from the video stream, and the first frame that exceeds the ORB feature point quantity threshold is the first key frame; The specific method of screening the remaining N-1 key frames is as follows: configuring a feature point matching number threshold and an average disparity threshold. Starting from the second key frame, each current frame needs to perform feature matching with the nearest key frame. When the number of matched ORB feature points exceeds the feature point matching number threshold, the average disparity of the matched ORB feature points between the two frames is calculated. When the average disparity exceeds the average disparity threshold, the current frame is determined to be a key frame.

8. The initialization method for a monocular VIO system under low motion excitation according to claim 7, characterized in that: The feature point quantity threshold range is 500 to 1200; and / or, the feature point matching quantity threshold range is 100 to 300; and / or, the average disparity threshold range is 20 to 40 pixels.

9. The initialization method for a monocular VIO system under low motion excitation according to claim 1, characterized in that: The S2 comprises the following sub-steps: S20. Extracting ORB feature points from the first key frame and the second key frame and performing feature matching to obtain a pair of matched feature points, constructing a set of simultaneous equations based on the matched feature points and solving them to obtain an essential matrix; S21. Calculate all parameters of the essential matrix, then perform singular value decomposition on the essential matrix to obtain the rotation matrix and displacement vector between the first key frame and the second key frame, that is, the relative posture transformation between the first key frame and the second key frame; S22. triangulate each feature point in the matched feature point pair into a plurality of three-dimensional space points, and obtain the coordinates of the plurality of three-dimensional space points based on the relative position transformation between the first key frame and the second key frame; S23. Based on the coordinates of the plurality of three-dimensional space points, the relative pose transformation between the first key frame and the second key frame, the relative pose transformation between the remaining N-2 key frames is calculated using an n-point perspective algorithm; S24. Obtain the SfM of the blur scale based on the relative pose transformation between the multiple three-dimensional space point coordinates and the N key frames.

10. A monocular VIO system, characterized in that: The monocular VIO system executes the initialization method of the monocular VIO system suitable for low motion excitation according to any one of claims 1 to 9, and the monocular VIO system is a monocular VIO system integrated with a single-point rangefinder; the single-point rangefinder has the same orientation as the visual camera in the monocular VIO system; the single-point rangefinder is used to obtain the real depth of the pixel point corresponding to the single-point rangefinder on the image captured by the visual camera.

Images