Real-time semantic slam based on spatial coordinate error in dynamic scene

Abstract

The present application relates to a kind of real-time semantic SLAM based on spatial coordinate error under dynamic scene, SCE-SLAM: real-time semantic SLAM system based on spatial coordinate error under dynamic scene is proposed, it is based on the RGB-D mode of ORB-SLAM3.SCE-SLAM is closely combined with semantic information and geometric information.Considering the real-time requirement, semantic module uses the latest and fastest object detection network YOLOv7 to provide semantic priori knowledge to geometric module.A new geometric constraint method is then proposed to filter dynamic feature points.This method makes full use of depth image and semantic information to recover three-dimensional feature points and initial camera pose.Three-dimensional coordinate space error is used as threshold, then SCE-SLAM removes dynamic points through K-means clustering algorithm.In this way, SCE-SLAM can effectively reduce the influence of dynamic points and meet the real-time requirement.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of computer vision, and relates to a real-time semantic SLAM based on spatial coordinate error in a dynamic scene, in particular to a technology for assisting a SLAM system to complete real-time positioning and mapping in a dynamic environment by using a target detection deep learning model. BACKGROUND

[0002] In recent years, autonomous robots, AR and UAV technologies have developed rapidly. In these applications, the task of reconstructing the surrounding environment and identifying its own state becomes particularly important. Compared with the laser radar camera, the visual camera has the advantages of low cost and rich image information. As the eyes of the robot, the visual camera can be divided into three main types: monocular camera, stereo camera and RGB-D camera. This technology uses the RGB-D camera, and visual simultaneous localization and mapping (VSLAM) has attracted the attention of many researchers as a prerequisite technology.

[0003] Many excellent SLAM systems have emerged, such as ORB-SLAM3, SVO, Vins-Mono, LSD-SLAM, but most of them have a basic assumption that the environment is static. However, the assumption of a static environment is too strict, and there are a large number of dynamic objects in the real world environment. Feature points located in dynamic objects can cause errors in camera motion estimation of the SLAM system, because they will cause false matches and greatly interfere with the position in the tracking process. Although these traditional SLAM systems use the RANSAC (Random Sample Consensus) and robust loss function to reduce the negative effects of dynamic points, when there are high dynamic objects, especially when dynamic objects occupy a large proportion in the scene, these algorithms may have little or no effect.

[0004] With the development of deep neural networks (DNN) in recent years, many deep learning networks have been widely used in the field of computer vision. In traditional SLAM systems, we use key points, descriptors, BOW (Bag-of-Words model) and other pixel-level information to describe an image. However, deep learning methods can provide more semantic information closer to the human visual system. Therefore, some research combines visual SLAM and DNN to design a dynamic SLAM system to eliminate the negative effects of dynamic points. However, there are still some problems in the existing dynamic SLAM systems. For example, semantic segmentation has high accuracy, but it may consume a lot of time, which may cause the SLAM system to fail to meet the real-time requirements in actual applications. On the contrary, object detection has high speed, but its bounding box may contain some static feature points. Some dynamic SLAM systems simply delete all feature points in the dynamic region, which will reduce the robustness of the system. SUMMARY

[0005] Technical problems to be solved

[0006] In order to avoid the shortcomings of the prior art, the present application proposes a real-time semantic SLAM based on spatial coordinate error in a dynamic scene, which solves the problem of poor effect and low running efficiency of the current dynamic SLAM system in a dynamic environment.

[0007] Technical solutions

[0008] A real-time semantic SLAM based on spatial coordinate error in a dynamic scene, characterized by the following steps:

[0009] Step 1: Use the neural network YOLOv7 trained with the COCO dataset to detect objects in the scene, and obtain the object information detected by YOLOv7, including the coordinates of the four points of the object bounding box and the type of the object;

[0010] All object types detected by YOLOv7 are divided into three categories: (1) static objects, (2) dynamic objects, and (3) potentially dynamic objects;

[0011] Take the feature points on the first type of object as static feature points, and the feature points on the last two types of objects as potentially dynamic feature points, to form two types of feature points;

[0012] Step 2: For static feature points, use LK optical flow to match with the previous frame image to obtain the corresponding static feature point matching pair, and then use the static feature point matching pair to calculate the fundamental matrix E = KFK T , and recover the initial camera pose through the fundamental matrix;

[0013] Step 3: For static feature points, use the camera pose to calculate the evaluation parameter Th of all points in this class, and after removing outliers, take the mean of the remaining static feature point evaluation parameters as the threshold value For potentially dynamic feature points, use the K-means clustering algorithm to divide them into k clusters, calculate the evaluation parameter of all potentially dynamic feature points in each cluster, and calculate its mean value τ k If the τ k of the cluster is greater than the threshold value Th of the static feature point class All feature points in the cluster are pure dynamic feature points, and the feature points less than the threshold value Th are filtered static feature points;

[0014] Step 4: Input the static feature points identified in step 1 and the filtered static feature points in step 3 into the subsequent process of ORB-SLAM3.

[0015] The neural network YOLOv7 is a neural network YOLOv7 trained with the COCO dataset.

[0016] The YOLOv7-detected RGB image object classification is further classified using the SCE-SLAM settings.

[0017] The essential matrix is obtained by using the OpenCV function "findessentialmat" on the static feature point matching pairs.

[0018] When matching the LK optical flow with the previous frame image, two different strategies are used to prevent extreme cases from occurring: (1) when the number of correct point matches does not reach 8, the feature points located in the dynamic boundary box but not in the static boundary box are retained for further testing; (2) when the number exceeds 8, the feature points located in the dynamic boundary box are directly deleted; in addition, feature points that are too close to the image edge and the boundary box are also discarded, and then the remaining feature points in the current frame are matched with the previous frame using the LK optical flow method.

[0019] According to the initial camera pose recovered from the essential matrix, at least eight corresponding feature point pairs are required, and the matching process is as follows: find the corresponding potential feature point pairs in the static area, and represent the potential static matching point sets in the previous frame and the current frame, respectively; then, the epipolar constraint and RANSAC are used to screen out the most accurate matching static feature point pairs;

[0020] The epipolar constraint model is:

[0021]

[0022] where F is the essential matrix, and are potential static feature points in S L and S C .

[0023] The distance between each in the current frame and the corresponding epipolar line is calculated:

[0024]

[0025] If the D value is greater than the threshold value, the corresponding feature point pair is deleted;

[0026] The remaining feature point pairs and the essential matrix E matrix are sent to the OpenCV function to recover the initial camera pose T, rotation matrix R, and displacement t.

[0027] The threshold value D is 0.1.

[0028] The formula of the evaluation parameter is Th i = 0.7 theta i + 0.3 l i , i = 1, 2, 3,..., n; wherein theta i is in the form of radian. It is multiplied by the 0.7 value close to 0.3 l i .

[0029] The threshold of the static feature point is i = 1, 2, 3,..., n * ; wherein n* is the number of remaining feature points in the static feature point class after removing outliers.

[0030] An application of the method, characterized in that: the output data is connected with ORB-SLAM3, in the subsequent process of ORB-SLAM3, tracking and mapping are carried out, and according to corresponding tasks, the motion state of the camera itself or information in the reconstructed scene is obtained.

[0031] Advantages

[0032] The application provides a real-time semantic SLAM based on spatial coordinate errors in a dynamic scene, and proposes SCE-SLAM: a real-time semantic SLAM system based on spatial coordinate errors in a dynamic scene, which is based on an RGB-D mode of ORB-SLAM3. SCE-SLAM closely combines semantic information and geometric information. Considering the real-time requirement, a semantic module uses the latest and fastest object detection network YOLOv7 to provide semantic prior knowledge to a geometric module. Then a new geometric constraint method is proposed to filter dynamic feature points. The method fully utilizes depth images and semantic information to recover three-dimensional feature points and an initial camera pose. Three-dimensional coordinate space errors are used as thresholds, and then SCE-SLAM removes dynamic points through a K-means clustering algorithm. In this way, SCE-SLAM can effectively reduce the influence of dynamic points and meet the real-time requirement. BRIEF DESCRIPTION OF DRAWINGS

[0033] Figure 1 : a flowchart of SCE-SLAM

[0034] Figure 2 : a detailed flowchart of SCE-SLAM

[0035] Figure 3 : YOLOV7 processes an RGB picture, and the picture is from a public RGBD dataset of Technical University of Munich

[0036] Figure 4 : red is a feature point in a dynamic bounding box, and green is a point in the dynamic bounding box but in a static bounding box, and the picture is from a public RGBD dataset of Technical University of Munich

[0037] Figure 5 : Matching feature points in consecutive frames are converted into 3D coordinates

[0038] Figure 6 : Histogram showing Th i In V static Two random images from the fr3_walking_halfsphere sequence. The x-coordinate represents the value of Th. The y-coordinate represents the number of each range of Th.

[0039] Figure 7 Feature point extraction results of ORB-SLAM3 and SCE-SLAM in the fr3_walking_halfsphere sequence.

[0040] Figure 8 : Real and estimated trajectories in fr3_warking_halfsphere. The first column is the comparison of 3D trajectories. The second column is the fitting results on X, Y, Z axes. The third column is the fitting results on roll, pitch, and yaw axes.

[0041] Figure 9 : ATE and RPE of ORB-SLAM3 and SCE-SLAM in fr3_warking_halfsphere. The first and second columns describe the ATE and RPE results of translation, respectively.

[0042] Figure 10 : Object detection module

[0043] Figure 11 : Self-motion estimation module

[0044] Figure 12 : Dynamic feature point removal module DETAILED DESCRIPTION

[0045] The present application will now be described in further detail, by way of example, with reference to the accompanying drawings, in which:

[0046] The detailed implementation process includes the following steps:

[0047] 1. Problem description

[0048] SCE-SLAM is mainly based on ORB-SLAM3, which itself consists of three parallel threads: tracking thread, mapping thread and loop closure thread. In order to further improve the robustness and accuracy of ORB-SLAM3, SCE-SLAM adds three new modules: real-time object detection module, ego-motion estimation module and dynamic feature point removal module. These new modules can work together to effectively reduce the influence of dynamic points and achieve real-time requirements by using semantic information and geometric information. Figure 1 An overview of our proposed SCE-SLAM is shown, Figure 2 More details are shown.

[0049] The process of the invention:

[0050] Object detection:

[0051] In order to improve the detection speed and ensure accuracy, SCE-SLAM uses YOLOv7 to detect objects in RGB images. YOLOv7 is the most advanced object detection network. In addition, its speed and accuracy are superior to all known object detection in the range of 5FPS to 160FPS. Compared with the R-CNN series, the accuracy of YOLO is slightly lower, but its detection speed is very fast.

[0052] As Figure 3 YOLOv7 detects an RGB image in the TUM dataset, it will provide several information: the coordinates of the four points of the bounding box and the type of object in it. Subsequently, SCE-SLAM uses this information for reclassification. In SCE-SLAM, we divide all object types detected by YOLOv7 into three categories according to their potential motion states: (1) static objects, such as walls, televisions, tables. (2) dynamic objects, such as people and cars, (3) potentially dynamic objects, such as mice, chairs, bottles. In this paper, we mainly consider the first two categories. So there will be two types of bounding boxes: static bounding boxes and dynamic bounding boxes. Correspondingly, the feature points will also be classified according to the coordinate information and category information of the bounding box in order to do the next work. The object detection thread that generates semantic prior knowledge from the input RGB image has two purposes. On the one hand, it provides semantic knowledge for the ego-motion estimation module to select static feature points. Subsequently, these static feature points are used to restore the initial pose of the camera. On the other hand, it extracts potential dynamic feature points for the dynamic feature point removal module to remove pure dynamic feature points.

[0053] Ego-motion estimation:

[0054] We use semantic prior information to select static feature points. Then, these points are used to compute the initial camera pose. To achieve this, we need to use the OpenCV function "findessentialmat" to obtain. Then the initial camera pose is recovered from the essential matrix, which requires at least eight pairs of corresponding feature points.

[0055] After the target detection thread, there are two types of bounding boxes in the current frame. However, existing dynamic SLAM systems that utilize target detection all directly remove all feature points in the dynamic bounding box. In fact, the area occupied by the bounding box in the RGB image is larger than the object. In particular, if we directly remove all feature points located in the dynamic bounding box, when the dynamic bounding box occupies a large portion of the image area, the remaining static feature points can not produce satisfactory point matching. As shown in Figure 4 , there are still some static feature points inside the dynamic bounding box. If we remove all feature points in the dynamic bounding box, the number of remaining static points is only 14 (in fact, the remaining 14 static points can compute the essential matrix. But not all static feature points can find a feature point pair in the last frame). However, as shown in the dynamic bounding box on the right of the figure, we cannot simply regard the green points as static points, because some green points are located at the edge of the person.

[0056] To improve the robustness and speed of SCE-SLAM, we propose two different strategies to prevent extreme cases from occurring: (1) When the number of correct point matches does not reach 8, the feature points located inside the dynamic bounding box but not inside the static bounding box will be retained for further testing. (2) When the number exceeds 8, the feature points located inside the dynamic bounding box will be directly removed. In addition, feature points that are too close to the image edge and the bounding box will also be discarded. Then the remaining feature points in the current frame are matched with the last frame using the LK optical flow method.

[0057] We find the corresponding potential feature point pairs in the static region. represent the potential static matching point sets in the last frame and the current frame, respectively. Then, we use epipolar constraint and RANSAC to filter out the most accurate matching static feature point pairs. The epipolar constraint model can be represented by equation (1):

[0058]

[0059] where F is the essential matrix, are the potential static feature points in S L and S C . ​​

[0060] For each We calculate the distance between the corresponding epipolar lines. The distance can be calculated by equation (2):

[0061]

[0062] We preset the threshold value as 0.1. If the D value is greater than the threshold value. The corresponding feature point pair will be deleted. E is the fundamental matrix. It can be calculated by equation (3):

[0063] E = KFK T (3)

[0064] Where K is the intrinsic matrix of the camera.

[0065] The remaining feature point pairs and E matrix are sent to the OpenCV function to recover the initial pose of the camera T, rotation matrix R and displacement t.

[0066] Dynamic feature point removal:

[0067] As Figure 4 shown, the feature points in the dynamic bounding box have static (such as those located on the display screen, keyboard, mouse) and dynamic (located on the dynamic parts of the human body). The main task of this module is to remove the pure dynamic feature points (feature points with displacement between the previous two frames) in the dynamic bounding box. In Figure 5 , we assume that P1 is a static point and P2 is a dynamic point, {q1, q2} represents the feature points in the image coordinate system at time t, and {p1, p2} represents the corresponding matching feature points at time t-1. Then we get the depth information of the previous frame image and project {p1, p2} to {P1, P2} in the camera coordinate system at time t-1. Similarly, we get {Q1, Q2} in the camera coordinate system at time t. Therefore, there will be two offset vectors: P1-Q1 and P2-Q2, l represents the module of the offset vector, which can be calculated by equation (7). θ represents the angle of the offset vector, which can be calculated by equation (8). In the ideal case, P1 and Q1 will overlap because P1 is a static point and {p1, q1} is a corresponding feature point pair. In fact, due to various random errors, there is inevitably an offset. However, as Figure 5 shown, the difference in the length and angle between P1-Q1 and P2-Q2 is clearly illustrated. We consider that a point is dynamic or static according to its actual movement in the spatial coordinates. We propose a very direct and easy-to-understand method to distinguish between dynamic and static points.

[0068]

[0069]

[0070]

[0071]

[0072]

[0073] where, π -1 is the function of back-projection, which depends on the camera type. Now P1 is in the camera coordinate system at time t-1. We project P1 in the initial camera pose T(R, t) at time t to P1-Q1 can be calculated by -Q1. When random error occurs, the module of this offset vector is also large if the depth d is large. On the contrary, the angle of this offset vector will be small. Therefore, it is suggested to use weighted average method to describe the offset vector.

[0074] In the current frame, the set of feature points not in the dynamic bounding box is V static = {p i , i = 1, 2, 3,..., n}. The set of feature points in the dynamic bounding box is V other = {q j , j = 1, 2, 3,..., m}. Through equations (4)-(8), we get the corresponding offset vector of p i . Then we calculate Th i by equation (9):

[0075] Th i = 0.7θ i + 0.3l i , i = 1, 2, 3,..., n; (9)

[0076] where θ i is in radian form. It is multiplied by 0.7 value close to 0.3l i .

[0077] In Figure 6 , we randomly show the histogram of Th static in V i = {p i , i = 1, 2, 3,..., n} of two frames as an example. Due to the wrong matching or depth information, there may be a small amount of outliers. We remove these outliers, and the remaining feature points are used to calculate the average value of Th i , which is denoted as

[0078]

[0079] Where n * It represents the number of remaining feature points.

[0080] We assume that 3D feature points do not move individually. Points that are close together belong to the same part of the object. We divide the 3D feature points in the current frame into k groups based on their 3D coordinates. k is one-tenth of the number of feature points in the dynamic bounding box. If this number is less than 10, each 3D feature point will be considered as its own cluster.

[0081] In V other ={q j In the set of three-dimensional feature points in the sequence {j = 1, 2, 3, ..., m}, the corresponding set of feature points is V. 3d-other ={Q j j =

[0082] 1, 2, 3, ..., m}. We use the K-means method to transform V 3d-other Divide into k clusters. Correspondingly, V other The clusters are also divided into k clusters. For each cluster, we calculate the mean of Th. This mean is then used to determine the type of each cluster. The mean of Th across the k clusters is denoted as τ. k :

[0083]

[0084] Where w is the number of feature points in the k-cluster.

[0085] For each cluster, we calculate τ k and (V static The average value of Th in the middle is compared. The type of k cluster can be determined by the following:

[0086]

[0087] If the type is determined to be dynamic, all feature points in that cluster are classified as purely dynamic; otherwise, the cluster is static. Finally, these purely dynamic points are filtered out from the entire extracted feature set. The remaining static feature points are then fed into the SLAM system for tracking and mapping.

[0088] Implementation method:

[0089] SCE-SLAM runs on the Linux platform, and we use ROS2 to link the communication between YOLOV7 and SCE-SLAM. First, start the SCE-SLAM node to import the image sequence. Then, start the YOLO node and let it subscribe to the image data from the SCE-SLAM node. In the YOLO node, for each input image, perform target detection and output the coordinates and class information of each detected bounding box. Package these information into ROS messages and publish them to the SCE-SLAM node through the ROS2 topic.

[0090] In the SCE-SLAM node, subscribe to the bounding box information published by YOLO, reclassify the bounding box information according to our pre-designed object categories, and classify the feature points according to the coordinate information of the bounding box, then perform self-motion estimation and dynamic feature point removal to improve the pose estimation results of the SLAM system under dynamic links.

[0091] Dataset experiment results:

[0092] We conducted a series of experiments on the TUM dataset fr3_walking_halfsphere sequence. To prove the effectiveness of SCE-SLAM, we compared it with ORB-SLAM3 (only RGB-D mode, no IMU).

[0093] In Figure 7 , the green points are selected feature points, we can see that in the results of the first row of ORB-SLAM3 running, there are a large number of feature points falling on the moving person, these feature points will affect the positioning and mapping of the SLAM system, but SCE-SLAM removes these feature points falling on the person and retains the static feature points in the background.

[0094] Figure 8 The trajectory graph and the comparison between the dataset real trajectory graph in

[0095] Figure 9 In

[0096] Table 1 shows the running time of each frame of SCE-SLAM and ORB-SLAM in the Fr3_walking_static dataset, SCE-SLAM takes more time than ORB-SLAM3, but basically meets the real-time requirements.

[0097]

Claims

1. A real-time semantic SLAM based on spatial coordinate error in dynamic scenes, characterized in that The steps are as follows: Step 1: using the neural network YOLOv7 trained with the COCO dataset to detect objects in the scene, obtaining object information detected by YOLOv7, including: the coordinates of the four points of the object bounding box and the type of the object; Classify all object types detected by YOLOv7 into three categories: (1) static objects, (2) dynamic objects, and (3) potentially dynamic objects; Take the feature points on the first type of object as static feature points, and the feature points on the last two types of objects as potentially dynamic feature points, to form two types of feature points; Step 2: For static feature points, match with the previous frame image using LK optical flow to obtain the corresponding static feature point matching pairs, and then calculate the fundamental matrix using the static feature point matching pairs , and restore the initial camera pose through the fundamental matrix; Step 3: For static feature points, use the camera pose to calculate the evaluation parameters of all points in this class , after removing outliers, the mean of the remaining static feature point evaluation parameters is taken as the threshold , for potential dynamic feature points, divide them into k clusters by K-means clustering algorithm, calculate the evaluation parameters of all potential dynamic feature points in each cluster and calculate their mean , if the of the cluster is greater than the threshold of the static feature point class , all feature points in the cluster are pure dynamic feature points, and the feature points less than the threshold are filtered static feature points; The evaluation parameter calculation process is: It is a static point. It is a dynamic point. Indicates time Feature points in the time-image coordinate system Indicates time The corresponding matching feature points; when the depth information of the previous frame image is obtained, and... Reprojection to time In the camera coordinate system ;Will Reprojection to time In the camera coordinate system ;Will In the camera coordinate system The camera pose calculated in step 2 is transferred to the same coordinate system; therefore, there will be two offset vectors: , , express The offset vector module is calculated using the following formula: denotes the angle of the offset vector is calculated by the following equation: Where the points that distinguish between dynamic and static are based on the following method: In time In the camera coordinate system, the following formula is used to... In time Initial camera pose Projected to ; wherein, represents a function of back-projection, which depends on the camera type; In the current frame, the feature points not in the dynamic bounding box are matched with the corresponding feature points in the previous frame by optical flow, and the corresponding offset vectors are calculated for the feature point pairs and the angle of the offset vector Then the following formula is used to calculate : Step 4: input the static feature points identified in step 1 and the static feature points filtered in step 3 into the subsequent process of ORB-SLAM3.

2. The real-time semantic SLAM based on spatial coordinate error in dynamic scene according to claim 1, characterized in that: The neural network YOLOv7 is a neural network YOLOv7 trained with the COCO dataset.

3. The real-time semantic SLAM based on spatial coordinate error in dynamic scene according to claim 1, characterized in that: The object classification of the RGB image detected by YOLOv7 is further classified using the settings of SCE-SLAM.

4. The real-time semantic SLAM based on spatial coordinate error in dynamic scene according to claim 1, characterized in that: The fundamental matrix is obtained by using the function findessentialmat of OpenCV for static feature point matching pairs.

5. The real-time semantic SLAM based on spatial coordinate error in dynamic scene according to claim 1, characterized in that: When matching with the previous frame image using LK optical flow, two different strategies are used to prevent extreme cases: (1) when the number of correct point matches does not reach 8, the feature points located in the dynamic bounding box but not in the static bounding box are retained for further testing; (2) when the number exceeds 8, the feature points located in the dynamic bounding box are directly deleted; in addition, feature points that are too close to the image edge and the bounding box are also discarded, and then the remaining feature points in the current frame are matched with the previous frame using the LK optical flow method.

6. The real-time semantic SLAM based on spatial coordinate error in dynamic scene according to claim 1, characterized in that: The restoring of the initial camera pose by the fundamental matrix requires at least eight pairs of corresponding feature points, and the matching process is as follows: finding corresponding potential feature point pairs in the static region, and respectively represent the potential static matching point sets in the last frame and the current frame; then, the epipolar constraint and RANSAC are used to screen the most accurate matching static feature point pairs; The epipolar constraint model is: where F is the fundamental matrix, and are and potential static feature points in Compute the distance between each of the epipolar lines corresponding epipolar line in the current frame: If the D value is greater than the threshold value, the corresponding feature point pair will be deleted; The remaining feature point pairs and essential matrix E matrix are sent to the OpenCV function to restore the initial pose of the camera , rotation matrix and displacement .

7. The real-time semantic SLAM based on spatial coordinate error in dynamic scene according to claim 1, characterized in that: The threshold value is 0.

1.

8. The real-time semantic SLAM based on spatial coordinate error in dynamic scene according to claim 1, characterized in that: The evaluation parameter is given by the formula In is in the form of radians, which multiplied by the value of 0.7 is close to .

9. The real-time semantic SLAM based on spatial coordinate error in dynamic scene according to claim 1, characterized in that: The threshold of the static feature point is wherein is the number of the remaining feature points in the static feature point class after removing outliers.

10. A system for real-time semantic SLAM based on spatial coordinate error in dynamic scenes according to any one of claims 1-9. The output static feature points are input to ORB-SLAM3, which tracks and maps the static feature points, and according to the corresponding task, obtains the motion state of the camera itself or the information of the reconstructed scene.

Images