A dynamic environment RGB-D visual slam method based on point cloud region correlation

Abstract

A dynamic environment RGB-D visual SLAM method based on point cloud region correlation can be used as the front-end pretreatment of ORBSLAM2 / 3 system, filters out the dynamic region, thereby reduces the wrong data association. The input of the whole system needs the current frame of RGB image and depth map to generate point cloud, after processing a frame, it is temporarily saved in the system as a reference frame without secondary processing. The clustering algorithm is used for clustering the depth map to divide the sub-point cloud, then the sub-pose change between the sub-point cloud and the reference frame is calculated in turn, at the same time, the idea of sub-point cloud dynamic and static correlation is proposed, combined with the idea and the sub-pose, which can effectively identify which sub-point cloud belongs to the dynamic point cloud, without relying on the epipolar geometry constraint, and without knowing the approximate self-pose of the camera in advance. The application can well reduce the influence of dynamic objects on the positioning of the robot, and improve the robustness of the system and the generalization of the scene.

Description

Technical Field

[0001] This invention relates to the field of robot global localization, specifically a dynamic environment RGB-D visual SLAM method based on point cloud region correlation. Background Technology

[0002] Visual real-time localization and mapping (SLAM) is currently a widely studied field, and it is being explored as a core component of localization in various devices, such as assistive devices for the blind, augmented reality technology, robotics, and autonomous driving. After nearly 20 years of development, the overall framework of visual SLAM has matured, and many advanced visual localization systems with high accuracy and robustness have been proposed. These vision-based systems can estimate the camera's self-motion from continuous images captured of the surrounding environment and generate a continuous motion trajectory representing the camera. However, to simplify the estimation problem, most excellent systems are designed under the assumption that the surrounding environment is static. If there are dynamic objects in the environment, these irregularly moving landmarks will be introduced into the actual observation, causing the system to be unable to distinguish whether it is itself moving or objects in the environment moving, ultimately leading to inaccurate localization information and serious deviations in environmental map construction. To address the poor performance of visual SLAM in dynamic environments, researchers have implemented various measures, such as patent number CN115393538A, which uses the GCNv2 algorithm to extract features from image information and employs an optical flow dynamic point removal algorithm to remove dynamic feature points from the image information; and patent number CN114283198A, which uses the YOLOv5 object detection neural network to detect dynamic targets in images; combining image depth information with a pixel-level segmentation method to achieve pixel-level segmentation. Current methods are all based on deep learning. However, deep learning-based methods require accurate prior knowledge of which objects are moving and which will appear within the device's field of view. Furthermore, the computational demands of the inference model are enormous, making them difficult to apply in low-cost, high-real-time scenarios.

[0003] Currently, robot localization in dynamic environments suffers from the following shortcomings: 1) It requires prior pose estimation information to identify unknown feature points, which may directly cause system failure in harsh environments. 2) Deep learning-based methods require accurate prior knowledge of which objects are moving and which will appear within the device's field of view. Furthermore, the computational demands of the inference model are enormous, making it difficult to apply in low-cost, high-real-time scenarios. 3) Geometric methods are prone to false negatives (static objects being labeled as dynamic). 4) The presence of dynamic objects in real-world environments is unavoidable, significantly impacting the device's localization accuracy and hindering further development of visual SLAM. Summary of the Invention

[0004] To address the aforementioned shortcomings of existing technologies, this invention provides a dynamic environment RGB-D visual SLAM method based on point cloud region correlation, which can improve localization accuracy and robustness in dynamic real-world environments. This invention is an independent segmentation method that does not require consideration of camera motion and eliminates the projective transformation relied upon by most current algorithms. Furthermore, it can achieve dynamic region recognition without the need for training samples or prior information about dynamic objects.

[0005] To achieve the above objectives, the present invention provides the following technical solution: a dynamic environment RGB-D visual SLAM method based on point cloud region correlation, comprising the following steps:

[0006] S1: Perform two-dimensional planar downsampling on the RGB image and combine it with the depth map to generate a point cloud;

[0007] The specific steps for generating point clouds include:

[0008] S11: For the current frame, the grayscale image is downsampled on a two-dimensional plane to generate a point cloud by simulating the data acquisition method of a rotating lidar, so as to accelerate the construction of the KD tree.

[0009] S12: Rasterize the downsampled point cloud using grids with side length l, and calculate the i-th grid G. i The average grayscale value of all points within the grid is used as the intensity value of the grid centroid.

[0010]

[0011] Represents grid G i The intensity value, I(x,y) represents the gray value of the pixel with coordinates (x,y).

[0012] S2: Minimize the point cloud generated by S1 into a sub-point cloud;

[0013] The specific steps for generating the sub-point cloud include:

[0014] S21: Remove depth-unstable regions. Based on the noise model of the depth camera and the empirical values ​​obtained in the experiment, when the depth value is greater than a certain value, we set the depth value of a certain point in the depth image to 0.

[0015] S22: Since the point cloud obtained from the original depth of the RGBD camera may have multi-layered structures or holes, the point cloud obtained from S1 is smoothed by moving least squares.

[0016] S23: Use the K-Means clustering algorithm to cluster the point cloud to obtain K sub-point clouds.

[0017] S3: Set the previous few frames of the current frame as reference frames;

[0018] S4: The method of fusing gray-scale assisted nearest neighbor iteration with traditional nearest neighbor iteration is used to obtain the accurate transformation matrix, including: performing gray-scale nearest neighbor iteration on each sub-point cloud of S2 and the target point cloud of the reference frame to calculate the approximate transformation matrix and corresponding points, and then using traditional nearest neighbor iteration to register the point cloud and calculate the accurate transformation matrix.

[0019] The intensity-assisted nearest neighbor iterative registration method specifically includes:

[0020] S41: Predict the initial pose based on the uniform motion model of ORBSLAM2. Providing an initial pose can effectively prevent getting trapped in local optima and speed up the iteration process.

[0021]

[0022] In equation (2), T (·) Both represent T in the world coordinate system. pre T represents the increment of pose transformation between the previous two normal frames. last T represents the camera pose of the previous frame. ref The pose of the reference frame.

[0023] S42: Based on the initial pose Search sub-cloud The α nearest points to the target point cloud are selected as candidate corresponding points.

[0024] S43: Calculate candidate points Based on the degree of matching, the point with the highest score S(i,k) is selected as the corresponding point.

[0025]

[0026]

[0027]

[0028] △T represents the increment matrix for each iteration, r G (*) represents the geometric residual, r I (*) represents the intensity residual, and w is the weight transformation function.

[0029] S431: The weight transformation function of S43 is assumed in this paper to follow a t-distribution, and its weight transformation function can be expressed as:

[0030]

[0031]

[0032]

[0033] v represents the degrees of freedom of the distribution, μ (*) This represents the median of the geometric or strength residuals; similarly, σ (*) It is expressed as variance.

[0034] S44: The paired points are calculated using S4. The gain matrix is ​​then calculated using these paired points to minimize the overall Euclidean distance sum.

[0035]

[0036] S45: Update the transformation matrix T * ←△TT * .

[0037] S5: Transform the source point cloud based on the transformation matrix calculated in S4, and estimate the correlation between each sub-point cloud.

[0038] The correlation calculation includes:

[0039] S51: Find the i-th point The nearest point P tgt (j) Calculate its Euclidean distance as the primary indicator. In addition, add an intensity value as a penalty. The average score of all points is then used as the relevance score.

[0040]

[0041]

[0042] T represents the pose transformation matrix T of the source point cloud through the j-th sub-point cloud. Bj In the point cloud obtained later The i-th point, Point The intensity value.

[0043] S6: Calculate the threshold to divide all sub-point clouds into two groups: dynamic correlation group and static correlation group;

[0044] S61: Relevance score of the sub-point cloud calculated based on S5 {R(j)} j∈K A threshold ε is set to distinguish between dynamic and static sub-point clouds. The threshold can be determined by the following formula:

[0045]

[0046]

[0047]

[0048] S62: Thresholds can be used to effectively determine which point clouds belong to static point clouds.

[0049]

[0050] S represents the static sub-point cloud set, and D belongs to the dynamic sub-point cloud set. All point clouds in the dynamic sub-point cloud set will be projected onto the camera plane to generate a mask.

[0051] S7: Project the point cloud of the motion-related group onto the camera plane and draw a circle with it as the center. At the same time, fill it with morphological geometry to finally form the required pixel mask.

[0052] S8: Feature points within the mask will not be included in the camera pose estimation;

[0053] S9: The processed frame will be saved in the system as a reference frame and used as a candidate frame for updating the reference frame.

[0054] S10: Refine the mask generated by keyframes using historical observation data to generate more stable map points.

[0055] S101: When keyframes are generated, the previous N frames are used as historical observation data, not all historical data. The pixel weights are estimated using the following formula:

[0056]

[0057] I i (x,y) represents the pixel with coordinates (x,y) in the i-th data observation, and ρ is the pixel weight. A pixel with a weight exceeding λ times the total number of observations is considered a dynamic pixel.

[0058]

[0059] S102: Since feature points are easily generated at the edges of objects, but these edge points are extremely unstable, the mask generated from the refined keyframes will be appropriately dilated so that the edge points do not participate in the camera pose estimation process.

[0060] This invention can be used as a front-end preprocessing step in the ORBSLAM2 / 3 system to filter out dynamic regions, thereby reducing erroneous data associations. The entire system requires the RGB image and depth map of the current frame as input to generate point clouds. After processing one frame, it is temporarily stored in the system as a reference frame without further processing. K-Means clustering is used to divide the depth map into sub-point clouds, and then the sub-pose changes between the sub-point clouds and the reference frame are calculated sequentially. Simultaneously, this invention proposes an idea for the dynamic-static correlation of sub-point clouds. Combining this idea with sub-pose can effectively identify which sub-point clouds belong to dynamic point clouds, without relying on epipolar geometry constraints or needing prior knowledge of the camera's approximate pose. This invention effectively reduces the impact of dynamic objects on robot localization, improving the system's robustness and scene generalization.

[0061] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0062] This invention presents a dynamic environment RGB-D visual SLAM method based on point cloud region correlation. It solves the problem that traditional dynamic visual SLAM requires consideration of camera motion to discriminate dynamic regions. Furthermore, it achieves the goal of eliminating the need for prior knowledge of training samples and dynamic objects. Simultaneously, it improves the localization accuracy and robustness of visual SLAM in dynamic environments. Attached Figure Description

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

[0064] Figures 2a to 2b This is a schematic diagram illustrating the detection effect of the dynamic region as described in the embodiments of the present invention, wherein, Figure 2a It is the original image. Figure 2b This is a schematic diagram of the dynamic region detection results;

[0065] Figures 3a to 3b This is a schematic diagram comparing the trajectory of the present invention with the real trajectory and the trajectory of the ORB-SLAM2 algorithm on the TUM dataset. Figure 3a The camera is translating along the XYZ axes. Figure 3b The camera moves along a hemisphere with a radius of 1 meter. The blue trajectory is the positioning result trajectory of this invention, the dashed line is the actual camera's movement trajectory, and the green trajectory is the positioning result trajectory of ORB-SLAM2.

[0066] Figures 4a to 4b This is a schematic diagram illustrating the keyframe refinement effect as described in the disclosed embodiments of the present invention, wherein, Figure 4a It is a sequence of historical observation data frames. Figure 4b These are the original keyframes before refinement. Figure 4c These are refined keyframes;

[0067] Figures 5a to 5b This is a schematic diagram illustrating the dynamic region detection effect in a real-world scenario as described in the embodiments of the present invention. Figure 5a It is the original image. Figure 5b This is a schematic diagram of the dynamic region detection results. Detailed Implementation

[0068] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings. It should be understood that the described embodiments are only for explaining the invention and are not intended to limit the invention.

[0069] This invention presents a flowchart of a dynamic environment RGB-D visual SLAM method based on point cloud region correlation, as shown below. Figure 1 As shown, it includes the following steps.

[0070] S1: Perform two-dimensional planar downsampling on the RGB image and combine it with the depth map to generate a point cloud;

[0071] The specific steps for generating the point cloud are as follows:

[0072] S11: The grayscale image of the current frame is extracted by simulating the data acquisition method of rotating LiDAR, with an interval of 4 pixels per row and 4 pixels per column. The grayscale image is then combined with the depth map to generate a point cloud, thereby achieving downsampling on the two-dimensional plane to accelerate the construction of the KD tree.

[0073] S12: Rasterize the downsampled point cloud using grids with a side length of 0.05, and calculate the value of the i-th grid G. i The average grayscale value of all points within the grid is used as the intensity value of the grid centroid.

[0074]

[0075] Represents grid G i The intensity value, I(x,y) represents the gray value of the pixel with coordinates (x,y).

[0076] S2: Minimize the point cloud generated by S1 into a sub-point cloud;

[0077] The specific steps for generating the sub-point cloud are as follows:

[0078] S21: Remove depth-unstable regions. Based on the noise model of the depth camera and the empirical values ​​obtained in the experiment, when the depth value is greater than 6 meters, we set the depth value of a certain point in the depth image to 0.

[0079] S22: Since the point cloud obtained from the original depth of the RGBD camera may have multi-layered structures or holes, the point cloud obtained from S1 is smoothed by moving least squares.

[0080] S23: Use the K-Means clustering algorithm to cluster the point cloud to obtain K=10 sub-point clouds.

[0081] S3: Set the three frames preceding the current frame as reference frames;

[0082] S4: The method of fusing gray-level assisted nearest neighbor iteration with traditional nearest neighbor iteration is used to obtain the accurate transformation matrix, including: performing 5 gray-level nearest neighbor iterations between each sub-point cloud of S2 and the target point cloud of the reference frame to calculate the approximate transformation matrix and corresponding points, and then using traditional nearest neighbor iteration to perform 5 iterations to register the point cloud and calculate the accurate transformation matrix.

[0083] The intensity-assisted nearest neighbor iterative registration method specifically includes:

[0084] S41: Predict the initial pose based on the uniform motion model of ORBSLAM2. Providing an initial pose can effectively prevent getting trapped in local optima and speed up the iteration process.

[0085]

[0086] In equation (2), T (·) Both represent T in the world coordinate system. pre T represents the increment of pose transformation between the previous two normal frames. last T represents the camera pose of the previous frame. ref The pose of the reference frame.

[0087] S42: Based on the initial pose Search sub-cloud The 10 nearest points (α=10) to the target point cloud are selected as candidate corresponding points.

[0088] S43: Calculate candidate points Based on the degree of matching, the point with the highest score S(i,k) is selected as the corresponding point.

[0089]

[0090]

[0091]

[0092] △T represents the increment matrix for each iteration, r G (*) represents the geometric residual, r I (*) represents the intensity residual, and w is the weight transformation function.

[0093] S431: The weight transformation function of S43 is assumed in this paper to follow a t-distribution, and its weight transformation function can be expressed as:

[0094]

[0095]

[0096]

[0097] v represents the degrees of freedom of the distribution, which was set to 5 in the experiment. μ (*) This represents the median of the geometric or strength residuals; similarly, σ (*) It is expressed as variance.

[0098] S44: The paired points are calculated using S4. The gain matrix is ​​then calculated using these paired points to minimize the overall Euclidean distance sum.

[0099]

[0100] S45: Update the transformation matrix T * ←△TT * .

[0101] S5: Transform the source point cloud based on the transformation matrix calculated in S4, and estimate the correlation between each sub-point cloud.

[0102] The correlation calculation includes:

[0103] Find the i-th point The nearest point P tgt (j) Calculate its Euclidean distance as the primary indicator. In addition, add an intensity value as a penalty. The average score of all points is then used as the relevance score.

[0104]

[0105]

[0106] This represents the pose transformation matrix of the source point cloud through the j-th sub-point cloud. In the point cloud obtained later The i-th point, Point The intensity value.

[0107] S6: Calculate the threshold to divide all sub-point clouds into two groups: dynamic correlation group and static correlation group;

[0108] S61: Relevance score of the sub-point cloud calculated based on S5 {R(j)} j∈KA threshold ε is set to distinguish between dynamic and static sub-point clouds. The threshold can be determined by the following formula:

[0109]

[0110]

[0111]

[0112] S62: Thresholds can be used to effectively determine which point clouds belong to static point clouds.

[0113]

[0114] S represents the static sub-point cloud set, and D belongs to the dynamic sub-point cloud set. All point clouds in the dynamic sub-point cloud set will be projected onto the camera plane to generate a mask.

[0115] S7: Project the point cloud of the motion-related group onto the camera plane and draw a circle with it as the center. At the same time, fill it with morphological geometry to finally form the required pixel mask.

[0116] S8: Feature points within the mask will not be included in the camera pose estimation;

[0117] S9: The processed frame is saved in the system as a candidate frame for updating the reference frame;

[0118] S10: Refine the mask generated by keyframes using historical observation data to generate more stable map points.

[0119] S101: When keyframes are generated, the first N=7 frames are used as historical observation data, not all historical data. The pixel weights are estimated using the following formula:

[0120]

[0121] I i (x, y) represents the pixel with coordinates (x, y) in the i-th data observation, and ρ is the pixel weight. In the experiment, the weight ρ of the keyframe is 3, the weight ρ of the ordinary frame is 2, and N is set to 7. A pixel with a weight exceeding λ = 0.6 times the total number of observations is considered a dynamic pixel.

[0122]

[0123] S102: Since feature points are easily generated at the edges of objects, but these edge points are extremely unstable, the mask generated from the refined keyframes will be appropriately dilated so that the edge points do not participate in the camera pose estimation process.

[0124] Figure 2a and Figure 2b This invention effectively identifies dynamic regions of a pedestrian on the TUM dataset, with minimal impact from camera rotation. (Appendix) Figure 3a and Figure 3b This is a comparison chart of the absolute positioning accuracy of the present invention with the actual trajectory and the ORB-SLAM2 algorithm. The trajectory of the present invention closely follows the actual trajectory, while the trajectory of ORB-SLAM2 shows a serious deviation, proving that the present invention can improve the positioning accuracy and robustness of the SLAM system in dynamic environments. Figure 4a and Figure 4b This is a visualization of the refined keyframes. Figure 4b The original keyframes before refinement were processed Figure 4a It is obtained by filtering the historical observation data frame sequence. Figure 4c The refined keyframes. Because the mask area of ​​the refined keyframes is significantly reduced, the impact of false negatives can be effectively reduced, improving the robustness of the system. Figure 5a and Figure 5b It is a visualization of dynamic regions in a real-world environment, which fully demonstrates the reliability and practicality of the invention.

[0125] It should be emphasized that the embodiments described in this invention are illustrative rather than limiting. Therefore, this invention includes, but is not limited to, the embodiments described in the specific implementation schemes. Any other similar implementations derived by those skilled in the art based on the technical solutions of this invention also fall within the protection scope of this invention.

Claims

1. A dynamic environment RGB-D visual SLAM method based on point cloud region correlation, characterized in that, Includes the following steps: S1: Perform two-dimensional planar downsampling on the RGB image and combine it with the depth map to generate a point cloud; S2: Minimize the point cloud generated by S1 into a sub-point cloud; S3: Set the previous few frames of the current frame as reference frames; S4: An accurate transformation matrix is ​​obtained by fusing gray-level assisted nearest neighbor iteration with traditional nearest neighbor iteration. This includes: performing gray-level nearest neighbor iteration on each sub-point cloud of S2 and the target point cloud of the reference frame to calculate the approximate transformation matrix and corresponding points; then using traditional nearest neighbor iteration to register the point clouds and calculate the accurate transformation matrix. The gray-level assisted nearest neighbor iteration calculation of the transformation matrix includes: S41: Predict the initial pose based on the uniform motion model of ORBSLAM2. Providing an initial pose can effectively prevent getting trapped in local optima and speed up the iteration process. （2） In formula (2) All mean In the world coordinate system, This represents the increment of the pose transformation between the previous two normal frames. This indicates the camera pose of the previous frame. The pose of the reference frame; S42: Based on the initial pose Search sub-cloud The closest to the target point cloud The nearest points are selected as candidate corresponding points. ; S43: Calculate candidate points Based on the degree of matching, select the score. The highest one is the corresponding point : （3） （4） （5） This represents the increment matrix for each iteration. Represents geometric residuals, Indicates strength residual, This is the weight transformation function; S431: The weight transformation function of S43 is assumed in this paper to follow a t-distribution, and its weight transformation function can be expressed as: （6） （7） （8） Describing the degrees of freedom of the distribution. The median of the geometric or strength residuals, similarly. Represented as variance; S44: The paired points are calculated using S4. The gain matrix is ​​then calculated using these paired points to minimize the overall Euclidean distance sum. （9） S45: Update the transformation matrix ; S5: Transform the source point cloud based on the transformation matrix calculated in S4, and estimate the correlation between each sub-point cloud. S6: Calculate the threshold to divide all sub-point clouds into two groups: dynamic correlation group and static correlation group; S7: Project the point cloud of the motion correlation group onto the camera plane and draw a circle with it as the center. At the same time, fill it with morphological geometry to finally form the pixel mask we need. S8: Feature points within the mask will not be included in the camera pose estimation; S9: Save the processed frame in the system as a candidate frame for reference frame update; S10: Refine the mask generated by keyframes using historical observation data to generate more stable map points.

2. The dynamic environment RGB-D visual SLAM method based on point cloud region correlation as described in claim 1, characterized in that, Point cloud generation in step S1 includes: S11: The grayscale image of the current frame is downsampled on a two-dimensional plane to generate a point cloud in order to accelerate the construction of the KD tree by simulating the data acquisition method of rotating lidar. S12: The side length will be... The raster is used to rasterize the downsampled point cloud, and the first raster is calculated. grid The average grayscale value of all points within the grid is used as the intensity value of the grid centroid. （1） Represents grid The intensity value, This represents the grayscale value of a pixel with coordinates (x, y).

3. The dynamic environment RGB-D visual SLAM method based on point cloud region correlation as described in claim 1, characterized in that, The sub-point cloud generation in step S2 includes: S21: Remove depth-unstable regions. Based on the noise model of the depth camera and the empirical values ​​obtained in the experiment, when the depth value is greater than a certain value, set the depth value of a certain point in the depth image to 0. S22: Since the point cloud obtained from the original depth of the RGBD camera may have multi-layered structures or holes, the point cloud obtained from S1 is smoothed by moving least squares. S23: Use the K-Means clustering algorithm to cluster the point cloud to obtain K sub-point clouds.

4. The dynamic environment RGB-D visual SLAM method based on point cloud region correlation as described in claim 1, characterized in that, Step S5, calculating the correlation between sub-point clouds, includes: Find the i-th point The nearest point The Euclidean distance is calculated as the primary indicator. In addition, an intensity value is added as a penalty. The average score of all points is then used as the relevance score. （10） （11） This indicates that the source point cloud is obtained through the first... Pose transformation matrix of a point cloud In the point cloud obtained later The i-th point, Point The intensity value.

5. The dynamic environment RGB-D visual SLAM method based on point cloud region correlation as described in claim 1, characterized in that, Step S6 calculates the threshold for distinguishing between dynamic and static correlation groups, including: S61: Relevance score of sub-point cloud calculated based on S5 Set threshold The following formula can be used to determine the threshold for distinguishing between dynamic and static sub-point clouds: （12） （13） （14） S62: Determine which point clouds belong to static point clouds based on a threshold: （15） S represents a static sub-point cloud set, and D belongs to a dynamic sub-point cloud set. All point clouds belonging to set D will be projected onto the camera plane to generate a mask.

6. The dynamic environment RGB-D visual SLAM method based on point cloud region correlation as described in claim 1, characterized in that, In step S7, all point clouds in the motion-related group are projected onto the camera plane, and a circle is drawn with it as the center. Then, the circle is filled using morphological geometry to finally form the required pixel mask.

7. The dynamic environment RGB-D visual SLAM method based on point cloud region correlation as described in claim 1, characterized in that, Step S10 includes: S101: When keyframes are generated, the previous N frames are used as historical observation data, not all historical data. The pixel weights are estimated using the following formula: （16） Let (x, y) represent the pixel at coordinates (x, y) of the i-th data observation. The pixel weighting is the proportion of the total observations. A multiple is considered a dynamic pixel: （17） S102: Since feature points are easily generated at the edges of objects, but these edge points are unstable, the mask generated by the refined keyframes is dilated so that the edge points do not participate in the camera pose estimation process.

Images