CFP-SLAM: A Real-time Visual SLAM Based on Coarse-to-Fine Probability in Dynamic Environments

When there is no semantic information as the priori, using reliable constraints to find the correct feature matching relationship is the basic method to deal with dynamic SLAM problem. Li et al.[14] propose a static weighting method of keyframe edge points, and integrated into the IAICP method to reduce tracking error. Sun et al. [15] roughly detect the motion of moving objects based on self motion compensation image difference, and enhance the motion detection by tracking the motion using particle filter. Then, they [16] propose a novel RGB-D data-based on-line motion removal approach, and build and update the foreground model incrementally. StaticFusion [17] simultaneously estimates the camera motion as well as a probabilistic static/dynamic segmentation of the current RGB-D image pair. DMS-SLAM [18] uses GMS[19] to eliminate mismatched points. Kim et al.[20] propose a dense visual mileage calculation method based on background model to estimate the nonparametric background model from depth scene. Dai et al.[21] distinguishe dynamic and static map points based on feature correlation. Flowfusion [22] uses optical flow residuals to highlight dynamic regions in rgbd point clouds. Because there is no need for deep learning networks to provide semantic priors, the above methods are usually fast in dealing with dynamic factors, but lack of accuracy.

Semantic segmentation or object detection can provide a steady and reliable priority constraint for dynamic SLAM. Detect-SLAM [2] detects objects in keyframes and propagates the motion probability of keypoints in real time to eliminate the influence of dynamic objects in SLAM. DS-SLAM [4] uses SegNet[23] to obtain semantic information, combines sparse optical flow and motion consistency detection to judge people’s dynamic and static attributes. Dyna-SLAM [5] combines mask R-CNN [24] and multi view geometry to process moving objects. Brasch et al.[6] present monocular SLAM approach for highly dynamic environments which models dynamic outliers with a joint probabilistic model based on semantic prior information predicted by a CNN. With the help of the initial segmentation results, Wang et al.[7] extract the accurate pose from the rough pose by identifying and processing the moving object and possible moving object respectively, and further help to make up for the error and boundary inaccuracy of the segmentation area. Dynamic-SLAM [3] compensates SSD for missed detection based on the speed invariance of adjacent frames, and eliminates dynamic objects combined with selective tracking algorithm. SaD-SLAM [8] extracts static feature points from objects judged as dynamic based on semantic by verifying whether the inter frame feature points meet the epipolar constraints. Vincent et al.[9] perform semantic segmentation of object instances in the image, and use EKF to identify, track and remove dynamic objects from the scene. DP-SLAM [10] combines the results of geometric constraints and semantic segmentation, the dynamic keypoints are tracked in the Bayesian probability estimation framework. Ji et al. [11] only perform semantic segmentation on keyframes, cluster the depth map and identifies moving objects combined with re-projection error to remove known and unknown dynamic objects. Blitz-SLAM [12] repairs the mask of BlitzNet[25] based on depth information, and classifies static and dynamic matching points in potential dynamic areas using epipolar constraints. Generally, the above methods can accurately eliminate dynamic objects in the environment, but it is difficult to give consideration to both localization accuracy and real-time, and the performance is generally poor in low dynamic scenes.

In this paper, common variables are defined as follows:

• •

  $F_{k}$ - Frame K.

• •

  $K$ - The intrinsic matrix of a pinhole camera model.

• •

  $T_{k,w}\in{R^{4\times 4}}$ - The transformation from world frame to camera frame K, which is composed of a rotation $R_{k,w}\in{R^{3\times 3}}$ and a translation $t_{k,w}\in{R^{3\times 1}}$.

• •

  $P_{i}^{k}$ - The keypoint with ID $i$ in $F_{k}$. Its pixel coordinate is $P_{i_{uv}}^{k}=\left[u_{i}^{k},v_{i}^{k}\right]^{T}$, camera coordinate is $P_{i_{k}}^{k}=\left[X_{i_{k}}^{k},Y_{i_{k}}^{k},Z_{i_{k}}^{k}\right]^{T}$, world coordinate is $P_{i_{w}}^{k}=\left[X_{i_{w}}^{k},Y_{i_{w}}^{k},Z_{i_{w}}^{k}\right]^{T}$. $\widetilde{(\cdot)}$ is the form of homogeneous coordinates in each coordinate system.

• •

  $P_{i^{*}}^{k-1}$ - The keypoint with ID $i^{*}$ in $F_{k-1}$ which forms a matching relationship with $P_{i}^{k}$.

• •

  $O_{i^{+}}^{k}$ - The static probability of potential moving object with ID $i^{+}$. $P_{i}^{k}$ is the extracted keypoint on the object.

• •

  $O_{Th}$ - The threshold to distinguish whether the object motion attribute is high dynamic or low dynamic.

• •

  $K_{i}^{k}$ - The static probability of $P_{i}^{k}$, which is in the update state and participates in camera pose optimization.

• •

  $K_{i}^{Dk},K_{i}^{Tk},K_{i}^{Fk}$ - The static probability of $P_{i}^{k}$ obtained by the DBSCAN clustering algorithm, the projection constraints and the epipolar constraints respectively.

• •

  $M_{i^{-}}^{k}$ - The static probability of the map point forming a matching relationship with $P_{i}^{k}$.

The overview of CFP-SLAM is demonstrated in Fig.1. Based on ORB-SLAM2[13], we design a complete static probability calculation and update framework of keypoints based on multiple constraints to deal with the influence of moving objects in dynamic environment. The system obtains semantic information based on YOLOv5, compensates for missed detection based on EKF and Hungarian algorithm, and then the box between adjacent frames is associated. In $F_{k}$, only calculate and update the static probability of the keypoints inside the potential moving object box. Firstly, the static probability of potential moving object $O_{i^{+}}^{k}$ is obtained by using the optical flow and the epipolar constraints, and the object is divided into high dynamic object and low dynamic object. Initialize $K_{i}^{k}$ as the static probability of the object to which the keypoint belongs. Then, foreground points and background points is distinguished and the $K_{i}^{Dk}$ is calculated by using the DBSCAN clustering results, and the $K_{i}^{k}$ is updated to estimate the camera pose in the first stage to obtain $T_{k,w}$. Next, $K_{i}^{Tk},K_{i}^{Fk}$ are obtained by using the projection constraints and the epipolar constraints, $K_{i}^{k}$ and $M_{i^{-}}^{k}$ are updated to participate in camera pose optimization as weights to obtain a more accurate $T_{k,w}$.

When processing dynamic objects, if the semantic information as a priori is suddenly missing in some frames, on the one hand, the subsequent methods based on semantic priors will not be able to process dynamic objects. On the other hand, the sudden emergence of dynamic objects in high dynamic scenes will lead to a sharp increase in the number of keypoints incorrectly matched between adjacent frames, which leads to the loss of tracking in SLAM system in high dynamic scenario. Therefore, stable and accurate semantic information is critical.

In order to solve the missed detection problem of YOLOv5, we introduce EKF and Hungarian algorithm to compensate the missed detection of potential moving objects. EKF is used to predict the boxes of potential moving objects in $F_{k}$, while the Hungarian algorithm is used to correlate the predicted boxes with the boxes detected by YOLOv5. If the predicted box does not find a matching detected box, it could be considered that $F_{k}$ has missed detection, and the prediction result of EKF is adopted to compensate the missed detection result. After missed detection compensation, EKF and Hungarian algorithm are used again for inter frame data association of boxes.

When calculating the static probability of each potential moving object, we use the idea of DS-SLAM [4] for reference to solve the fundamental matrix $LF_{k,k-1}$ and get the polar error $Ld_{i}^{F_{k,k-1}}$. We use the epipolar constraints and chi-square distribution to test the epipolar error. Since the pixel coordinates of the matching point pair obtained by the optical flow tracking have $k=2$ degrees of freedom, if they are assumed to follow the Gauss Distribution $N(0,1)$, then according to the chi-square distribution:

The definition of the function $\Gamma(v)$ is:

The single estimation result of $O_{i^{+}}^{k}$ can be obtained:

After all estimation results are obtained by using all optical flow point pairs belonging to the object, all estimation results are sorted from small to large. Let the number of all estimation results be $M$, and take the average value of $(O_{i^{+}}^{k})_{m}$ at $0.1M,0.2M,0.3M$ position after ranking as the estimated value of object static probability $O_{i^{+}}^{k}$.

According to the calculation result of the static probability of the object and the real motion of the object, and taking into account that the negative effect of the dynamic point is generally greater than the positive effect of the increase of the static constraint when the camera pose is estimated, we set $O_{Th}=0.9$, the object motion attributes are divided into high dynamic and low dynamic, which are provided to the subsequent methods as a priori information for processing with different strategies. The static probability of all keypoints in the box of the potential moving object is initialized to $O_{i^{+}}^{k}$, and the static probability of other keypoints is initialized to $1.0$.

In the dynamic SLAM scene, the motion of the object, the incomplete appearance of the object to be detected in the camera field of view, the blurred image and the singular angle of view caused by camera rotation all bring severe challenges to the object detection, very easy to cause miss detection, even will lead to continuous frame miss detection. Fig.2(a)-(d) and Fig.2(e) show the results of missed detection compensation of object detection in the above four cases and six consecutive frames, respectively. Fig.3 shows the DBSCAN clustering results after missed detection compensation. We select two consecutive frames to show the clustering effect. The foreground points are marked with red and the background points are marked with green. The upper image group contains two people sitting on the chair and moving slowly respectively, and the people in the lower image group are in the fast walking state. It is worth noting from Fig.3 that many keypoints are extracted from the edge of the person, which is generally the part with the highest dynamic attributes. However, semantic segmentation is difficult to accurately judge the boundary of objects[12], which leads to the misjudgment of dynamic and static attributes of keypoints. We use DBSCAN algorithm to cluster keypoints based on depth information, which can well avoid this problem. The experimental results fully show the effectiveness and robustness of the missed detection compensation algorithm and clustering algorithm.

We contrast with ORB-SLAM2 [13] and forth most advanced dynamic SLAM methods, including DS-SLAM [4], Dyna-SLAM [5], Blitz-SLAM [12] and TRS [11]. Like our method, these algorithms are all improved based on ORB-SLAM2. Without calculating the static probability of the object, we provide a lower performance version of the algorithm in this paper with higher real-time performance, which is called CFP-SLAM^-. The quantitative comparison results are shown in Tables I, II and III, in which the best results are highlighted in bold and the second-best are underlined. The data of DS-SLAM, Dyna-SLAM, Blitz-SLAM and TRS comes from the source literature, $/$ indicates that the corresponding data is not provided in the source literature. The experimental results show that, unlike other dynamic SLAM algorithms, which only have advantages over ORB-SLAM2 in high dynamic scenarios, this algorithm can achieve almost the best results in high dynamic and low dynamic scenarios. Even the low-performance version we provide shows better performance than other algorithms. In rpy sequences, on the one hand, the epipolar constraints cannot be used, on the other hand, the large change of camera angle leads to insufficient feature matching, so our method performs slightly worse. The ATE and RPE plots of our algorithm on 8 sequences are shown in Fig.4.

In order to prove the function of each module of our algorithm, We design a series of ablation experiments, and the experimental results are shown in Table IV. Among them, CFP-SLAM: The algorithm of this paper; CFP-SLAM^-: Do not use static probability of objects; W/O-MDC: Without missed detection compensation; W/O-DBS: Without DBSCAN clustering; W/O-KSP: Without the static probability of keypoints, that is, all the foreground points after missed detection compensation and DBSCAN clustering are directly eliminated; Only-YOLO: Directly eliminate all keypoints in the box with human category.

The experimental results show that CFP-SLAM^- shows worse performance in low dynamic scenes, because we cannot distinguish between high dynamic objects and low dynamic objects, so all objects are processed according to high dynamic. W/O-MDC is almost unaffected in low dynamic scenes, but the performance is very poor in high dynamic scenes, especially in w/rpy, when the camera and objects are moving violently. In fact, the tracking is often lost in w/xyz, w/half and w/rpy because of missed detection. W/O-DBS and W/O-KSP show general performance in all sequences, which illustrates the effectiveness of DBSCAN clustering and the limitation of dealing with non-rigid bodies with partial motion as a whole, respectively. Only-YOLO encounters difficulties in initialization due to insufficient features in almost all sequences, and tracking is lost in some sequences.

Real-time performance is one of the important evaluation indexes of SLAM system. We test the average running time of each module, as shown in Table V. EKF represents the missed detection compensation and data association of boxes module, OSP represents the static probability calculation module of objects, and KSP represents the static probability calculation module of keypoints based on the epipolar constraints and the projection constraints. Semantic threads based on YOLOv5s run in parallel with ORB feature extraction. The results show that the average processing time per frame for the main threads of CFP-SLAM and CFP-SLAM^- is 42.7 ms and 24.77 ms, that is, the running speed reaches 23 Fps and 40 Fps respectively. Compared with the SLAM system based on semantic segmentation, it can better meet the real-time requirements while ensure the accuracy.