RGB-D SLAM pose optimization method constrained by dual visual odometry

Abstract

The present invention relates to the technical field of synchronous positioning and map construction, and in particular to an RGB-D SLAM pose optimization method constrained by dual visual odometry. The method improves on the traditional RGB-D SLAM technology, constructs a first visual odometry based on ORB feature point matching calculation, and constructs a second visual odometry based on a point cloud data registration method. The dual visual odometry is used to add back-end map optimization constraints, thereby effectively correcting the drift generated by system operation without affecting the normal operating speed of RGB-D SLAM. At the same time, local optimization is used to better improve the accuracy of map construction. In addition, since the visual odometry based on ORB feature points in the present invention optimizes all frames in the image, the influence of the superposition of early errors on subsequent frame positioning and mapping can be reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of synchronous positioning and map construction, and in particular to an RGB-D SLAM pose optimization method constrained by dual visual odometry. Background Art

[0002] In recent years, simultaneous localization and mapping (SLAM) technology, which relies on its own sensors to achieve real-time positioning and tracking, has developed rapidly and is widely used in machine vision fields such as robot navigation and autonomous driving.

[0003] Based on the number and type of sensors, visual SLAM can be categorized as monocular SLAM, binocular SLAM, and RGB-D SLAM. The ORB-SLAM algorithm, proposed by Dr. Raulmur in 2015, is currently the most effective monocular SLAM solution. The subsequently released ORB-SLAM2 system is compatible with monocular, binocular, and RGB-D sensors and builds semi-dense maps in real time. ORB-SLAM methods primarily use image ORB feature points for matching, tracking, and pose transformation calculations. However, large variations between images can lead to significant errors in pose matrix calculations, impacting subsequent localization and mapping. Existing RGB-D SLAM technologies primarily aim to build dense 3D point cloud maps, but their localization and tracking functions are still based on RGB image matching. Some RGB-D SLAM technologies utilize depth information from image feature points to assist bundle adjustment for pose optimization, but their accuracy is still determined by the matching accuracy of the ORB feature points extracted by the monocular SLAM. Existing RGB-D SLAM technologies do not effectively utilize the depth data provided by depth cameras to improve localization and tracking accuracy. Summary of the Invention

[0004] The purpose of the present invention is to provide an RGB-D SLAM pose optimization method constrained by dual visual odometry, aiming to solve the technical problems that existing simultaneous localization and mapping methods cannot take into account both speed and accuracy at the same time and the pose data provision efficiency is low.

[0005] To achieve the above object, the present invention provides an RGB-D SLAM pose optimization method constrained by dual visual odometry, comprising the following steps:

[0006] Calibrate the RGB-D sensor to obtain the intrinsic parameters and relative extrinsic parameters of the RGB camera and depth camera;

[0007] Use the calibrated RGB-D sensor to acquire RGB and depth images and set the RGB-DSLAM system parameters.

[0008] Selecting an initial frame and setting the initial frame as the zero coordinate of the map;

[0009] Construct the first visual odometry and obtain the initial pose matrix of each frame image;

[0010] Build the second visual odometry and obtain the constraint matrix;

[0011] Pose graph optimization based on dual visual odometry;

[0012] Loop detection and closed loop correction;

[0013] Perform global optimization to improve pose accuracy;

[0014] Complete map construction.

[0015] Among them, in the process of calibrating the RGB-D sensor and obtaining the intrinsic parameters and relative extrinsic parameters of the RGB camera and the depth camera, the Zhang Zhengyou calibration method is used to calibrate the RGB-D sensor. The intrinsic parameters include the camera's principal point, focal length, and distortion parameters. The relative extrinsic parameters are obtained by calibrating the relative relationship between the RGB camera and the depth camera.

[0016] Among them, RGB images and depth images are acquired by synchronously collecting RGB cameras and depth cameras. The RGB-D SLAM system parameters include the number of feature points, scale factor and image acquisition frequency.

[0017] The initial frame is the first frame of the RGB image and the depth image, which is set as the zero coordinate of the map and the image feature points are extracted for the next frame matching calculation.

[0018] Among them, the first visual odometry is constructed based on ORB feature point matching calculation, and the initial pose matrix is ​​obtained by performing ORB feature extraction and matching calculation on the RGB image.

[0019] The second visual odometry is constructed based on a point cloud data registration method, and a constraint matrix is ​​obtained through point cloud registration.

[0020] The difference between the first visual odometry and the second visual odometry lies in the different speed and accuracy of pose estimation. The first visual odometry has a fast pose estimation speed but low accuracy, while the second visual odometry has a high pose estimation accuracy but a slow speed.

[0021] Among them, in the pose graph optimization process based on dual visual odometry, the sliding window size and the frame interval parameters of the two visual odometry are first set. Then, after reaching the sliding window, the initial matrix obtained by the first visual odometry is used as the initial value and the constraint matrix given by the second visual odometry is used as the restriction condition, and the camera pose is optimized according to the graph optimization calculation theory.

[0022] Among them, in the process of loop detection and closed-loop correction, loop detection is performed based on the BOW bag-of-words model, and after the loop conditions are met, loop constraints are added to perform global pose optimization.

[0023] Among them, in the process of completing the map construction, two-dimensional path map construction and three-dimensional point cloud data map construction are carried out simultaneously while real-time tracking and positioning.

[0024] The present invention provides an RGB-D SLAM pose optimization method constrained by dual visual odometry. The method improves on the traditional RGB-D SLAM technology, constructs a first visual odometry based on ORB feature point matching calculation, and constructs a second visual odometry based on a point cloud data registration method. The method efficiently gives an initial pose value through high-speed, low-precision feature point matching calculation in the first visual odometry, and obtains a constraint matrix as a reliable restriction condition through low-speed, high-precision point cloud registration calculation in the second visual odometry. The two are combined under graph optimization calculation theory to output optimized pose parameters, thereby improving the system positioning accuracy without affecting the normal operating speed of RGB-D SLAM. In addition, since the visual odometry based on ORB feature points in the present invention optimizes all frames in the image, the influence of the superposition of early errors on subsequent frame positioning and mapping can be reduced. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0026] Figure 1 This is a flow chart of a dual-visual odometry-constrained RGB-D SLAM pose optimization method of the present invention.

[0027] Figure 2 This is a schematic diagram of the execution steps of a dual-visual odometry-constrained RGB-D SLAM pose optimization method of the present invention.

[0028] Figure 3 It is a schematic diagram of the traditional nonlinear optimization method.

[0029] Figure 4 Schematic diagram of the dual-visual odometry constrained pose optimization method of the present invention.

[0030] Figure 5 Schematic diagram of the basic framework of the dual-visual odometer of the present invention.

[0031] Figure 6 3D point cloud data according to a specific embodiment of the present invention. DETAILED DESCRIPTION

[0032] The following describes embodiments of the present invention in detail, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and are not to be construed as limiting the present invention.

[0033] See also Figure 1 , the present invention proposes a dual visual odometry constrained RGB-D SLAM pose optimization method, comprising the following steps:

[0034] S1: Calibrate the RGB-D sensor to obtain the intrinsic parameters and relative extrinsic parameters of the RGB camera and depth camera;

[0035] S2: Use the calibrated RGB-D sensor to acquire RGB and depth images and set the RGB-DSLAM system parameters.

[0036] S3: Selecting an initial frame and setting the initial frame as the zero point coordinate of the map;

[0037] S4: Build the first visual odometry and obtain the initial pose matrix of each frame image;

[0038] S5: Build the second visual odometry and obtain the constraint matrix;

[0039] S6: Pose graph optimization based on dual visual odometry;

[0040] S7: loop detection and closed loop correction;

[0041] S8: Perform global optimization to improve pose accuracy;

[0042] S9: Complete map construction.

[0043] When calibrating the RGB-D sensor and obtaining the intrinsic parameters and relative extrinsic parameters of the RGB camera and depth camera, the Zhang Zhengyou calibration method is used to calibrate the RGB-D sensor. The intrinsic parameters include the camera's principal point, focal length, and distortion parameters. The relative extrinsic parameters are obtained by calibrating the relative relationship between the RGB camera and the depth camera.

[0044] RGB images and depth images are acquired through synchronous acquisition by an RGB camera and a depth camera. The RGB-DSLAM system parameters include the number of feature points, scale factor, and image acquisition frequency.

[0045] The initial frame is the first frame of the RGB image and the depth image, which is set as the zero coordinate of the map and the image feature points are extracted for the next frame matching calculation.

[0046] The first visual odometry is constructed based on ORB feature point matching calculation, and the initial pose matrix is ​​obtained by performing ORB feature extraction and matching calculation on the RGB image.

[0047] The second visual odometry is constructed based on a point cloud data registration method, and a constraint matrix is ​​obtained through point cloud registration.

[0048] The difference between the first visual odometry and the second visual odometry lies in the different pose estimation speed and accuracy. The first visual odometry has a fast pose estimation speed but low accuracy, while the second visual odometry has a high pose estimation accuracy but a slow speed.

[0049] In the process of optimizing and calculating SLAM pose parameters based on the pose graph of dual visual odometry, the sliding window size and the frame interval parameters of the two visual odometry are first set. Then, after reaching the sliding window, the camera pose is optimized according to the graph optimization calculation theory, using the initial matrix obtained by the first visual odometry as the initial value and the constraint matrix given by the second visual odometry as the restriction condition.

[0050] During the process of loop detection and closed-loop correction, loop detection is performed based on the BOW bag-of-words model, and after the loop conditions are met, loop constraints are added to perform global pose optimization.

[0051] During the map construction process, two-dimensional path map construction and three-dimensional point cloud data map construction are carried out simultaneously with real-time tracking and positioning.

[0052] The detailed execution process is as follows Figure 2 As shown, the implementation steps are further detailed and explained below:

[0053] Step 1: Use Zhang Zhengyou calibration method to calibrate the RGB-D sensor's intrinsic parameter K, extrinsic parameter, and relative extrinsic parameter RT to obtain the principal point, focal length, and distortion parameters of the RGB-D sensor, as well as the relative extrinsic parameters of the RGB sensor and depth sensor.

[0054] The parameters described in step 1 are collected by calibrating the image with the help of a checkerboard. The relevant parameters can be obtained using the Zhang Zhengyou calibration method. The obtained parameters are used to calibrate and correct the sensor to improve the measurement accuracy.

[0055] Step 2: Use the calibrated RGB-D sensor to synchronously acquire continuous color images and depth images;

[0056] In step 2, it is important to ensure that the RGB camera and the depth camera collect data synchronously, otherwise the results will be affected;

[0057] Step 3: Set parameters, including the number of feature points, sliding window size, image acquisition frequency, etc.

[0058] The number of feature points N described in step 3 is generally set to 1000, the sliding window L is set to 15, and the image acquisition frequency is 30fps / s;

[0059] Step 4: Initialize the RGB image and depth map of the first frame as the initial frame, and set it as the zero point coordinate of the map. At the same time, extract the RGB image feature point information as the feature information for the next frame matching;

[0060] In step 4, when the number of feature points detected in the first frame reaches the set threshold, the first frame is defaulted to the initial frame. Subsequent calculations are performed based on this frame, and this frame is set as the initial zero coordinate of the map;

[0061] Step 5: Construct a visual odometry based on the ORB feature point matching method (called the "first visual odometry"). Its main technologies and methods are basically the same as those of ORB-SLAM2, mainly including feature point extraction and feature matching of images, calculation of basic matrix and essential matrix, conversion calculation of feature points to 3D points, initial pose estimation, and pose optimization of single-frame bundle adjustment;

[0062] Step 5 includes the following specific steps:

[0063] 1) Extract ORB features from the RGB image and perform matching calculations with the feature points extracted from the previous frame. Generally, the 8-point method is used to calculate the essential matrix H or basic matrix F of the two frames;

[0064]

[0065] 2) Combining the calculated essential matrix or fundamental matrix with the camera's intrinsic parameters, the transformation matrix (R, t) between the two frames can be calculated, and the camera's pose matrix P can be calculated based on the defined initial frame;

[0066]

[0067] 3) Triangulate the feature points matched in the two frames and calculate the three-dimensional coordinates of the feature points. Given the pixel coordinates of the feature points on the two frames, the principle of bundle adjustment can be used to optimize the camera pose and feature point coordinates, minimizing the reprojection error, that is, minimizing the value of the following formula;

[0068]

[0069] Through step 5, the pose matrix P of each frame of RGB image can be obtained and used as the initial pose to be optimized;

[0070] Step 6: Convert the depth map input according to the interval frame into point cloud data;

[0071] The conversion formula of step 6 is:

[0072]

[0073] Step 7: Construct a visual odometry based on the point cloud data registration method (called "second visual odometry"), and obtain the constraint matrix Q through the point cloud registration in the visual odometry;

[0074] Set the relevant parameters of the visual odometry, including the interval frame value I, the sliding window K, and the downsampling ratio β;

[0075] The parameter interval frame I described in step 7 is 5, and the sliding window size K is 15. The selection of these values ​​depends on the synchronization between the calculation speed of point cloud matching and the speed of feature point matching, so as not to affect the real-time performance of the entire image. The specific values ​​are obtained from actual experimental tests.

[0076] Step 8: Point cloud ICP registration;

[0077] In step 8, to ensure the speed and accuracy of registration, the point cloud data needs to be downsampled and simply filtered before point cloud registration. Downsampling is achieved using octree sampling, and filtering is generally done by counting outliers to remove noise.

[0078] The implementation formula of ICP registration is:

[0079]

[0080] Finally, the pose transformation matrix of the corresponding frame is obtained, which is used as the optimized constraint matrix Q;

[0081] Step 9: Graph optimization of dual visual odometry;

[0082] Traditional graph optimization methods such as Figure 3 As shown in the figure, circles represent feature points and triangles represent camera poses. This method can optimize the coordinates of feature points and the camera pose at the same time. However, graph optimization methods generally only use the pose transformation relationship between frames as a constraint to optimize the pose of each camera. This method is monotonic and has large errors in its results. This method is used in the single-frame optimization of the ORB feature point matching method, but its accuracy is limited.

[0083] The pose graph optimization of the dual visual odometry described in step 9 is shown in the schematic diagram below: Figure 4As shown in the figure, due to the limitations of traditional graph optimization, the graph optimization method of the present invention adds a constraint to the traditional method. The camera pose P calculated by the odometry of ORB feature point matching is used as the initial pose, and a pose transformation matrix between intervals of I frames is added as a constraint for graph optimization to improve the accuracy of the camera pose.

[0084] The architecture of dual visual odometry is as follows Figure 5 As shown in the figure, its working principle is to efficiently obtain the initial pose value through the high-speed, low-precision feature point matching calculation in the first visual odometry, and obtain the constraint matrix as a reliable constraint condition through the low-speed, high-precision point cloud registration calculation in the second visual odometry. The two are combined under the graph optimization calculation theory to output the optimized pose parameters;

[0085] Step 10: Loop detection and closed loop correction;

[0086] The technology used in step 10 is mainly based on the BOW bag-of-words model. By performing real-time loop detection on each key frame, when a loop is detected, the loop pose is added to the entire loop data, and the BA optimization method is used to perform closed loop correction to improve the overall accuracy and reduce the cumulative error.

[0087] Step 11: Global optimization. Global optimization is performed after all frames are calculated or when the loop condition is met. The global BA method is used for optimization to ensure the overall accuracy of all data.

[0088] Step 12: Map construction: While tracking and positioning in real time, two-dimensional path map construction and three-dimensional point cloud data map construction are also carried out synchronously;

[0089] Step 13: Output the results. Write the timestamp and pose information of each frame of image into a text file for storage. The pose is expressed in quaternion form to facilitate subsequent data analysis and result comparison.

[0090] The present invention also provides a specific embodiment, which builds an RGB-D_SLAM system based on the Linux platform, conducts experiments on scene image sequences of the public RGB-D dataset TUM, and compares them with open source RGB-D visual SLAM solutions. The experimental data are shown in Table 1:

[0091] Table 1: Comparison of system positioning accuracy

[0092] method trajectory error Rotation error / (°) ATE / m RPE / m ORB_SLAM2 0.033261 0.005028 0.233718 0.0325 Ours(double-slam) 0.029046 0.004857 0.108450 0.0282

[0093] The dense 3D point cloud map constructed experimentally is as follows Figure 6 shown.

[0094] In summary, in order to address the problem that existing RGB-D SLAM technology does not fully utilize depth data, resulting in errors in the positioning and map construction process, the present invention fully utilizes depth information, uses the point cloud matching method to calculate the pose transformation matrix, and proposes a RGB-D SLAM pose optimization method for dual-visual odometry. A dual-visual odometry is constructed, and a back-end optimization framework based on graph optimization is added to the graph optimization to effectively correct the drift caused by the system operation. Experimental results confirm that the method proposed in the present invention can improve the accuracy of camera pose to a certain extent, thereby improving the accuracy of synchronized positioning and mapping.

[0095] The above disclosure is only a preferred embodiment of the present invention, and certainly cannot be used to limit the scope of the rights of the present invention. Ordinary technicians in this field can understand that all or part of the processes of the above embodiment and equivalent changes made in accordance with the claims of the present invention are still within the scope of the invention.

Claims

1. A dual-visual odometry constrained RGB-D SLAM pose optimization method, characterized in that: The following steps are involved: Calibrate the RGB-D sensor to obtain the intrinsic parameters and relative extrinsic parameters of the RGB camera and depth camera; Use the calibrated RGB-D sensor to acquire RGB and depth images and set the RGB-DSLAM system parameters. RGB and depth images are acquired by synchronously capturing RGB and depth images with an RGB camera and a depth camera. RGB-DSLAM system parameters include the number of feature points, scale factor, and image acquisition frequency. Selecting an initial frame and setting the initial frame as the zero coordinate of the map; Construct the first visual odometry and obtain the initial pose matrix of each frame image; Build the second visual odometry and obtain the constraint matrix; The second visual odometry is constructed based on a point cloud data registration method, and a constraint matrix is ​​obtained through point cloud registration; The relevant parameters of the second visual odometry include the interval frame value I, the sliding window K, and the downsampling ratio β. The selection of the parameter value depends on the synchronization of the calculation speed of the point cloud matching and the speed of the feature point matching; Pose graph optimization based on dual visual odometry; Specifically, the camera pose P calculated by the odometry of ORB feature point matching is used as the initial pose, and a pose transformation matrix between intervals of I frames is added as a constraint for graph optimization; Loop detection and closed loop correction; Perform global optimization to improve pose accuracy; Complete map construction.

2. The RGB-D SLAM pose optimization method constrained by dual visual odometry as claimed in claim 1, wherein When calibrating the RGB-D sensor and obtaining the intrinsic parameters and relative extrinsic parameters of the RGB camera and depth camera, the Zhang Zhengyou calibration method is used to calibrate the RGB-D sensor. The intrinsic parameters include the camera's principal point, focal length, and distortion parameters. The relative extrinsic parameters are obtained by calibrating the relative relationship between the RGB camera and the depth camera.

3. The RGB-D SLAM pose optimization method constrained by dual visual odometry as claimed in claim 1, wherein The initial frame is the first frame of the RGB image and the depth image, which is set as the zero coordinate of the map and the image feature points are extracted for the next frame matching calculation.

4. The RGB-D SLAM pose optimization method constrained by dual visual odometry as claimed in claim 1, wherein The first visual odometry is constructed based on ORB feature point matching calculation, and the initial pose matrix is ​​obtained by performing ORB feature extraction and matching calculation on the RGB image.

5. The RGB-D SLAM pose optimization method constrained by dual visual odometry as claimed in claim 1, wherein The difference between the first visual odometry and the second visual odometry lies in the different pose estimation speed and accuracy. The first visual odometry has a fast pose estimation speed but low accuracy, while the second visual odometry has a high pose estimation accuracy but a slow speed.

6. The RGB-D SLAM pose optimization method constrained by dual visual odometry as claimed in claim 1, wherein In the pose graph optimization calculation process based on dual visual odometry, the sliding window size and the frame interval parameters of the two visual odometry are first set. Then, after reaching the sliding window, the initial matrix obtained by the first visual odometry is used as the initial value and the constraint matrix given by the second visual odometry is used as the restriction condition, and the camera pose is optimized according to the graph optimization calculation theory.

7. The RGB-D SLAM pose optimization method constrained by dual visual odometry as claimed in claim 1, wherein During the process of loop detection and closed-loop correction, loop detection is performed based on the BOW bag-of-words model, and after the loop conditions are met, loop constraints are added to perform global pose optimization.

8. The RGB-D SLAM pose optimization method constrained by dual visual odometry as claimed in claim 1, wherein During the map construction process, two-dimensional path map construction and three-dimensional point cloud data map construction are carried out simultaneously with real-time tracking and positioning.

Images