Visual SLAM (Simultaneous Localization and Mapping) method for dynamic indoor environment

Abstract

The invention discloses a dynamic indoor environment-oriented visual SLAM (Simultaneous Localization and Mapping) method, which belongs to the technical field of computer vision and deep learning, and is technically characterized by comprising the following steps of: 1, inputting a depth image into the front end of a system to extract feature points; 2, introducing a CBAM attention mechanism into a C3 module in a Backbone and Neck network of a YOLOv5 model, modifying an EIOU loss function, and performing dynamic point elimination on the image after the feature points are detected in the step 1 by using the improved YOLOv5; 3, performing re-judgment verification by adopting a multi-view geometry and K-Means combined method, and determining the motion state again; 4, performing feature point matching on the determined static feature points, and then performing initial pose estimation, loopback detection and global optimization operation to obtain a final global pose result; and 5, calculating the similarity according to the TF-IDF, and selecting a drawing key frame. The method has the advantages that the adverse effect of a moving object on a visual SLAM system in a complex environment is reduced, and the precision and robustness of positioning and mapping are improved.

Description

Technical Field

[0001] The present invention relates to the field of computer vision and deep learning technology, and in particular to a visual SLAM method for dynamic indoor environments. Background Art

[0002] The specific meaning of Simultaneous Localization and Mapping (SLAM) is that a moving subject, equipped with specific sensors, estimates its own motion while building an environmental model in the absence of prior information about the environment (i.e., in an unknown environment). Visual SLAM using visual sensors is attracting many researchers and engineers to become a research hotspot because of its low cost, lightweight, and ability to collect relatively rich image information. However, in complex indoor scenes (such as those containing moving people), dynamic objects will have a significant adverse effect on the correct posture solution of the system, making it impossible to build an accurate environmental map. In addition, the sparse maps constructed by traditional visual SLAM cannot meet the requirements of advanced tasks such as obstacle avoidance and path planning.

[0003] At present, the basic design concept of dynamic scene SLAM system is to filter out the information introduced by moving objects as outliers. Based on this design concept, it is divided into methods based on pure geometric constraints and methods based on deep learning models.

[0004] In the pure geometry-based method, the geometric constraint relationship between image frames is used to judge the motion state of the object and remove dynamic objects. However, this method is greatly affected by the uncertainty of image depth, and relies on predefined thresholds to judge the similarity and difference between consecutive frames, which can easily lead to erroneous removal. Compared with pure geometry methods, the use of depth methods can usually better filter out dynamic information. However, relying solely on the target detection network is too dependent on semantic information and can only detect predefined moving objects, which is not enough to cope with complex actual scenes. In general, the current visual SLAM methods for dealing with dynamic scenes are relatively limited, and there are still cases where dynamic object detection is not accurate or is mistakenly removed. Summary of the invention

[0005] In view of the deficiencies in the prior art, an object of an embodiment of the present invention is to provide a visual SLAM method for a dynamic indoor environment to solve the problems in the above-mentioned background technology.

[0006] To achieve the above object, the present invention provides the following technical solutions:

[0007] A visual SLAM method for dynamic indoor environments comprises the following steps:

[0008] Step 1: Input the depth image into the system front end to extract feature points;

[0009] Step 2: Introduce the CBAM attention mechanism into the C3 module in the Backbone and Neck networks of the YOLOv5 model, modify the EIOU loss function, and use the improved YOLOv5 to remove dynamic points from the image after the feature points are detected in step 1;

[0010] Step 3: Use the method combining multi-view geometry and K-Means to re-judge and verify and re-determine its motion state;

[0011] Step 4: Perform feature point matching on the determined static feature points, and then perform initial pose estimation, loop closure detection and global optimization operations to obtain the final global pose result;

[0012] Step 5: Calculate the similarity based on TF-IDF, select the drawing keyframes, project the feature point pixels in each drawing keyframe into a three-dimensional point cloud, and perform splicing and filtering to construct a dense static point cloud map, and convert it into the corresponding octree and raster map.

[0013] As a further solution of the present invention, in the step 1, robustness and extraction efficiency are comprehensively considered. The present invention adopts ORB as the front-end feature point. The ORB algorithm consists of two parts: detecting FAST corner points and calculating BRIEF descriptors.

[0014] The FAST algorithm selects a total of 16 pixels on a circle with a radius of 3 with pixel p as the center. If the grayscale difference between 9 consecutive pixels among the 16 pixels and p is greater than the set threshold T, p is judged to be a FAST corner point, which can be expressed by the calculation formula:

[0015]

[0016] Where n is the total number of pixels on the circumference of point p, I(x) is the grayscale value of any pixel on the circumference, I(p) is the grayscale value of the pixel to be detected, and T is the corner detection threshold;

[0017] BRIEF is a binary descriptor, which is a description vector composed of binary strings represented by 0 and 1. For the grayscale values ​​of two pixels (x and y) in the neighborhood of the feature point, if x is greater than y, it takes 1, otherwise it takes 0.

[0018] As a further solution of the present invention, the CBAM attention mechanism is introduced into the C3 module in step 2. First, the spatial information of the input feature map is aggregated by using maximum pooling and average pooling operations to generate two different spatial context descriptors: and They are then forwarded to a shared network consisting of MLPs. After each descriptor has been applied to the shared network, the output feature vectors are merged using element-wise summation to finally produce a channel attention map.

[0019] In the spatial attention module, average pooling and max pooling operations are first applied along the channel axis to generate two 2D maps: and Represent the average pooling features and maximum pooling features in the channel respectively, connect them to generate a valid feature descriptor, and then use the convolution layer to generate the spatial attention map

[0020] As a further solution of the present invention, in step 3, the feature points are re-judged using a multi-view geometric constraint method to obtain the true motion conditions of all image pixels;

[0021] The K-Means clustering algorithm is used to divide the 3D point cloud into k clusters. The number of pixels in the image is divided by 2000 as the value of k. If it is less than 2000, a new cluster is automatically formed. 200 pixels are randomly selected in each cluster, and the motion characteristics of each selected point are determined using the multi-view geometric constraint method. Then, the overall motion state of the cluster is determined based on the majority motion characteristics of the selected points.

[0022] As a further solution of the present invention, in the step 4, the static feature points after the semantic and geometric information is eliminated in step 2 and step 3 are matched between frames, and then the PNP and ICP methods are used to estimate the pose, and then the BA graph optimization method is used to adjust the camera's pose information and the spatial position of the image feature points, so that the light emitted by the feature points converges to the optical center of the camera, and the camera pose and spatial landmark points are used as target optimization variables. The reprojection error of the spatial points is used to construct the cost function, and then the incremental equation of the cost function is solved with the help of the Schur elimination method. The reprojection error can be expressed as:

[0023]

[0024] Among them, T i is the current frame pose, P j is the coordinate of the landmark point, z ij In position T i Observe the landmark point P j The observation data generated.

[0025] As a further solution of the present invention, in step five, a drawing key frame selection strategy is first adopted to establish a key frame information library based on the information of the mapping points observed by the key frame. When a new key frame is inserted, the similarity between the key frame and the information library is calculated based on the BOW model. The higher the similarity, the more redundant information the current frame has, and vice versa.

[0026] As a further solution of the present invention, the i-th key frame similarity s i The calculation uses TF-IDF,

[0027] s i =TF i *IDF i

[0028]

[0029] TF i Indicates the frequency of a feature vector appearing in the current key frame image, IDF i Indicates the frequency of a feature vector in the information database, m i is the number of a certain feature vector in the current key frame, n i is the total number of feature vectors in the current key frame image, N i is the number of a certain feature vector in the information database, and N is the total number of feature vectors in the information database.

[0030] In summary, compared with the prior art, the embodiments of the present invention have the following beneficial effects:

[0031] By introducing the CBAM attention mechanism and modifying the loss function in the C3 module of the YOLOv5 algorithm, the detection accuracy and speed of YOLOv5 target detection (especially for small local targets) are improved;

[0032] A new dynamic visual SLAM method is proposed. On the one hand, it uses the improved YOLOv5 to find potential dynamic points, and on the other hand, it uses the multi-view geometric constraint method accelerated by K-Means for re-judgment, so that the visual SLAM can maintain good positioning accuracy and robustness in dynamic indoor scenes.

[0033] Compared with traditional visual SLAM that can only construct sparse point cloud maps, the present invention successfully constructs a clearer static point cloud map and simultaneously generates the corresponding octree and two-dimensional grid map, which can be directly used for advanced tasks such as navigation and obstacle avoidance.

[0034] In order to more clearly illustrate the structural features and effects of the present invention, the present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 It is a structural schematic diagram of an embodiment of the invention.

[0036] Figure 2 This is a structural diagram of the C3 module before improvement in the embodiment of the invention.

[0037] Figure 3 This is a flowchart of the YOLOv5 algorithm elimination in an embodiment of the invention.

[0038] Figure 4 This is a K-Means clustering algorithm elimination flow chart in an embodiment of the invention.

[0039] Figure 5 The following are the dynamic point removal effect diagrams of various algorithms in different scenarios in the embodiments of the invention.

[0040] Figure 6 It is a dynamic object class sequence diagram in an embodiment of the invention.

[0041] Figure 7 The point cloud map, static point cloud map, octree and grid map constructed without eliminating dynamic points in the embodiments of the invention. DETAILED DESCRIPTION

[0042] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0043] The specific implementation of the present invention is described in detail below in conjunction with specific embodiments.

[0044] In one embodiment, a visual SLAM method for dynamic indoor environments is described. Figure 1 to Figure 7 , including the following steps:

[0045] Step 1: Input the depth image into the system front end to extract feature points;

[0046] Step 2: Introduce the CBAM attention mechanism into the C3 module in the Backbone and Neck networks of the YOLOv5 model, modify the EIOU loss function, and use the improved YOLOv5 to remove dynamic points from the image after the feature points are detected in step 1;

[0047] Step 3: Use the method combining multi-view geometry and K-Means to re-judge and verify and re-determine its motion state;

[0048] Step 4: Perform feature point matching on the determined static feature points, and then perform initial pose estimation, loop closure detection and global optimization operations to obtain the final global pose result;

[0049] Step 5: Calculate the similarity based on TF-IDF, select the drawing keyframes, project the feature point pixels in each drawing keyframe into a three-dimensional point cloud, and perform splicing and filtering to construct a dense static point cloud map, and convert it into the corresponding octree and raster map.

[0050] For further information, see Figure 1 to Figure 7 In the step 1, considering the robustness and extraction efficiency comprehensively, the present invention adopts ORB as the front-end feature point, and the ORB algorithm consists of two parts: detecting FAST corner points and calculating BRIEF descriptors;

[0051] The FAST algorithm selects a total of 16 pixels on a circle with a radius of 3 with pixel p as the center. If the grayscale difference between 9 consecutive pixels among the 16 pixels and p is greater than the set threshold T, p is judged to be a FAST corner point, which can be expressed by the calculation formula:

[0052]

[0053] Where n is the total number of pixels on the circumference of point p, I(x) is the grayscale value of any pixel on the circumference, I(p) is the grayscale value of the pixel to be detected, and T is the corner detection threshold;

[0054] BRIEF is a binary descriptor, which is a description vector composed of binary strings represented by 0 and 1. For the grayscale values ​​of two pixels (x and y) in the neighborhood of the feature point, if x is greater than y, it takes 1, otherwise it takes 0.

[0055] For further information, see Figure 1 to Figure 7 In step 2, the CBAM attention mechanism is introduced into the C3 module. First, the spatial information of the input feature map is aggregated by using the maximum pooling and average pooling operations to generate two different spatial context descriptors. and They are then forwarded to a shared network consisting of MLPs. After each descriptor has been applied to the shared network, the output feature vectors are merged using element-wise summation to finally produce a channel attention map.

[0056] In the spatial attention module, average pooling and max pooling operations are first applied along the channel axis to generate two 2D maps: and Represent the average pooling features and maximum pooling features in the channel respectively, connect them to generate a valid feature descriptor, and then use the convolution layer to generate the spatial attention map

[0057] For further information, see Figure 1 to Figure 7In the step 3, the feature points are re-judged using the multi-view geometric constraint method to obtain the true motion of all image pixels;

[0058] The K-Means clustering algorithm is used to divide the 3D point cloud into k clusters. The number of pixels in the image is divided by 2000 as the value of k. If it is less than 2000, a new cluster is automatically formed. 200 pixels are randomly selected in each cluster, and the motion characteristics of each selected point are determined using the multi-view geometric constraint method. Then, the overall motion state of the cluster is determined based on the majority motion characteristics of the selected points.

[0059] For further information, see Figure 1 to Figure 7 In the step 4, the static feature points after the semantic and geometric information is eliminated in step 2 and step 3 are matched between frames, and then the PNP and ICP methods are used to estimate the pose. Then, the BA graph optimization method is used to adjust the camera pose information and the spatial position of the image feature points, so that the light emitted by the feature points converges to the optical center of the camera. The camera pose and the spatial landmark points are used as target optimization variables, and the reprojection error of the spatial points is used to construct the cost function. Then, the incremental equation of the cost function is solved with the help of the Schur elimination method. The reprojection error can be expressed as:

[0060]

[0061] Among them, T i is the current frame pose, P j is the coordinate of the landmark point, z ij In position T i Observe the landmark point P j The observation data generated.

[0062] For further information, see Figure 1 to Figure 7 In the step 5, a drawing key frame selection strategy is firstly adopted to establish a key frame information base according to the information of the mapping points observed by the key frame. When a new key frame is inserted, the similarity between the key frame and the information base is calculated according to the BOW model. The higher the similarity, the more redundant information the current frame has, and vice versa.

[0063] For further information, see Figure 1 to Figure 7 , the i-th key frame similarity s i The calculation uses TF-IDF,

[0064]

[0065] TF i Indicates the frequency of a feature vector appearing in the current key frame image, IDF i Indicates the frequency of a feature vector in the information database, m iis the number of a certain feature vector in the current key frame, n i is the total number of feature vectors in the current key frame image, N i is the number of a certain feature vector in the information database, and N is the total number of feature vectors in the information database.

[0066] In this embodiment, if Figure 1 As shown, the present invention provides a visual SLAM method for dynamic indoor environments. The specific steps are as follows:

[0067] Input the depth image into the system front end to extract feature points.

[0068] The CBAM attention mechanism is introduced into the C3 module in the Backbone and Neck networks of the YOLOv5 model, and the EIOU loss function is modified. The improved YOLOv5 removes dynamic points from the image after the feature points are detected in step (1).

[0069] The method combining multi-view geometry and K-Means is used to re-judge and verify and re-determine its motion state.

[0070] Feature point matching is performed for the determined static feature points, and then a series of operations such as initial pose estimation, loop detection, and global optimization are performed to obtain the final global pose result.

[0071] The similarity is calculated based on TF-IDF, and the key frames of the drawing are selected. The feature point pixels in each key frame of the drawing are projected into a three-dimensional point cloud and stitched and filtered to construct a dense static point cloud map, which is then converted into the corresponding octree and grid map.

[0072] Furthermore, in step (1), if Figure 1 As shown, when the visual SLAM system starts running, the system obtains raw data through the RGB-D camera, including RGB images and corresponding depth map information. Taking into account the robustness and extraction efficiency, the present invention adopts ORB as the front-end feature point. The ORB algorithm consists of two parts: detecting FAST corner points and calculating BRIEF descriptors. The FAST algorithm selects a total of 16 pixels on a circle with a radius of 3 with pixel p as the center. If the grayscale difference between 9 consecutive pixels of these 16 pixels and p is greater than the set threshold T, p is judged to be a corner point of FAST. The calculation formula can be expressed as:

[0073]

[0074] Where n is the total number of pixels on the circumference of point p, I(x) is the grayscale value of any pixel on the circumference, I(p) is the grayscale value of the pixel to be detected, and T is the corner detection threshold.

[0075] BRIEF is a binary descriptor, which is a description vector composed of binary strings represented by 0 and 1. For the grayscale values ​​of two pixels (x and y) in the neighborhood of the feature point, if x is greater than y, it takes 1, otherwise it takes 0.

[0076] To ensure that the algorithm can stably extract feature points in images at different distances, the original image is used as the bottom layer, and the image pyramid is constructed by scaling up layer by layer at a fixed factor (the scaling factor is generally taken as 1.2 based on engineering experience). In this way, images of different resolutions are obtained. The higher the number of pyramid layers, the farther the camera is from the object. The image size of the i-th layer can be expressed as:

[0077]

[0078] In order to make the BRIEF descriptor have angle information, the geometric center of the image block and the centroid are connected to obtain the direction vector to meet the rotation invariance. The so-called centroid refers to the center with the gray value of the image block as the weight. When calculating the descriptor later, the pixels around the target feature point can be rotated to the main direction for calculation. The rotation angle can be defined as:

[0079]

[0080] Where m 01 With m 10 is the image moment along the two coordinate axes X and Y:

[0081]

[0082] Furthermore, in step (2), since the C3 module in the Backbone and Neck networks plays an important role in the entire YOLOv5 network, the structure of the C3 module before improvement is as follows: Figure 2 (a) As shown in the figure. The present invention introduces the CBAM attention mechanism into the C3 module. CBAM uses the attention mechanism on the channel axis and the spatial axis to emphasize the two-dimensional meaningful features. The two attention modules, channel and space, calculate complementary attention and focus on "what" and "where" respectively. The improved C3 module structure is shown in the figure. Figure 2 (b) In the channel attention module, the spatial information of the input feature map is first aggregated by using the maximum pooling and average pooling operations to generate two different spatial context descriptors. and The representation ability of the network is greatly improved. They are then forwarded to a shared network consisting of MLP (Multilayer Perceptron). After each descriptor is applied to the shared network, the output feature vectors are merged using element-wise summation to finally produce a channel attention map. This spatial attention map encodes the locations of emphasis or suppression. In the spatial attention module, average pooling and maximum pooling operations are first applied along the channel axis to generate two 2D maps: and Represent the average pooling features and maximum pooling features in the channel respectively. They are connected to generate effective feature descriptors, and then a convolutional layer is used to generate a spatial attention map. The spatial attention map encodes the locations that are emphasized or suppressed. The specific calculation formula is:

[0083]

[0084] M c (F r )=F rc =σ(MLP(Avgpool(F r ))+MLP(Maxpool(F r )))

[0085] M s (F rc )=σ(f 7×7 ([Avgpool(F rc );Maxpool(F rc )]))

[0086] Here F r represents the input feature, F r ' is the output feature, M c and M s They represent the channel attention map and the spatial attention map respectively, σ represents the Sigmoid function, and F rc Represents the input feature F r The output of the channel attention map, MLP is a shared network composed of multi-layer perceptrons, f 7×7 Represents a convolution operation with a filter size of 7×7.

[0087] In order to make YOLOv5 better complete BBR (Bounding Box Regression) and speed up the convergence, a more effective loss function is proposed based on the CIOU loss function, which divides the loss function into three parts: IOU loss L IOU , distance loss L dis and aspect ratio loss L asp . Its definition is as follows.

[0088]

[0089] Among them, w c and h care the width and height of the smallest enclosed box in the target box and the predicted box, respectively.

[0090] The improved YOLOv5 algorithm is used to remove dynamic points, and the feature points falling into the Bounding-Box area are marked as potential dynamic points. The specific removal process is as follows: Figure 3 shown.

[0091] Furthermore, in step (3), the previous method using semantic information may miss the detection of some dynamic targets due to image blur, incomplete targets and possible movement of undefined objects. Moreover, when the detection frame of the improved YOLOv5 occupies too large a proportion of the screen, some static pixels that may have existed locally are also excessively eliminated, which easily causes the loss of feature points and leads to system tracking failure. Therefore, the multi-view geometric constraint method is further used to re-judge the feature points to obtain the true motion of all image pixels.

[0092] The geometric constraints are independent of the specific objects in the picture and are only related to the camera internal parameters and the relative pose between frames. i and the next frame observation point c j , p i and p j are the corresponding pixels in the two images, p w and p' w are their projection points in the 3D real world. Using the camera pose T of the current frame i from the depth image i w The information of its depth value z can obtain the coordinates p of the projection point of the current frame in the three-dimensional world w .

[0093] p w =T i w π -1 (p i ,z)

[0094] Here π -1 represents the reverse perspective projection function, which depends on the intrinsic parameter type of the camera. If the current 3D world coordinate point p is w is a static point, then the image pixel point p of the next frame j There is a corresponding projection transformation relationship with it.

[0095]

[0096] Here π represents the perspective projection function, which depends on the intrinsic parameter type of the camera. Indicates the camera pose of the next frame j in the tracking thread. Combined with the depth value information z' of the jth frame, we can get the pixel p jThe actual coordinates of the projection in the three-dimensional world.

[0097]

[0098] The distance between the 3D world projection coordinates of the target pixel point in the i-th frame and the j-th frame is recorded as d. If the target pixel point is a static point, the 3D world projection coordinates corresponding to the i-th frame and the j-th frame should be very close or even overlap. In this case, d is a relatively small value. If the distance d between the two coordinates is large, the point is a dynamic point. In order to more specifically and accurately determine the dynamic and static characteristics of the pixel point, the distance threshold d is introduced. th A dynamic evaluation function is established with the depth value z.

[0099] d err =|d th +kz-d|

[0100] where d th According to engineering experience, it is generally set to 2, and k is the proportional coefficient that changes with the depth value, which is generally 0.025. err If the value is less than 0.1, the current pixel is considered a static point, otherwise it is a dynamic point.

[0101] In addition, in order to further accelerate the distinction between dynamic and static pixels and reduce calculation and running time, the present invention uses the K-Means clustering algorithm to detect dynamic pixels. The K-Means clustering algorithm is used to divide the three-dimensional point cloud into k clusters, and the result of dividing the number of pixels in the image by 2000 is used as the value of k. If it is less than 2000, a new cluster is automatically formed. 200 pixels are randomly selected in each cluster, and the motion characteristics of each selected point are determined using the multi-view geometric constraint method.

[0102] Then the overall motion state of the cluster is determined based on the majority motion characteristics of the selected points.

[0103]

[0104] cluster i Indicates the motion state of the pixel in the i-th cluster, num dynamic and num static Respectively represent the number of dynamic points and static points in the points selected by the i-th cluster. If the number of dynamic points is greater than the number of static points, the points in the cluster are dynamic, otherwise the points in the cluster are all static points. The specific elimination process is as follows Figure 4 shown.

[0105] Furthermore, in step (4), inter-frame matching is performed on the static feature points after semantic and geometric information removal in steps (2) and (3), and then PNP (Perspective-n-Point) and ICP (Iterative Closest Point) are used to estimate the pose. Then, the BA (Bundle Adjustment) graph optimization method is used to adjust the camera's pose information and the spatial position of the image feature points, so that the light emitted by the feature points converges to the optical center of the camera. The specific method is to use the camera pose and spatial landmark points as target optimization variables, use the reprojection error of the spatial points to construct the cost function, and then use the Schur elimination method to solve the incremental equation of the cost function. The reprojection error can be expressed as:

[0106]

[0107] Among them, T i is the current frame pose, P j is the coordinate of the landmark point, z ij In position T i Observe the landmark point P j The observation data generated. The Gauss-Newton method is further used to solve the objective function. At the same time, the sliding window strategy and loop detection algorithm are introduced to correct the drifted posture optimization to obtain the camera posture estimation, and finally the optimal motion trajectory of the camera is obtained.

[0108] Furthermore, in step (5), a drawing key frame selection strategy is firstly adopted, and its specific method is to establish a key frame information library according to the information of the mapping points observed by the key frame. When a new key frame is inserted, the similarity between the key frame and the information library is calculated according to the BOW (Bag-of-Words) model. The higher the similarity, the more redundant information the current frame has, and vice versa. i The calculation uses TF-IDF.

[0109] s i =TF i *IDF i

[0110]

[0111] Here TF i Indicates the frequency of a feature vector appearing in the current key frame image, IDF i Indicates the frequency of a feature vector in the information database. i is the number of a certain feature vector in the current key frame, n i is the total number of feature vectors in the current key frame image. N iis the number of a certain feature vector in the information database, and N is the total number of feature vectors in the information database.

[0112] Set the similarity threshold λ min , only when the similarity between the current key frame and the information base is less than λ min It is used as a drawing keyframe only when it is used.

[0113] For the feature point P in the drawing key frame c (x c ,y c ,z c ), using the transformation matrix T from the world coordinate system to the camera coordinate system cw Get the corresponding coordinate point P in the world coordinate system w , that is, the spatial position of the three-dimensional point cloud.

[0114]

[0115] All the obtained three-dimensional point clouds are stitched together to obtain a dense point cloud map of the scene. The outlier noise points are processed by statistical filtering to generate a dense point cloud map. Finally, the processed point cloud data is published using the dense point cloud node in the ROS system. At the same time, octree map nodes and two-dimensional grid map nodes are created and subscribed to, and these point cloud data are converted into octree maps and two-dimensional grid maps with navigation and obstacle avoidance functions through the octomap_server function package. Finally, these three map nodes are subscribed in the Rviz visualization platform to realize the visualization of the three maps.

[0116] Simulation experiment:

[0117] The simulation environment for the visual SLAM method experiment for dynamic indoor environments is: GPUNVIDIA RTX3060, CPUi9-12900H, memory 16GB, Ubuntu 18.04LTS, CUDA 11.1.

[0118] In order to verify the accuracy and robustness of the present invention in dynamic indoor environments, the public TUM dataset is used for verification, and ORB-SLAM2 and the present invention are compared. TUM contains dynamic object category sequences, which are specifically divided into high dynamic environment and low dynamic environment. Figure 6 In the sequence a, b, and c in the figure, two people move around the desk in a relatively large range. In the low-dynamic environment sequence (such as Figure 6 In the d sequence in Figure 1, two people sit in chairs at a desk and talk with gestures without moving their bodies.

[0119] Figure 5The dynamic point removal effects of each algorithm in different scenarios are shown. (a1)(b1) show the feature points extracted by ORB-SLAM2, (a2)(b2) show the effect of ORB-SLAM2 adding YOLOv5 to remove dynamic feature points, and (a3)(b3) show the effect of using the method of the present invention to remove dynamic feature points.

[0120] Figure 6 The ATE error comparison results of the camera estimated trajectory and the ground truth of each algorithm are shown. The first, second and third columns respectively represent the ATE error of ORB-SLAM2, ORB-SLAM2 with YOLOv5, and the method of the present invention under each dynamic sequence. The dotted line in the figure represents the ground truth of the camera trajectory, the dotted line is the trajectory estimated by the algorithm, and the shaded area composed of the line segments represents the absolute difference between the estimated trajectory and the ground truth. The larger the shaded area, the larger the error and the less accurate the positioning. The rectangular box indicates that the algorithm has tracking loss in some frames.

[0121] Table 1 shows the error comparison between each algorithm and the ground truth more quantitatively by calculating the RMSE root mean square error and STD standard deviation. The root mean square error and standard deviation can more intuitively reflect the stability and robustness of the SLAM system.

[0122] Table 1. Comparison of absolute trajectory error (ATE) (unit: m)

[0123]

[0124]

[0125] It can be seen that due to the movement of dynamic targets in the data set, the SLAM system generates many incorrect pose estimates. After adding YOLOv5 to remove dynamic points, the overall error of the algorithm is significantly reduced. However, in some frames, due to the excessive proportion of moving people in the picture, excessive removal leads to the system "losing track". Figure 7 As shown in the yellow box in (a2)(b2), the method of the present invention further adds multi-view geometric re-judgment on the basis of target detection, so that the trajectory error estimated by the system is smaller, and more available feature points are retained, making the system performance more robust.

[0126] Figure 7The point cloud map constructed without eliminating dynamic points, the static point cloud map constructed by the present invention, and the octree and grid map effects are respectively demonstrated. It can be seen that if the dynamic points are not eliminated, the system cannot process the dynamic objects in the image, and can only directly construct a dense point cloud map based on the feature points obtained, and the point cloud result is very messy. The moving figures in the data set block the tables and chairs, making it impossible to determine the appearance of the real static environment. The present invention selects the drawing key frames by eliminating the dynamic feature points in the picture and calculating the similarity of each key frame, so that the real static environment can be clearly displayed, and less map point redundancy is guaranteed, which fully proves the effectiveness of the present invention. The octree and grid map further generated according to the dense point cloud map can be directly used for advanced tasks such as navigation and obstacle avoidance.

[0127] The working principle of the present invention is:

[0128] First, ORB feature points are extracted based on the depth image information, and then the dynamic feature points are eliminated by combining the improved YOLOv5 and geometric constraint methods. Image matching is performed based on the eliminated feature points to obtain the initial pose estimation, complete the real-time local mapping and image optimization, and then use loop detection to eliminate the accumulated error to obtain the global pose optimization result. Finally, the drawing key frame is selected, and the feature points are projected from two dimensions to three dimensions according to the pose of each frame, and dense static point cloud maps, octrees and grid maps are constructed by splicing. The present invention effectively reduces the adverse effects of moving objects in complex environments on the visual SLAM system, and improves the accuracy and robustness of positioning and mapping.

[0129] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A visual SLAM method for dynamic indoor environments, characterized in that: The following steps are involved: Step 1: Input the depth image into the system front end to extract feature points; Step 2: Introduce the CBAM attention mechanism into the C3 module in the Backbone and Neck networks of the YOLOv5 model, modify the EIOU loss function, and use the improved YOLOv5 to remove dynamic points from the image after the feature points are detected in step 1; Step 3: Use the method combining multi-view geometry and K-Means to re-judge and verify and re-determine its motion state; Step 4: Perform feature point matching on the determined static feature points, and then perform initial pose estimation, loop detection and global optimization operations to obtain the final global pose result; Step 5: Calculate the similarity based on TF-IDF, select the drawing keyframes, project the feature point pixels in each drawing keyframe into a three-dimensional point cloud, and perform splicing and filtering to construct a dense static point cloud map, and convert it into the corresponding octree and raster map.

2. The visual SLAM method for dynamic indoor environments according to claim 1, characterized in that: In the step 1, robustness and extraction efficiency are comprehensively considered. The present invention adopts ORB as the front-end feature point. The ORB algorithm consists of two parts: detecting FAST corner points and calculating BRIEF descriptors. The FAST algorithm selects a total of 16 pixels on a circle with a radius of 3 with pixel p as the center. If the grayscale difference between 9 consecutive pixels among the 16 pixels and p is greater than the set threshold T, p is judged to be a FAST corner point, which can be expressed by the calculation formula: Where n is the total number of pixels on the circumference of point p, I(x) is the grayscale value of any pixel on the circumference, I(p) is the grayscale value of the pixel to be detected, and T is the corner detection threshold; BRIEF is a binary descriptor, which is a description vector composed of binary strings represented by 0 and 1. For the grayscale values ​​of two pixels (x and y) in the neighborhood of the feature point, if x is greater than y, it takes 1, otherwise it takes 0.

3. The visual SLAM method for dynamic indoor environments according to claim 2, characterized in that: In step 2, the CBAM attention mechanism is introduced into the C3 module. First, the spatial information of the input feature map is aggregated by using maximum pooling and average pooling operations to generate two different spatial context descriptors. and They are then forwarded to a shared network consisting of MLPs. After each descriptor has been applied to the shared network, the output feature vectors are merged using element-wise summation to finally produce a channel attention map. In the spatial attention module, average pooling and max pooling operations are first applied along the channel axis to generate two 2D maps: and Represent the average pooling features and maximum pooling features in the channel respectively, connect them to generate a valid feature descriptor, and then use the convolution layer to generate the spatial attention map 4. The visual SLAM method for dynamic indoor environments according to claim 3, characterized in that: In the step 3, the feature points are re-judged using a multi-view geometric constraint method to obtain the true motion of all image pixels; The K-Means clustering algorithm is used to divide the 3D point cloud into k clusters. The number of pixels in the image is divided by 2000 as the value of k. If it is less than 2000, a new cluster is automatically formed. 200 pixels are randomly selected in each cluster, and the motion characteristics of each selected point are determined using the multi-view geometric constraint method. Then, the overall motion state of the cluster is determined based on the majority motion characteristics of the selected points.

5. The visual SLAM method for dynamic indoor environments according to claim 4, characterized in that: In the step 4, the static feature points after the semantic and geometric information is eliminated in step 2 and step 3 are matched between frames, and then the PNP and ICP methods are used to estimate the pose. Then, the BA graph optimization method is used to adjust the camera's pose information and the spatial position of the image feature points, so that the light emitted by the feature points converges to the optical center of the camera. The camera pose and the spatial landmark points are used as target optimization variables, and the reprojection error of the spatial points is used to construct the cost function. Then, the incremental equation of the cost function is solved with the help of the Schur elimination method. The reprojection error can be expressed as: Among them, T i is the current frame pose, P j is the coordinate of the landmark point, z ij In position T i Observe the landmark point P j The observation data generated.

6. The visual SLAM method for dynamic indoor environments according to claim 5, characterized in that: In the step five, a drawing key frame selection strategy is first adopted to establish a key frame information library based on the information of the mapping points observed by the key frame. When a new key frame is inserted, the similarity between the key frame and the information library is calculated based on the BOW model. The higher the similarity, the more redundant information the current frame has, and vice versa.

7. The visual SLAM method for dynamic indoor environments according to claim 6, characterized in that: The i-th key frame similarity s i The calculation uses TF-IDF, s i =TF i *IDF i TF i Indicates the frequency of a feature vector appearing in the current key frame image, IDF i Indicates the frequency of a feature vector in the information database, m i is the number of a certain feature vector in the current key frame, n i is the total number of feature vectors in the current key frame image, N i is the number of a certain feature vector in the information database, and N is the total number of feature vectors in the information database.

Images