Line feature-based SLAM system fused by RGBD camera and IMU

Abstract

The invention relates to the field of robot mapping, in particular to an efficient visual inertia synchronous localization and mapping (SLAM) method based on point and line features through fusion of an RGBD camera and an IMU, and a PL-RGBD-VINS. At present, when a traditional monocular camera is initialized, feature points and lines need to be triangularized, the calculation amount is large, errors are obvious, and SLAM based on points is poor in performance in weak texture or motion blur scenes. In an actual scene, the calculated amount of line extraction and description matching is huge, and the real-time performance of an SLAM system is difficult to guarantee. Therefore, the RGBD camera is adopted to avoid triangularization, and the line feature quality is improved through improvement measures such as short line fusion, line feature uniform distribution and adaptive threshold extraction. After a monocular camera of the VINS is expanded into an RGBD camera, a dense color point cloud can be generated, and line features are added to enhance positioning and mapping precision, so that a line map with better environmental characterization capability is constructed.

Description

Technical Field

[0001] The present invention relates to the field of synchronous positioning and map construction, and in particular to a SLAM system PL-RGBD-VINS based on line features and fused by an RGBD camera and an IMU. Background Art

[0002] Simultaneous localization and mapping (SLAM) refers to the process of completing self-positioning and simultaneously building a map of the surrounding environment by continuously observing environmental features in an unknown environment. In recent years, SLAM technology has played an important role in the fields of robots, drones, and the currently popular unmanned driving of cars. With the rapid advancement of the manufacturing of new energy smart cars and intelligent driving solutions in China, it is particularly important to study and improve SLAM technology. However, using only a single sensor has its own shortcomings: GNSS can only provide reliable positioning information when the weather is clear; LiDAR has reflection problems for objects on glass surfaces and cannot cope with degraded environments; IMU measurement results are noisy, so the inertial navigation system may drift rapidly due to error accumulation; monocular cameras are sensitive to light sources, and monocular SLAM can only recover a certain proportion of the motion trajectory, which is easy to lose when the camera moves quickly or the light changes drastically. Therefore, multi-sensor fusion SLAM technology has achieved more robust results by integrating the advantages of different sensors, and has become a leading research direction, especially the sensor fusion method of visual inertial navigation system has received widespread attention.

[0003] The monocular visual inertial navigation system (VINS-Mono) can work stably in complex indoor environments and complex and large-scale outdoor environments by tightly coupling monocular camera and IMU data through factor graph optimization, combined with relocalization, closed loop detection and global pose graph optimization. However, the monocular camera cannot provide depth information and requires a lot of computing resources to estimate the spatial position of feature points through triangulation. The RGB-D camera can provide direct measurement of image pixel depth and support the construction of dense 3D maps. On the other hand, there are abundant line features in man-made scenes, which can provide higher-dimensional constraints than traditional point features. In addition, line features can effectively compensate for the degradation problem of visual SLAM in weak texture scenes and improve the robustness and accuracy in texture-deficient environments. Therefore, this paper proposes a SLAM system based on line features and fused by RGBD camera and IMU, PL-RGBD-VINS, to improve the accuracy and stability of the monocular SLAM system based on point features, improve its poor performance in scenes such as weak texture and motion blur, and build dense color maps. Summary of the invention

[0004] The purpose of the present invention is to provide a point and line feature fusion visual-inertial SLAM system PL-RGBD-VINS adapted to RGB-D cameras. The whole system is developed based on VINS-MONO and EPLF-VINS, and mainly consists of three modules: data preprocessing, local visual odometer, and global loop optimization.

[0005] Data preprocessing includes extraction of single-frame visual features in the image, tracking of visual features between frames, and pre-integration of IMU data;

[0006] In the process of extracting features from a single-frame image, the adaptive uniformly distributed Shi-Tomasi corner detection algorithm is used for each frame of the image, combined with the EDLines algorithm for edge line feature extraction. This method effectively ensures the uniform distribution of the extracted features in the image, thereby improving the availability and stability of the features, and facilitating subsequent feature matching and tracking.

[0007] Inter-frame image feature tracking uses the LK point optical flow method to track image features, and the line optical flow method is used to track line features. Through this dual tracking strategy, the system can achieve accurate association between features between images. The reverse optical flow method is used to eliminate erroneous tracking and association, further enhancing the robustness and accuracy of the system in dynamic scenes.

[0008] At the same time, the IMU data is pre-integrated to provide continuous and stable motion information in subsequent steps, which significantly enhances the system's pose estimation accuracy and real-time performance.

[0009] Local visual odometry includes alignment of image features with IMU data, triangulation of visual features, local sliding window optimization, and key frame screening;

[0010] The feature associations between two consecutive frames of images and the IMU pre-integration results are strictly aligned according to the timestamp. This process ensures the consistency of the data in the time dimension and significantly improves the accuracy and reliability of pose estimation.

[0011] Next, the three-dimensional spatial coordinates of the new image point and line features are restored based on the depth map information as the initial pose value of the feature. This restoration process utilizes the depth information provided by the RGB-D camera to improve the accuracy of feature reconstruction and the richness of spatial information.

[0012] The new image features and IMU pre-integration results are incorporated into the sliding window, and residual constraints of image point features, line features, IMU pre-integration, and loop closure pose are constructed respectively, and nonlinear optimization is performed within the sliding window. This optimization process aims to improve the accuracy of the overall state estimation and the stability of the system by minimizing the residual.

[0013] After optimization, the disparity and descriptor of the pose are calculated to determine whether to generate a new keyframe. The generation of new keyframes not only helps enhance the system's understanding of the environment, but also promotes real-time updating and expansion of the map.

[0014] Global loop closure optimization includes loop closure detection and global factor graph optimization;

[0015] In the global optimization process, the key frames generated by the local visual odometry are searched for loop frames in the Bag of Words (BoW) database and the key frame database, and feature matching is used to determine whether there is a loop. Loop detection is one of the key technologies to improve long-term positioning accuracy and robustness.

[0016] If a loop is detected, the system will calculate the loop pose and globally optimize the pose of the keyframes using factor graph optimization technology. This global optimization process can effectively eliminate the accumulated trajectory errors and improve the positioning accuracy and consistency of the entire system.

[0017] If no loop is detected, the newly generated keyframe is added to the keyframe database, ensuring that the system can continuously adapt and expand its map in a changing environment.

[0018] During the global optimization process, the system also introduced the technology of dense color map reconstruction. By fusing the depth information and color information of multiple frames, the system is able to generate a dense color 3D map. The reconstruction process combines the restored 3D points with the corresponding RGB values ​​to ensure that the generated map has real color information and rich details, thereby improving the realism of the environment reconstruction. This dense map not only provides richer visual information for navigation, but also lays a solid foundation for subsequent tasks such as object recognition and scene understanding.

[0019] The innovation of the present invention is that the performance of the SLAM system in complex environments is significantly improved through the effective fusion of point and line features, especially the robustness and accuracy in sparse or dynamic scenes. In addition, the system ensures high efficiency and stability in dynamic and changing environments through efficient data preprocessing and sliding window optimization methods. The introduction of global loop optimization technology enables the system to effectively correct trajectory drift, ensuring that high-precision positioning and mapping capabilities are maintained in long-term operation and large-scale environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 It is a framework diagram of the PL-VINS-RGBD system of the present invention;

[0021] Figure 2 This is a schematic diagram of IMU pre-integration in the front-end visual odometer;

[0022] Figure 3Schematic diagram of extracting line features for EDLines;

[0023] Figure 4 This is a schematic diagram of the pose of point and line features restored in the camera coordinate system through the RGBD camera;

[0024] Figure 5 Schematic diagram for calculating point residuals and line residuals;

[0025] Figure 6 This is a standard definition picture of the experimental car;

[0026] Figure 7 This is a comparison chart of the test results of the proposed algorithm and VINS-MONO on the open source RGB-D IMU dataset. The blue, green, and dotted lines represent the VINS-MONO trajectory, the proposed algorithm trajectory, and the true value trajectory, respectively;

[0027] Figure 8 This is a comparison of the point cloud reconstruction results of VINS-MONO and the proposed algorithm in the dataset. VINS-MONO can only reconstruct discrete point clouds, while the proposed algorithm can generate dense colored point clouds, which are more suitable for environmental modeling and downstream tasks;

[0028] Fig. 9 This is the test result diagram in the Handheld-Simple dataset;

[0029] Fig.10 The running results of PL-RGBD-VINS and VINS-MONO in the Handheld dataset and Wheelrobot dataset. The gray dotted line represents the true trajectory, and the green and blue realizations represent the trajectory of PL-RGBD-VINS and VINS-MONO pose estimation respectively;

[0030] Fig.11 The following is a comparison chart of the running results of PL-RGBD-VINS and VINS-MONO in the Handheld dataset and Wheelrobot dataset. DETAILED DESCRIPTION

[0031] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0032] like Figure 1As shown, the present invention provides a SLAM system based on line features and fused with an RGBD camera and an IMU. The entire system is developed based on VINS-MONO and EPLF-VINS, and is mainly composed of three modules: a front-end visual odometer, a back-end nonlinear optimization, and a loop detection.

[0033] A SLAM system based on line features and fused with an RGBD camera and an IMU, wherein the front-end visual odometer includes image and IMU data preprocessing (IMU pre-integration), visual feature extraction and tracking, and odometer calculation;

[0034] The IMU pre-integration includes time synchronization of the image and the IMU input, and processing two adjacent frames of images and the IMU data collected during this period;

[0035] like Figure 2 As shown, firstly, the image and IMU data are aligned on the time axis according to the acquisition time of the image and IMU data;

[0036] Since high-frequency IMU data can provide motion information, for each frame of image data, the IMU data between the current frame and the previous frame are collected and packaged together with the current image frame into a processing unit for use by subsequent modules, thereby completing the data alignment between sensors;

[0037] For the input IMU data, the accelerometer and gyroscope data measured by the IMU are pre-integrated between two frames according to the IMU motion model;

[0038] In the PL-VINS-RGBD system, IMU pre-integration estimates the pose change by accumulating IMU data between image frames. This method reduces the impact of frequent IMU measurements on pose estimation and improves the real-time performance and accuracy of the system.

[0039] The main process of IMU pre-integration is as follows:

[0040] (1) Pre-integration formula: The pre-integration process uses the following formula to accumulate acceleration and angular velocity:

[0041] Pre-integrated displacement Δp, velocity Δv, and rotation ΔR:

[0042]

[0043] Among them, a wb and w wb are the IMU measurements of acceleration and angular velocity, respectively.

[0044] (2) Bias correction: Since there is bias in the IMU measurement value, bias correction is required. The corrected acceleration and angular velocity are:

[0045]

[0046] are the biases of acceleration and angular velocity respectively.

[0047] (3) Recursive formula:

[0048] The IMU pre-integration gradually updates the pre-integrated displacement, velocity, and rotation through a recursive formula:

[0049]

[0050]

[0051] A SLAM system based on line features and fused with an RGBD camera and an IMU, wherein the visual feature extraction and tracking includes the extraction and tracking of point features and the extraction and tracking of line features;

[0052] The extraction and tracking of point features include extracting Shi-Tomasi corner features for each frame of image data. Shi-Tomasi corner detection is a concise and efficient corner detection method that uses the minimum eigenvalue of the autocorrelation matrix to identify significant feature points in the image. It has significant changes in image gradients and is therefore easy to track through optical flow.

[0053] The present invention uses the KLT optical flow method to track point features, uses the reverse optical flow method to track and remove points with large differences, uses the improved line feature detection EDLines for each frame, and uses the optical flow line tracking method to match the line segments detected in the previous frame;

[0054] In order to ensure that the Shi-Tomasi corner points are evenly distributed in the image and not concentrated in certain areas, so as to control the distribution density and number of the corner points, the present invention adopts an image grid division method: the image is divided into a number of small grids k×k, each grid size is w×h;

[0055] Perform Shi-Tomasi corner point detection in each grid to find the local corner points in each grid. In each grid, select the first n corner points with the largest response value (if any) to ensure that a certain number of corner points are selected in each grid. Combine the corner points selected in all grids into the final corner point set.

[0056] Different from the common feature extraction-matching method, the present invention uses the optical flow method to track feature points between consecutive frames, which reduces the computational consumption of the matching process and alleviates the impact of unstable feature extraction on the system.

[0057] The optical flow method is based on the basic assumption of photometric invariance (in an image sequence, the brightness value of a moving object remains unchanged in a short period of time) and estimates the motion information in the scene by analyzing the motion of each pixel in the image sequence.

[0058] The optical flow model in the present invention is based on the grayscale invariance theory. First, the first frame of the video is read and converted into a grayscale image. The grayscale value of each pixel in the image is defined as:

[0059] I(x+dx,y+dy,t+dt)=I(u,v,t);

[0060] The Taylor expansion of the left side of the above equation yields:

[0061]

[0062] in, and Respectively represented as I u and I v , respectively represent the grayscale gradient of the image at that point in the u and v directions, Note as I t , represents the change of image gray value over time. Then the matrix form of the above formula is:

[0063]

[0064] According to the grayscale invariance assumption, the change in the image grayscale value over time is always 0, so the above formula can be integrated over time to obtain the following formula:

[0065]

[0066] By solving the equation through Gauss-Newton iteration, the relative motion du and dv of the feature points between two frames can be obtained, thereby realizing the tracking of the feature points.

[0067] In order to eliminate the points where tracking fails, the present invention performs forward and reverse calculations on the optical flow (from the reference frame to the current frame, and from the current frame to the reference frame), and eliminates the failed points based on the consistency error of the two calculation results.

[0068] A SLAM system based on line features and fused by an RGBD camera and an IMU, wherein the extraction and tracking of line features include extracting line features using an EDLines algorithm, which generates coherent line segments by detecting and connecting edge pixels;

[0069] As an edge extraction algorithm, EDLine contains a large number of short lines in its extraction results, and the line features are unevenly distributed. In order to extract higher quality line features and distribute them as evenly as possible in the image, the present invention makes the following improvements to the EDLine extraction algorithm: short line fusion, uniform distribution of line features, and adaptive threshold extraction;

[0070] To avoid occlusion and noise from splitting a line into multiple segments, we merge short lines by judging collinearity and endpoint distance, thereby improving the stability of line flow tracking and optimization;

[0071] To ensure that line features are evenly distributed in the entire image at a certain interval, we add a wide mask at the position of the detected line to prevent the extraction of new line features in the mask area to reduce information redundancy and computational workload;

[0072] If the number of extracted lines is still less than the preset threshold after applying the above strategy, we will reduce the gradient threshold parameter of EDLines and re-perform line feature extraction in the case of weak texture and small grayscale changes to ensure that sufficient line features are obtained.

[0073] Calculate line optical flow between consecutive frames: The line flow model in this invention extends the point optical flow model. For example, formula (x) is valid for any pixel in the image, so it is valid on the line segment, and the points on the line must also satisfy the collinearity constraint. This paper defines the set of points on the line:

[0074] l={(u1,v1),(u2,v2),…,(u n ,v n ),…,(u m ,v m )};

[0075] Finally, we get:

[0076]

[0077] like Figure 3 As shown, Figure 3 (a) and Figure 3 (b) The poses of the same feature line in different frames. By calculating du, dv and dθ in Figure (b), the pose of the line in the next frame is obtained, thereby achieving line matching between the two frames.

[0078] A SLAM system based on line features and fused with an RGBD camera and an IMU, wherein the odometer calculation includes calculating point features and line features on an image, measuring the depth of the points and lines using the depth information of the RGBD camera, and calculating the relative positions of the image points and lines in the camera coordinate system, thereby inferring the world coordinates of the points and lines;

[0079] like Figure 4 As shown in the figure, when restoring point features, the depth information of the pixel corresponding to the point on the image is taken to restore the position corresponding to the point in the camera projection model;

[0080] When restoring line features, several points are discretely taken on the line, and then the 3D depth information of these points is taken out. Then, the corresponding positions of these points in the camera projection model are restored through the depth information. Finally, these restored points are fitted into a line to calculate the relative position of the line under the camera projection model.

[0081] A SLAM system based on line features and fused by an RGBD camera and an IMU, wherein the back-end nonlinear optimization comprises:

[0082] Triangulate points and lines and construct reprojection residuals;

[0083] In order to obtain better landmark estimates, the inverse depth of point features is estimated using all observations;

[0084] For line triangulation, we only select the two frames with the farthest spatial distance in the sliding window to initialize coordinate, Although the coordinates can concisely represent the transformation process, they are not easy to optimize directly due to their over-parameterization. Therefore, a standard orthogonal representation consisting of four parameters is used for optimization;

[0085] The three-dimensional space line L can be expressed as: L = (U, W), U∈SO(3) and W∈SO(2). The equation from the coordinate system to the orthogonal coordinate system can be expressed as:

[0086]

[0087] The residuals of points and lines are Figure 5 As shown, after we get the initial estimate of the map points / lines and the IMU body state predicted by the IMU measurements, we use sliding window optimization to find the optimal state. All state variables in the sliding window are optimized by minimizing the sum of the cost terms of all measurement residuals:

[0088]

[0089] in is the measurement residual of IMU from frame i to frame i+1 in the body coordinate system, X b is the set of all IMU pre-integrations in the sliding window, and are the reprojection errors of point features and line features, respectively, p and X lare the point features and line features observed in the first frame, respectively, e prior is the prior information calculated after the sliding window edge is marginalized for one frame, ρ is the Cauchy function that suppresses outliers;

[0090] In order to limit the size of the state vector X, a bidirectional marginalization strategy is used to remove states from the sliding window. n+N-1 When it is a key frame, the earliest frame x n and all its measured values ​​are marginalized;

[0091] If the second latest frame is not a key frame, the visual measurement of this frame will be abandoned, and its IMU measurement will be retained in the pre-integration measurement. New prior information is obtained based on the edge measurement, and the constraint information of the removed state is retained;

[0092] Finally, invalid map points and lines are culled. If a point’s inverse depth is negative, we remove the point from the map. If a line’s reprojection residual exceeds a threshold, it is removed from the map.

[0093] A SLAM system based on line features and fused by an RGBD camera and an IMU, wherein the loop detection includes:

[0094] Loop closure optimization is a key technique used to improve the long-term accuracy and robustness of visual inertial odometry;

[0095] The system of the present invention calculates image similarity through a bag-of-words vector keyframe database to determine loop candidate frames. Once it is detected that the features of the current frame are highly similar to those of a certain historical keyframe, it is determined that there is a loop.

[0096] To avoid mismatching, the system performs geometric consistency verification to ensure that the detected loops are reliable;

[0097] After confirming the loop, the system will perform four-degree-of-freedom pose optimization, eliminate mismatches, and align the camera pose with the loop pose through tightly coupled joint optimization (bundle adjustment, BA), and then perform global pose optimization on all key frames to eliminate accumulated errors and trajectory drift. New loop constraints are added to the graph through the pose graph optimization algorithm;

[0098] This step involves recalculating the poses of all keyframes to minimize the reprojection error of the entire graph;

[0099] After adding the loop closure constraint, the system performs local and global optimization to adjust the poses of adjacent frames and the entire trajectory to reduce the cumulative error;

[0100] Through loop optimization, the system can effectively correct the accumulated drift errors, ensuring high-precision positioning and mapping in long-term operation and large-scale environments.

[0101] The experimental results are as follows Fig. 9 As shown, the first row of left pictures shows the comparison between the trajectories of PL-RGBD-VINS and VINS-MONO and the true trajectory. It can be seen that PL-RGBD-VINS is closer to the true trajectory and has the ability to estimate poses with higher accuracy. The middle and right pictures in the first row show the dense color point cloud map constructed by PL-RGBD-VINS and the sparse point cloud map constructed by VINS-MONO, respectively. It can be seen that the map constructed by PL-RGBD-VINS contains more information, has better visualization effects and wider application potential. The original RGB and depth map data corresponding to the scenes in the red and yellow dotted boxes are shown in the second row.

[0102] pass Fig.10 and Fig.11 The experimental data comparison shows that in all sequences, the trajectory of PL-RGBD-VINS is closer to the true trajectory and has a smaller error. Especially in the Handheld dataset, PL-RGBD-VINS shows a clear advantage. Therefore. PL-RGBD-VINS has higher accuracy than VINS-MONO. The quantitative error comparison is shown in the following table. Compared with VINS-MONO, the average displacement error of PL-RGBD-VINS in all sequences is reduced from 0.237m to 0.114m, and the accuracy is improved by 51.9%. Improve the accuracy and stability of the monocular SLAM system based on point features, improve its poor performance in scenes such as weak textures and motion blur, and build a dense color map.

[0103] Compared with VINS-MONO, the SLAM system based on line features and fused with RGBD camera and IMU proposed in this invention reduces the average displacement error of PL-RGBD-VINS in all sequences from 0.237m to 0.114m, and the accuracy is improved by 51.9%. The accuracy and stability of the monocular SLAM system based on point features are improved, and its poor performance in scenes such as weak texture and motion blur is improved, and a denser color map is constructed. The constructed map is more accurate and robust, which has a certain reference value in the field of synchronous positioning and map construction.

Claims

1. A point and line feature fusion visual-inertial SLAM system (PL-RGBD-VINS) adapted to RGB-D camera, characterized in that: The system comprises: Data preprocessing module, used to extract and process image features; Local visual odometry module for pose estimation and optimization; A global loop closure optimization module is used to correct trajectory drift.

2. The system according to claim 1, wherein: The data preprocessing module is specifically used for: Extract adaptive and evenly distributed Shi-Tomasi corner features in each new frame to ensure the diversity and uniformity of features in the image; Apply EDLines algorithm to extract edge line features to improve the system's ability to recognize line features; The LK optical flow method is used to track point features, and the line optical flow method is used to track line features to enhance the correlation between features.

3. The system according to claim 2, wherein: The inter-frame feature tracking eliminates erroneous tracking results through the inverse optical flow method, further improving the robustness and accuracy of the system in dynamic environments.

4. The system according to claim 1, wherein: The local visual odometry module includes: Temporally align image features with IMU data to ensure data consistency in the time dimension; Use the depth map information to triangulate the image points and line features and restore their coordinates in three-dimensional space as the initial value for pose estimation; The sliding window optimization method is used to perform nonlinear optimization on the residual constraints of image features, IMU pre-integration and loop closure pose to improve the state estimation accuracy of the overall system.

5. The system according to claim 4, wherein: The triangulation step uses the depth information provided by the RGB-D camera to accurately calculate the position of image features in three-dimensional space, thereby improving the accuracy of pose estimation.

6. The system according to claim 1, wherein: The global loop optimization module includes: The loop detection function searches for similar key frames in the Bag of Words database to determine whether the current frame has a loop relationship with the historical key frames; When a loop is detected, the loop pose is calculated and the pose of the keyframes is globally optimized through factor graph optimization to eliminate the accumulated trajectory error.

7. The system according to claim 6, wherein: The global optimization process involves recalculating the poses of all keyframes to minimize the reprojection error of the entire graph, thereby improving the global localization accuracy of the system.

8. The system according to claim 1, wherein: The system also includes a color map dense reconstruction module based on depth information, which generates a dense color three-dimensional map by fusing depth information and color information of multiple frames of images.

9. The system according to claim 8, wherein: The color map dense reconstruction module generates a dense 3D map with real color information by combining the restored 3D points with the corresponding RGB values ​​to improve the realism and detail richness of the environment reconstruction.

10. The system according to any one of claims 1 to 9, characterized in that: The system significantly improves the robustness and accuracy of the SLAM system in complex environments through the effective fusion of point and line features, especially in scenes with sparse features or dynamic changes.

11. The system according to any one of claims 1 to 10, characterized in that: The system is able to maintain high-precision positioning and mapping capabilities over long periods of time and in a wide range of environments, ensuring continuous adaptability in complex and dynamic environments.

12. The system according to any one of claims 1 to 11, wherein: The architecture design of the system supports the integration of multiple sensors, improving the flexibility and scalability of the system in different application scenarios.

Images