A visual slam method combining sparse scene flow and weighted features for dynamic scenes

Abstract

The application discloses a visual SLAM method combining a dynamic scene with sparse scene flow and weighted features, and relates to the technical field of visual positioning. The method of the application uses an instance segmentation network SparseInst to acquire semantic information in an environment, identify potential moving objects and obtain image masks of the moving objects; the relative motion of feature points in a camera coordinate system is obtained by calculating sparse scene flow of the feature points and Mahalanobis distance of the feature points; detection and elimination of dynamic feature points are completed by using a chi-square test method; a static feature point is divided into a configuration weight, a weighted bundle adjustment method optimization objective function is established; and finally, subsequent pose optimization is completed by using the weighted feature points. Experimental results in a public data set and a real scene show that the method achieves good positioning accuracy in a dynamic scene.

Description

Technical Field

[0001] This invention relates to the field of visual positioning technology, specifically a visual SLAM method that combines dynamic scenes with sparse scene flow and weighted features. Background Technology

[0002] In an unknown environment, robots can receive visual sensor data to estimate pose with maximum a posteriori probability and build a map of the surrounding environment, creating conditions for autonomous navigation. Visual SLAM has attracted the attention of many researchers due to the rich texture information in the environment and the low cost of visual sensors.

[0003] However, most existing advanced visual SLAM systems are based on static scene assumptions and only use traditional geometric methods to combat interference from dynamic objects. Real-world scenes and camera motions are complex, and current visual SLAM systems for dynamic scenes typically use limited epipolar geometric constraints, resulting in poor performance in real-world scenarios.

[0004] Therefore, a new solution is needed to address the above problems. Summary of the Invention

[0005] The purpose of this invention is to provide a visual SLAM method that combines sparse scene flow and weighted features for dynamic scenes. It uses the SparseInst instance segmentation network to acquire semantic information from the environment, identify potential moving objects, and obtain their image masks. By calculating the sparse scene flow and Mahalanobis distance of feature points, the relative motion of feature points in the camera coordinate system is obtained. The chi-square test is used to detect and remove dynamic feature points. Confidence weights are assigned to static feature points, and a weighted bundle adjustment method is established to optimize the objective function. Finally, the weighted feature points are used to complete subsequent pose optimization. Experimental results on public datasets and real-world scenes demonstrate that this method achieves good localization accuracy in dynamic scenes.

[0006] To achieve the above objectives, the present invention provides the following technical solution: a visual SLAM method combining dynamic scenes with sparse scene flow and weighted features, comprising at least the following steps:

[0007] S1: For images acquired by an RGB-D camera, the SparseInst network model is used for instance segmentation to obtain the mask images of objects.

[0008] S2: Extract and match feature points from adjacent frames to calculate the sparse scene flow and its Mahalanobis distance, and obtain the relative motion of feature points in the camera coordinate system.

[0009] S3: Use the chi-square test method to detect and remove dynamic feature points.

[0010] S4: Assign weights to static feature points and establish a weighted bundle adjustment method to optimize the objective function.

[0011] S5: Use weighted feature points to complete subsequent pose optimization.

[0012] Preferably, S1 includes at least the following steps:

[0013] The SparseInst model uses ResNet-50 as its backbone network.

[0014] Train the SparseInst network model on the COCO dataset;

[0015] The ONNXRUNTIME framework is used for acceleration and real-time inference to obtain the mask image and label of the object.

[0016] Preferably, step S2 includes at least the following steps:

[0017] ORB feature points are extracted from two adjacent frames, and LK optical flow method is used for feature point matching.

[0018] Calculate the scene flow and the Mahalanobis distance between the feature points on the static background and the object, respectively;

[0019] Let the depth be d, the camera intrinsic parameter matrix be K, and the spatial coordinates of a map point can be written as: P i =[X i Y i Z i ] T =d i K -1 [u i v i 1] T The scene flow computation can be written as M = P' i -P i ;

[0020] The Mahalanobis distance mentioned above is the Mahalanobis distance of each scene flow relative to the mean of the scene flows on the static background. Its calculation process is written as follows: Since the scene flow solution only depends on the local grayscale of the feature points, and the local grayscale is only related to the location of the feature points, the covariance matrix is:

[0021]

[0022] Where s represents the scaling down of the image pyramid, and p represents the standard deviation of the bottom layer of the pyramid;

[0023] The chi-square test method is used to detect and remove dynamic feature points.

[0024] Preferably, the chi-square test method sets a threshold of 7.81;

[0025] The dynamic feature point judgment condition of the chi-square test is:

[0026] Preferably, the assignment of confidence weights to static feature points in S3 includes calculating the confidence level of static feature points through a chi-square distribution and adding weight terms to the original optimization objective function.

[0027] Preferably, the pose optimization in S5 is performed using the Levenberg-Quarkmat method.

[0028] Compared with the prior art, the beneficial effects of the present invention are:

[0029] This invention, based on traditional visual SLAM algorithms, first considers the influence of dynamic objects and introduces an instance segmentation network model. It also proposes a novel geometric method to identify the motion state of feature points, a weighted pose optimization method, and a visual SLAM method combining dynamic scenes with sparse scene flow and weighted features. Specifically, the introduced instance segmentation enhances the robot's environmental perception capability; the constructed new geometric constraint method eliminates dynamic features, avoiding the limitations of traditional epipolar constraints and improving the localization accuracy in dynamic scenes; and the proposed pose optimization method based on weighted features further improves localization accuracy. Attached Figure Description

[0030] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0031] Figure 1 This invention discloses a visual SLAM framework that combines dynamic scenes with sparse scene flow and weighted features.

[0032] Figure 2 This invention discloses a visual SLAM flowchart that combines dynamic scene flow and weighted features.

[0033] Figure 3 This is a schematic diagram of the scene flow;

[0034] Figure 4 The final feature point extraction diagram;

[0035] Figure 5 This is a schematic diagram of the trajectory obtained after pose optimization. Detailed Implementation

[0036] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments.

[0037] Please see Figures 1-5 A visual SLAM method that combines dynamic scenes with sparse scene flow and weighted features;

[0038] This SLAM system is based on ORB-SLAM2, maintaining the original local mapping and loop closure threads, while improving the tracking thread and adding a parallel instance segmentation thread. The improved visual SLAM system of this invention is as follows: Figure 1 As shown, after receiving an RGB-D image input, the system obtains semantic information through the instance segmentation thread and then sends the mask to the tracking thread. In the tracking thread, ORB feature points are first extracted from the RGB image. Combined with the semantic information obtained from instance segmentation, the scene flow and its Mahalanobis distance on the static background are calculated, and dynamic feature points are detected and removed using the chi-square test. Finally, weights are assigned to the static features, and the feature points are then fed into the joint optimization module for weights and poses to complete the entire tracking process.

[0039] like Figure 2 Here is a flowchart of the present invention, which includes the following steps:

[0040] S1: For images acquired by an RGB-D camera, the SparseInst network model is used for instance segmentation to obtain the mask images of objects.

[0041] S2: Extract and match feature points from adjacent frames to calculate the sparse scene flow and its Mahalanobis distance, and obtain the relative motion of feature points in the camera coordinate system.

[0042] S3: Use the chi-square test method to detect and remove dynamic feature points.

[0043] S4: Assign weights to static feature points and establish a weighted bundle adjustment method to optimize the objective function.

[0044] S5: Use weighted feature points to complete subsequent pose optimization.

[0045] In specific implementation, step S1 includes the following specific steps:

[0046] A SparseInst network model with ResNet-50 as the backbone is constructed. A single-stage approach is adopted, in which the backbone network extracts image features, then the encoder enhances the multi-scale representation of the single-level features, and finally the decoder calculates the activation mapping of the instance and performs recognition and segmentation.

[0047] The inference framework is deployed in a C++ environment to achieve real-time instance segmentation.

[0048] In specific implementation, step S2 includes the following specific steps:

[0049] ORB feature points are extracted from two adjacent frames.

[0050] The FAST (features from accelerated segment test) algorithm is used to detect feature points, and an image pyramid is employed to address the scale consistency problem. In a small image patch B, the moments of the image patch are defined as follows:

[0051]

[0052] Find the centroid of the image patch using moments. Connecting the geometric center and centroid of the image patch yields a direction vector, i.e., the direction of the feature points θ = arctan(m 10 ,m 01 );

[0053] Feature point matching is performed using the LK optical flow method, and the matching point pairs are obtained by minimizing the gray-level difference within the window.

[0054] Let the window's dimensions be w. x and w y Let the displacements of the two 2-D matching features on the imaging plane be Δu and Δv, and I(u,v) represent the gray values ​​at the corresponding positions. Then, the objective function can be written as:

[0055]

[0056] Calculate the scene flow and the Mahalanobis distance between the feature points on the static background and the object, respectively.

[0057] Let the depth be d, the camera intrinsic parameter matrix be K, and the spatial coordinates of a map point can be written as: P i =[X i Y i Z i ] T =d i K -1 [u i v i 1] T The scene flow computation can be written as M = P' i -P i Its schematic diagram is as follows Figure 3 As shown.

[0058] The Mahalanobis distance mentioned above is the Mahalanobis distance of each scene flow relative to the mean of the scene flows on the static background. Its calculation process is written as follows:

[0059]

[0060] Since the scene flow solution only depends on the local grayscale of feature points, and the local grayscale is only related to the location of feature points, the covariance matrix is:

[0061]

[0062] Where s represents the scaling factor of the image pyramid, and p represents the standard deviation of the bottom layer of the pyramid. In practical applications, s should be set to 1.2 and p to 1.

[0063] In specific implementation, step S3 includes the following specific steps:

[0064] The squared Mahalanobis distance of the feature points follows a chi-square distribution with 3 degrees of freedom. Based on this, the value of r calculated in step 2 is used to calculate the value of r. 2 Its value can be calculated;

[0065] The chi-square test method sets a threshold of 7.81. The squared Mahalanobis distance of dynamic features will exceed the threshold, while that of static features will not. Therefore, the judgment condition for dynamic features can be written as:

[0066]

[0067] The final static feature points are as follows Figure 3 As shown.

[0068] In specific implementation, step S4 includes the following specific steps:

[0069] To obtain the confidence score of each feature point, a chi-square distribution is used. The weight of each feature point is calculated using the squared Mahalanobis distance of the scene flow, denoted as w. i chsq(a,b) represents the probability value corresponding to error a under a chi-square distribution with b degrees of freedom. Feature weight calculation: w i =chsq(r 2 ,3).

[0070] The original ORB-SLAM2 did not consider the confidence level of feature points, treating all feature points equally in the optimization. Let the rotation and translation be R and t, the Huber function be ρ(·), and the projection function be π(·). Its optimization objective function is:

[0071]

[0072] The weighted bundle adjustment objective function for this SLAM system is as follows:

[0073]

[0074] In specific implementation, step S5 includes the following specific steps:

[0075] Use the g2o library to construct error vertices and error edges, and then construct the terms to be optimized.

[0076] The Levenberg-Marquardt method is used for joint optimization of pose and weights, and the Jacobian matrix corresponding to the weight and pose updates during the optimization process is constructed.

[0077] Let the error term be e, and the Jacobian matrix of the weights be written as:

[0078] Where f x f y c X c y bf is the camera's intrinsic parameter.

[0079] Jacobian matrix of pose: J represents the pose Jacobian matrix of ORB-SLAM2. The result of pose optimization is as follows: Figure 4 As shown.

[0080] The methods and pose optimization results described above demonstrate that the method disclosed in this invention enables the camera to achieve relatively good positioning accuracy in dynamic scenes, effectively completing the positioning task. This proves the effectiveness of this invention in dynamic scenes.

[0081] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other specific forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered in all respects as exemplary and non-limiting, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

Claims

1. A visual SLAM method combining dynamic scenes with sparse scene flow and weighted features, characterized in that: At least the following steps are included: S1: For images acquired by an RGB-D camera, the SparseInst network model is used for instance segmentation to obtain the mask images of objects; S2: Extract and match feature points from adjacent frames to calculate the sparse scene flow and its Mahalanobis distance, and obtain the relative motion of feature points in the camera coordinate system. S2 includes: ORB feature points are extracted from two adjacent frames, and LK optical flow method is used for feature point matching. Calculate the scene flow and the Mahalanobis distance between the feature points on the static background and the object, respectively; Let the depth be d, the camera intrinsic parameter matrix be K, and the spatial coordinates of the map points be written as: The scene stream computation writing ; The Mahalanobis distance mentioned above is the Mahalanobis distance of each scene flow relative to the mean of the scene flows on the static background. Its calculation process is written as follows: Since the scene flow solution only depends on the local grayscale of the feature points, and the local grayscale is only related to the position of the feature points, the covariance matrix is: Where s represents the scaling down of the image pyramid, and p represents the standard deviation of the bottom layer of the pyramid; S3: Use the chi-square test method to detect and eliminate dynamic feature points; The chi-square test method is set with a threshold of 7.81; The dynamic feature point judgment condition of the chi-square test is: ; S4: Assign weights to static feature points and establish a weighted bundle adjustment method to optimize the objective function; S4 includes the following specific steps: The confidence level of each feature point is obtained by calculating the chi-square distribution. The weights of the feature points are calculated using the squared Mahalanobis distance of the scene flow, and denoted as weights. , Let represent the probability value corresponding to error a under a chi-square distribution with b degrees of freedom; Feature weight calculation: ; The objective function for the weighted bundle adjustment method is established as follows: in, For rotation, For translation, the Huber function is: The projection function is ; S5: Use weighted feature points to complete subsequent pose optimization.

2. The visual SLAM method combining dynamic scene flow and weighted features according to claim 1, characterized in that: S1 includes at least the following steps: The SparseInst model uses ResNet-50 as its backbone network. Train the SparseInst network model on the COCO dataset; The ONNXRUNTIME framework is used for acceleration and real-time inference to obtain the mask image and label of the object.

3. The visual SLAM method combining dynamic scene flow and weighted features according to claim 1, characterized in that: The pose optimization in S5 is performed using the Levenberg-Quarkmat method.

Images