Monocular visual-inertial SLAM method and system based on adaptive robust kernel
By adopting adaptive robust core technology in monocular vision SLAM system, the monocular initialization and back-end optimization process is optimized, the problems of scale uncertainty and error accumulation are solved, the system robustness and accuracy are improved, and the system is realized Stable operation in dynamic scenarios and complex environments.
Patent Information
- Application Number
- CN202111376206.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-11-19
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2041-11-19
AI Technical Summary
The existing monocular visual SLAM system has scale uncertainty, failure of pure rotation initialization, and error accumulation in monocular initialization and back-end optimization, which leads to unstable operation in dynamic scenarios and complex environments, making it difficult to achieve high robustness. and high precision.
Using an adaptive robust kernel-based method, the homography matrix and basic matrix model are calculated in parallel when monocular initialization is performed, geometric robust information is used to select the motion model, and it is applied in local graph construction and loop detection. The adaptive robust core performs beam adjustment and Sim(3) solution to reduce external point influence and cumulative error.
Improve the robustness and accuracy of the monocular visual SLAM system, solve the problems of scale uncertainty and pure rotation initialization, reduce the impact of external points on the results, and reduce the accumulated error in the loop correction process, and improve the system stability and positioning accuracy.
Smart Images

Figure CN114821113B_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the field of visual robots and relates to a monocular visual-inertial SLAM method and system based on an adaptive robust kernel, and in particular to optimization of monocular initialization, backend optimization and loop detection parts in the monocular visual-inertial SLAM. Background Art
[0002] With the rapid development of automation integration technology, communication and electronic technology, and high-precision sensor technology, robotics technology has become an irreplaceable part of social production and life, and will have a more extensive impact on the global automation level in the future. The robotics industry has been recognized by the world as one of the most important high-tech industries with the broadest development prospects, but it is undeniable that there are still many problems waiting for us to explore in both the field of robotics and artificial intelligence. Therefore, many experts and scholars have emerged to devote themselves to the in-depth research of "robots' perception and exploration of unknown environments and accurate self-positioning problems."
[0003] Simultaneous Localization and Mapping (SLAM) is a major research hotspot in the field of machine perception and navigation, and has attracted much attention from researchers. At present, SLAM technology has been applied to robot navigation, unmanned driving, virtual reality and augmented reality, and three-dimensional reconstruction. As one of the core technologies of intelligent mobile robots, SLAM can generate environmental maps in scenes such as indoors, underwater, and tunnels where GPS fails. Robots can use maps to perform autonomous positioning and navigation. In the field of unmanned driving, SLAM technology can locate lane lines and achieve centimeter-level positioning accuracy, thereby assisting GPS positioning. SLAM technology can make virtual objects maintain geometric consistency with real scenes, making superimposed virtual objects realistic. Using the map construction function of SLAM technology, three-dimensional models of objects and scenes can also be obtained.
[0004] In the early stage of SLAM technology development, it was mainly centered around LiDAR SLAM. However, LiDAR with high accuracy is expensive, and LiDAR with slightly lower accuracy has large errors, which greatly affects the accuracy of the SLAM system, so it has not been widely used. Compared with LiDAR sensors, cameras have the advantages of low cost, small size, small mass, and images contain rich information. With the improvement of hardware performance and the development of computer vision, visual SLAM based on visual sensors, referred to as VSLAM (Visual SLAM), came into being. Because visual sensors are low in price and contain rich texture information, they have attracted widespread attention from researchers. Although VSLAM technology can achieve good results in general scenarios, it performs poorly in environments with fast motion, image geometry changes caused by viewpoint changes, drastic changes in illumination, or even no light. In order to improve the robustness of the SLAM system, researchers began to shift their research direction to multi-sensor fusion SLAM, the most representative of which is visual inertial SLAM. VISLAM introduces an inertial measurement unit on the basis of visual SLAM, which can not only make up for the defects of pure visual navigation systems, but also improve the matching speed of feature points and enhance the robustness of the algorithm.
[0005] Nowadays, although the research on visual SLAM has achieved fruitful results in many directions, there are still many problems that need to be optimized and solved. For example, in dynamic scenes, texture loss and complex environments, visual SLAM is difficult to operate stably for a long time. The existing monocular vision research has the following main shortcomings: (1) In monocular vision, the need to normalize the translation vector leads to scale uncertainty of monocular vision. Monocular initialization cannot obtain depth information from a single frame. In addition, when there is only pure rotation during the monocular initialization process, the monocular will not be able to initialize. (2) In the local mapping thread, although the overall error caused by mismatching and other reasons is reduced, some large errors will still have unpredictable effects on the results and cannot significantly improve the accuracy. (3) In the visual SLAM front end, whether it is pose estimation or mapping, it is completed by using the relationship between adjacent frames. This algorithm that relies on local constraints will inevitably lead to the accumulation of optimization errors frame by frame, and ultimately produce a large error drift. Studying a highly robust and accurate visual SLAM system is not only of great value to scientific research in fields such as visual robotics and computer vision, but also has a broad range of theoretical applications. Therefore, further research on visual SLAM technology is of great significance. Summary of the invention
[0006] Purpose of the invention: In view of the above shortcomings of the prior art, the purpose of the present invention is to propose a monocular visual-inertial SLAM method and system based on an adaptive robust kernel, improve the monocular initialization process, use an adaptive robust kernel to reduce the influence of external points, enhance the optimization effect, and improve the robustness of the system.
[0007] Technical solution: To achieve the above purpose, the present invention adopts the following technical solution:
[0008] A monocular visual-inertial SLAM method based on an adaptive robust kernel mainly includes optimizing monocular initialization, local mapping and loop detection, specifically including:
[0009] During the monocular initialization, the initial feature matching is performed between the current frame and the reference frame, and the two motion models of the homography matrix and the basic matrix are calculated in parallel. The homography matrix or the basic matrix is selected based on the geometric robust information content, and the model is decomposed and bundle adjustment BA optimization is performed.
[0010] A cost function that minimizes the reprojection error is constructed in the local mapping thread, and an adaptive robust kernel is used for bundle adjustment (BA) optimization.
[0011] In the loop detection thread, an adaptive robust kernel is used to reduce the accumulated error in the loop correction process.
[0012] As a preference, the model is selected by geometrically robust information content, expressed as:
[0013]
[0014]
[0015] Among them, i is the geometric robust information content, k is the key point sequence number, L is the robust function, e is the reprojection error, It is used to limit the size of the reprojection error. β represents the size of the model, where β = 2 is the homography matrix, β = 3 is the basic matrix, and n is the number of map points tracked between two keyframes. represents the number of degrees of freedom in the model, where represents the basic matrix, represents the homography matrix, θ 2 represents the covariance of the key point positioning error, λ represents the dimension of the data, and the two-dimensional correspondence between two frames is λ=4.
[0016] Preferably, the kernel function ρ of the adaptive robust kernel is expressed as:
[0017]
[0018] Among them, μ, ω, c are parameters, and the constant c>0 is used to constrain the scale of the secondary loss area near μ=0.
[0019] As a preference, in the local mapping thread, the cost function that minimizes the reprojection error is expressed as:
[0020]
[0021] Among them, R is the rotation matrix, t is the translation vector, is the set of all matching feature points, ρ is the robust kernel function, is the projection coordinate, X α is the coordinate of the projection point in the world coordinate system, Represents the sum of squares of the reprojection error.
[0022] As a preference, in the loop detection thread, an adaptive robust kernel function is applied to solve Sim(3), which is expressed as:
[0023]
[0024] Among them, S 12 is the transformation matrix, p 1,i is the key point i, p in key frame 1 1,j is the key point j that matches i in key frame 2, P 1,i is the 3D coordinate of key point i in the world coordinate system, P 2,j is the 3D coordinate of key point j matching key point i in the world coordinate system, N is the number of matching key points, π i (i=1,2) represents the projection function.
[0025] Based on the same inventive concept, the present invention provides a monocular visual-inertial SLAM system based on an adaptive robust kernel, comprising:
[0026] The monocular initialization module is used to perform initial feature matching between the current frame and the reference frame, calculate the homography matrix and the basic matrix in parallel, and select the homography matrix H or the basic matrix F according to the geometric robust information content to perform model decomposition and bundle adjustment BA optimization;
[0027] The local mapping module is used to construct the cost function that minimizes the reprojection error and uses an adaptive robust kernel for BA optimization;
[0028] And, a loop detection module is used to reduce the accumulated error in the loop correction process by using an adaptive robust kernel.
[0029] Based on the same inventive concept, the present invention also provides a computer system, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the computer program is loaded into the processor, the monocular visual-inertial SLAM method based on the adaptive robust kernel is implemented.
[0030] Beneficial effects: The present invention provides a monocular visual-inertial SLAM method based on an adaptive robust kernel, which mainly proposes improved methods for monocular initialization, backend optimization, and loop detection. The main advantages are as follows:
[0031] (1) For the scale uncertainty in monocular vision and the pure rotation problem of the monocular camera, the method of motion estimation during the initialization of the monocular system is improved. This can solve the initialization failure problem when there is little or no translation, build the initial point cloud map and provide initial values for subsequent calculations.
[0032] (2) Many state estimations in SLAM use minimizing the reprojection error as the cost function. An outlier with a large deviation will have a huge impact on the estimation result. By using an adaptive robust kernel function to directly optimize the model parameters and reducing the weight of the outliers, the impact of erroneous data on the overall robustness can be reduced, thereby improving the system accuracy.
[0033] (3) The accumulated error generated in the SLAM tracking process is reduced by using an adaptive robust kernel in the loop detection thread during the loop correction process. BRIEF DESCRIPTION OF THE DRAWINGS
[0034] Figure 1 It is a flow chart of a method according to an embodiment of the present invention. DETAILED DESCRIPTION
[0035] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments.
[0036] like Figure 1 As shown, an embodiment of the present invention provides a monocular visual-inertial SLAM method based on an adaptive robust kernel. First, feature points are extracted from the image frame for feature matching, and the initial data association is found. Then, the image is dedistorted (only radial distortion is considered), and the homography matrix model and the basic matrix model are calculated in parallel. The motion model is selected according to the geometric information content, and then the selected motion model is subjected to external point elimination and decomposition to obtain the initial motion estimate, and finally, global BA optimization initial reconstruction is performed. The embodiment of the present invention mainly involves (1) analyzing the monocular initialization process and designing a new model selection mechanism; (2) constructing a cost function that minimizes the reprojection error, and using an adaptive robust kernel for BA optimization; (3) using an adaptive robust kernel to reduce the cumulative error in the loop correction process. The specific steps are as follows:
[0037] 1. The monocular initialization process of monocular visual-inertial SLAM based on adaptive robust kernel includes the following steps:
[0038] (1) Perform initial feature matching, i.e. the current frame F c With reference frame F r Feature matching in:
[0039] Extract the current frame F c The ORB (Oriented Fast and Rotated Brief) feature in the reference coordinate system F r Search for matching feature points in If not enough matches are found, the reference coordinate system is reset. Find matching points, extract ORB features from the first two frames, and then perform feature point matching. If the number of matches is less than 100, the match fails. Then, dedistort the image frames for feature matching.
[0040] (2) Parallel calculation of the homography matrix model and the basic matrix model, and selecting the homography matrix H or the basic matrix F based on the geometrically robust information content;
[0041] Compute a homography matrix H in parallel threads cr and a fundamental matrix F cr :
[0042] x c =H cr x r #(1)
[0043]
[0044] The model selection method based on geometric robust information content is adopted, as shown in formula (3):
[0045]
[0046]
[0047] Among them, G is the geometric robust information content, k is the key point sequence number, L is the robust function, e is the reprojection error, It is used to limit the size of the reprojection error. β represents the size of the model, where β = 2 is the homography matrix, β = 3 is the basic matrix, and n is the number of map points tracked between two keyframes. represents the number of degrees of freedom in the model, where represents the basic matrix, represents the homography matrix, θ 2represents the covariance of the keypoint localization error, λ represents the dimensionality of the data, and the two-dimensional correspondence between two frames λ = 4. The geometrically robust information content of the homography matrix and the basis matrix are calculated separately, so that when there is little or no translation between frames, the homography is selected to better represent the motion process to enhance the monocular initialization process.
[0048] (3) Decomposing the motion model to obtain an initial motion estimate;
[0049] For the homography matrix H, a numerical method is used to decompose it to obtain R and t.
[0050] For the fundamental matrix F, use the calibration matrix K to transform it into the fundamental matrix E:
[0051] E=K T FK#(5)
[0052] According to the basic matrix, the rotation matrix and translation vector of the camera motion are restored through singular value decomposition:
[0053] E=U∑V T #(6)
[0054] Among them, U and V are orthogonal matrices, ∑ is a singular value matrix, and four motion hypotheses for R and t are obtained, and the best solution is obtained through triangulation. If there are not enough solutions, the initialization fails and the reference coordinate system is reset.
[0055] (4) Bundle adjustment;
[0056] BA is performed to optimize the initial reconstruction.
[0057] 2. The main process of the monocular visual inertial SLAM local mapping thread based on adaptive robust kernel:
[0058] (1) Process key frames;
[0059] In this process, key frames are selected from the image frame sequence, the BoW vectors of the key frame feature points are calculated, the connection between the key frames is established, and then the key frames are inserted into the map.
[0060] (2) Eliminate redundant map points;
[0061] Traverse all map points of the current key frame and remove those that do not meet the conditions.
[0062] (3) Add new map points;
[0063] During the camera movement, some map points can be restored through triangulation with adjacent key frames.
[0064] (4) If the current key frame is the last frame in the key frame queue, perform local BA.
[0065] Furthermore, the cost function that minimizes the reprojection error is obtained:
[0066] Map point 3D location Rotation and keyframe pose T iw ∈SE(3), where SE(3) is a special Euclidean group, W represents the world coordinate system, and the matching key points The reprojection error is optimized. The error term for observing map point j in key frame i is:
[0067] e i,j =x i,j -π i (T iw ,X w,j )#(7)
[0068] Among them, π i Represents the projection function:
[0069]
[0070] [x i,j y i,j z i,j ] T =R i,w X w,j +t i,w #(9)
[0071] Where R iw ∈SO(3) and T iw The rotation and translation part of SO(3) is a special orthogonal group, (f i,u ,f i,v ) and (c i,u ,c i,v ) is the focal length and principal point associated with the camera of the i-th frame, and u and v represent the dedistortion of the image. The cost function to minimize the reprojection error is:
[0072]
[0073] Where ρ is the Huber kernel function, is the projection coordinate, X α is the coordinate of the projection point in the world coordinate system, is the set of all matching feature points. In case of global BA, all points and keyframes are optimized, except for the first keyframe which is kept fixed as the origin. In local BA, all points contained in the local region are optimized, while a subset of keyframes is fixed. In pose optimization or motion-only BA, all points are fixed and only the camera pose is optimized.
[0074] Adaptive robust kernel is applied to BA optimization:
[0075] In BA optimization, the objective function is to minimize the sum of squared norms of the error terms. If a large error term appears, it will affect other correct values. The robust kernel function replaces the original error norm with a function that grows less quickly while ensuring the smoothness of the function, making the entire optimization result more robust. The adaptive robust kernel is as follows:
[0076]
[0077] By analyzing the value of ω, the final robust kernel function is:
[0078]
[0079] Among them, μ, ω, c are parameters, ω is used to control the robust kernel shape, c is generally a constant, and c>0 is a scale parameter used to constrain the size of the quadratic loss region near μ = 0. Interpolation is performed between a series of robust kernels by adjusting ω. In order to automatically determine the optimal kernel shape through the parameter ω, ω is regarded as an additional unknown parameter while minimizing the error:
[0080]
[0081] By choosing ω, all residuals are weighted to small values and the reprojection error is minimized without affecting the model parameters R and t.
[0082] (5) Redundant key frames are eliminated to reduce the amount of system calculation, and then the selected key frames are added to the loop detection thread for loop correction.
[0083] 3. In the loop detection thread of monocular visual inertial SLAM based on adaptive robust kernel, the similarity transformation Sim(3) calculation of the adaptive robust kernel applied to loop correction is:
[0084] (1) Detect whether the key frame sequence generates a loop;
[0085] (2) Calculate the similarity transformation matrix for the detected loop candidate keyframes;
[0086] Given keyframe 1 and keyframe 2, and a set of N matching keypoints in keyframes 1, 2, and the 3D map points associated with the keypoints, for each pair of matching points, find the following transformation relationship:
[0087] p 2,j =sRp 1,j +t#(13)
[0088] Where s is the scale factor, R is the rotation matrix, and t is the translation vector. In fact, the data will inevitably have noise and errors. Define the errors e1 and e2 and find a suitable combination of scale factors, rotations, and translations to minimize the error for all data. Optimize the relative Sim (3) transformation matrix To minimize the reprojection error in the two images:
[0089] e1=p 1,i -π1(S 12 ,P 2,j )#(14)
[0090]
[0091] An adaptive robust kernel function is applied to Sim(3) and the cost function to minimize:
[0092]
[0093] The above formula can be used to obtain the scale factor, rotation matrix, translation vector and parameter ω that satisfy the above relationship, thereby optimizing the entire closed loop and minimizing the closed loop error.
[0094] (3) Loop correction: Construct a continuous group of the current key frame and correct the pose of the key frame in the continuous group according to the calculated similarity transformation matrix; recalculate the world coordinates of all map points of adjacent key frames according to the updated camera pose (similarity transformation matrix), then perform map point fusion and perform global BA optimization.
[0095] The standard deviation (Std / m) of the experimental results of the present invention and ORB-SLAM3 on the KITTI data set is shown in the following table. By comparing the experimental results, it can be seen that the positioning accuracy of the present invention is better than that of ORB-SLAM3.
[0096]
[0097]
[0098] Based on the same inventive concept, an embodiment of the present invention provides a monocular visual-inertial SLAM system based on an adaptive robust kernel, including: a monocular initialization module for performing initial feature matching between the current frame and the reference frame, and calculating the two motion models of the homography matrix and the basic matrix in parallel, and selecting the homography matrix H or the basic matrix F by the geometric robust information content, and performing model decomposition and bundle adjustment BA optimization; a local mapping module for constructing a cost function that minimizes the reprojection error, and using an adaptive robust kernel for BA optimization; and a loop detection module for using an adaptive robust kernel to reduce the cumulative error in the loop correction process. The specific implementation of each module refers to the above method embodiment and is not repeated here.
[0099] Based on the same inventive concept, the present invention also provides a computer system, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the computer program is loaded into the processor, the aforementioned monocular visual-inertial SLAM method based on an adaptive robust kernel is implemented.
Claims
1. A monocular visual-inertial SLAM method based on an adaptive robust kernel, characterized in that: include: During the monocular initialization, the initial feature matching is performed between the current frame and the reference frame, and the two motion models of the homography matrix and the basic matrix are calculated in parallel. The homography matrix or the basic matrix is selected based on the geometric robust information content, and the model is decomposed and bundle adjustment BA optimization is performed. In the local mapping thread, a cost function that minimizes the reprojection error is constructed, and an adaptive robust kernel is used for BA optimization. In the loop detection thread, an adaptive robust kernel is used to reduce the accumulated error in the loop correction process; Model selection via geometrically robust information content, expressed as: Among them, G is the geometric robust information content, k is the key point sequence number, L is the robust function, e is the reprojection error, It is used to limit the size of the reprojection error. β represents the size of the model, where β = 2 is the homography matrix, β = 3 is the basic matrix, and n is the number of map points tracked between two keyframes. represents the number of degrees of freedom in the model, where represents the basic matrix, represents the homography matrix, θ 2 represents the covariance of the key point positioning error, λ represents the dimension of the data, and the two-dimensional correspondence between two frames is λ=4; The kernel function ρ of the adaptive robust kernel is expressed as: Among them, μ, ω, c are parameters, and the constant c>0 is used to constrain the scale of the secondary loss area near μ=0.
2. The monocular visual-inertial SLAM method based on an adaptive robust kernel according to claim 1, characterized in that: In the local mapping thread, the cost function that minimizes the reprojection error is expressed as: Among them, R is the rotation matrix, t is the translation vector, is the set of all matching feature points, ρ is the robust kernel function, is the projection coordinate, X α is the coordinate of the projection point in the world coordinate system, Represents the sum of squares of the reprojection error.
3. The monocular visual-inertial SLAM method based on an adaptive robust kernel according to claim 1, characterized in that: In the loop detection thread, the adaptive robust kernel function is applied to solve Sim(3), which is expressed as: Among them, S 12 is the transformation matrix, p 1,i is the key point i, p in key frame 1 1,j is the key point j that matches i in key frame 2, P 1,i is the 3D coordinate of key point i in the world coordinate system, P 2,j is the 3D coordinate of key point j matching key point i in the world coordinate system, N is the number of matching key points, π i Represents the projection function, i=1,2.
4. A monocular visual-inertial SLAM system based on an adaptive robust kernel, characterized in that: include: The monocular initialization module is used to perform initial feature matching between the current frame and the reference frame, calculate the homography matrix and the basic matrix in parallel, and select the homography matrix H or the basic matrix F according to the geometric robust information content to perform model decomposition and bundle adjustment BA optimization; The local mapping module is used to construct the cost function that minimizes the reprojection error and uses an adaptive robust kernel for BA optimization; and, a loop closure detection module for reducing the accumulated error in the loop closure correction process using an adaptive robust kernel; In the monocular initialization module, the model selection based on geometric robust information content is expressed as: Among them, G is the geometric robust information content, k is the key point sequence number, L is the robust function, e is the reprojection error, It is used to limit the size of the reprojection error. β represents the size of the model, where β = 2 is the homography matrix, β = 3 is the basic matrix, and n is the number of map points tracked between two keyframes. represents the number of degrees of freedom in the model, where represents the basic matrix, represents the homography matrix, θ 2 represents the covariance of the key point positioning error, λ represents the dimension of the data, and the two-dimensional correspondence between two frames is λ=4; The kernel function ρ of the adaptive robust kernel is expressed as: Among them, μ, ω, c are parameters, and the constant c>0 is used to constrain the scale of the secondary loss area near μ=0.
5. The monocular visual-inertial SLAM system based on the adaptive robust kernel according to claim 4, characterized in that: In the local mapping module, the cost function for minimizing the reprojection error is expressed as: Among them, R is the rotation matrix, t is the translation vector, is the set of all matching feature points, ρ is the robust kernel function, is the projection coordinate, X α is the coordinate of the projection point in the world coordinate system, represents the sum of squares of the second norm of the reprojection error; In the loop detection module, the adaptive robust kernel function is applied to solve Sim (3), which is expressed as: Among them, S 12 is the transformation matrix, p 1,i is the key point i, p in key frame 1 1,j is the key point j that matches i in key frame 2, P 1,i is the 3D coordinate of key point i in the world coordinate system, P 2,j is the 3D coordinate of key point j matching key point i in the world coordinate system, N is the number of matching key points, π i Represents the projection function, i=1,2.
6. A computer system comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the computer program is loaded into a processor, the monocular visual-inertial SLAM method based on an adaptive robust kernel according to any one of claims 1 to 3 is implemented.
Citation Information
Patent Citations
Video target tracking method based on manifold particle filter algorithm
CN102982556A
Improved nonlinear optimization method of monocular inertial navigation SLAM
CN110726406A
Monocular vision SLAM initialization method for improving robustness
CN112734839A
Visual inertia SLAM method based on image edge features
CN112749665A
Initialising keyframes for visual-inertial localisation and / or mapping
GB202016445D0
