PLDS-SLAM: Point and Line Features SLAM in Dynamic Environment

Abstract

Visual simultaneous localization and mapping (SLAM), based on point features, achieves high localization accuracy and map construction. They primarily perform simultaneous localization and mapping based on static features. Despite their efficiency and high precision, they are prone to instability and even failure in complex environments. In a dynamic environment, it is easy to keep track of failures and even failures in work. The dynamic object elimination method, based on semantic segmentation, often recognizes dynamic objects and static objects without distinction. If there are many semantic segmentation objects or the distribution of segmentation objects is uneven in the camera view, this may result in feature offset and deficiency for map matching and motion tracking, which will lead to problems, such as reduced system accuracy, tracking failure, and track loss. To address these issues, we propose a novel point-line SLAM system based on dynamic environments. The method we propose obtains the prior dynamic region features by detecting and segmenting the dynamic region. It realizes the separation of dynamic and static objects by proposing a geometric constraint method for matching line segments, combined with the epipolar constraint method of feature points. Additionally, a dynamic feature tracking method based on Bayesian theory is proposed to eliminate the dynamic noise of points and lines and improve the robustness and accuracy of the SLAM system. We have performed extensive experiments on the KITTI and HPatches datasets to verify these claims. The experimental results show that our proposed method has excellent performance in dynamic and complex scenes.

1. Introduction

Traditional visual SLAM solutions generally develop, and design algorithms are based on corner feature landmarks. Ideally, sensors and models can accurately estimate the system state regardless of time and environment spanning. However, imperfect sensor measurements, inaccurate system modeling, complex dynamic environments, and inaccurate constraints will seriously affect the accuracy and reliability of feature detection. Most SLAM algorithms cannot effectively extract transformation concerning invariant features in complex environments, which may lead to problems, such as map point tracking loss and positioning failure. As a dynamic environment is an important aspect of complex positioning, navigation, and decision-making, the effectiveness and accuracy of perception will directly affect the positioning and navigation results. Therefore, it is of great significance to study invariant feature extraction and representation algorithms for dynamic and complex environments to deal with the problems of map point tracking loss, positioning failure, and pose drift in complex scenes.

Most SLAM schemes assume that the environment is a static scene, and only static three-dimensional feature points in space can satisfy the projection relationship of multi-view geometry. This assumption is also the basic premise of camera inter-frame pose estimation and space pose graph optimization algorithms. However, the real environment cannot avoid highly dynamic moving objects and special backgrounds, resulting in abnormal landmark tracking data of features in other frames, which greatly limits the application of visual SLAM systems in complex and dynamic environments. Therefore, it is important for navigation and positioning in complex dynamic environments to develop feature extraction and noise filtering methods based on dynamic environments to solve the problem of a sharp decline in positioning and mapping accuracy caused by map point drift.

In addition, although the SLAM method based on feature points has high time efficiency and accuracy, it is prone to instability and even failure in complex environments. For the complex dynamic environment, the paper conducts research on the key technologies of visual positioning and pose estimation based on dynamic point-line SLAM. Using the feature extraction and representation method of point-line invariance in complex environments, the prior dynamic region features are obtained by detecting and segmenting the dynamic region, a geometric constraint method based on line segments is proposed, and the potential dynamic noise features are obtained by combining the polar constraint method of feature points. At the same time, we propose an outlier elimination method based on Bayesian theory point and line feature tracking, which overcomes the unsmoothed SLAM positioning and mapping caused by the complex coupling of static features and dynamic environments. It also tackles the scattering of moving object features to cause SLAM positioning, resolving issues, such as the sharp drop in mapping accuracy.

The main contributions of this paper are:

(1)

A novel stereo dynamic SLAM system based on point-line feature fusion is proposed. The prior dynamic region features are obtained by detecting and segmenting the dynamic region, and the geometric constraints are used to obtain richer static features for the prior dynamic objects.

(2)

A line-segment-based geometric constraint algorithm is proposed to obtain potential dynamic and mis-matched linear features through geometric constraints on line segments to improve the accuracy and robustness of line feature extraction and data management.

(3)

A set of prior dynamic object recognition algorithms based on semantic segmentation is designed. The geometric constraint algorithm is used to solve of feature deviation and insufficiency for map matching and motion tracking caused by the existing algorithm without distinguishing between dynamic and static objects, which leads to tracking failure and trajectory deviation.

(4)

A Bayesian theory-based outlier elimination algorithm constrained by point-line features is proposed. This method removes dynamic point and line feature noise in complex environments and improves the accuracy of dynamic noise removal by continuous frame tracking of dynamic noise, thereby improving the accuracy and stability of the SLAM system.

2. Related Work

The main task of visual SLAM is to solve the camera position and construct the scene graph through pixel matching of image sequences. After more than ten years of research, traditional visual SLAM has been well developed. MonoSLAM [1] is the earliest proposed visual SLAM system based on an extended Kalman filter. It estimates the pose by tracking sparse features, but the complexity has a cubic relationship with the number of three-dimensional points, and it is limited to smaller running scenarios. PTAM [2], as a milestone work of visual SLAM, was the first work to propose that the tracking and mapping be divided into two threads to run in parallel, which ensured the real-time performance of the tracking and replaced the filter framework with bundle adjustment [3] optimization. The map management method improves overall accuracy. ORB-SLAM [4] and ORB-SLAM2 [5] further improved the PTAM framework, using front-end and back-end multi-threading and keyframe-based optimization, and they achieved efficient relocation and loop closure detection with the help of the visual bag of words [6] method. Thanks to the design of the essential graph, ORB-SLAM can efficiently complete the loop closure optimization. In the latest ORB-SLAM3 [7], the method of automatically generating a new map is adopted to solve the situation of visual loss, which further improves the robustness of the system. Although the SLAM method based on feature points has high time efficiency and precision, it is prone to instability and even failure in complex environments. Unlike the feature point-based methods mentioned above, direct methods use image pixel-level grayscale to estimate motion and structure by minimizing photometric error and are usually more robust in weakly textured scenes. Representative works include LSD-SLAM [8], SVO [9], DSO [10], etc. However, grayscale invariance is a strong assumption, which is difficult to satisfy and is susceptible to overexposure and image blurring. Furthermore, direct methods rely entirely on gradient search, reducing the objective function to compute the camera pose. Its objective function needs to take the gray value of the pixel, and the image is a strongly non-convex function. This makes it easy for the optimization algorithm to reach the minimum, and the direct method can only succeed when the motion is small. To solve these problems, the SLAM method, based on line features, is proposed. PEL-SLAM [11] proposes a point-line SLAM method based on EDline [12], which solves the problem of localization failure in poor textures to a certain extent. PL-SLAM [13] proposes a new stereo vision SLAM system that extends the previous VO method and is based on the combination of keypoint and line segment features, a powerful and versatile system capable of working in all types of environments, including low-textured environments, while producing geometrically meaningful maps. The authors of [14] proposed a robust SLAM system based on point-line features, which uses an orthogonal representation of straight lines, and proposed a point-line-based graph optimization method. However, these methods cannot distinguish between static and dynamic objects in the scene, resulting in reduced localization and map construction accuracy in visual SLAM systems due to incorrect data association. Therefore, it is necessary to further study visual SLAM algorithms in dynamic environments.

The main idea of the method based on geometric positioning is to use robust weighting functions or motion consistency constraints to eliminate outliers and improve positioning accuracy. Kim et al. [15] implemented background model-based camera motion estimation by estimating a non-parametric background model of the depth scene to remove outliers. Li et al. [16] proposed a RGB-D SLAM based on the frame-to-keyframe edge, adding the static weight of the keyframe foreground edge point to the intensity-assisted iterative closest point method (IAICP) to calculate the current frame pose. Sun et al. [17] proposed a RGB-D-based online motion removal method. This method constructs and incrementally updates the foreground model and uses the optical flow method for tracking. Furthermore, the authors of [15] extended previous work by utilizing optical flow to identify and remove dynamic feature points with RGB images as the only input. Static fusion [18] achieves static background reconstruction by simultaneously estimating camera motion and probabilistic static segmentation. Dai et al. [19] proposed establishing a Delaunay triangulation to determine the correlation of feature points by comparing the changes of triangle edges in adjacent frames, to distinguish dynamic and static map points. Geometry-based methods improve the performance of existing systems to a certain extent and are faster. However, those methods mainly focus on the constraints based on feature points, and none of them perform constraints on straight lines. Additionally, they lack semantic information and cannot exploit the prior knowledge of the scene to detect moving objects. Geometry-based SLAM methods are generally less robust than semantic-based SLAM methods in dynamic scenes [20].

Semantic-based dynamic SLAM uses a deep learning network to segment the input image to obtain object masks and semantic labels and removes potential objects from the scene according to the semantic information of a single frame image [21]. To improve the stability of dynamic object recognition, most semantic-based dynamic SLAM performs instance segmentation for each input frame. For example, DS-SLAM [22] is based on ORB-SLAM2 [5] and SegNet [23] and uses polar coordinate line constraints to judge the motion consistency of semantic segmentation objects. DynaSLAM [24] combines Mask R-CNN [25] and depth inconsistency checking to segment moving objects and further colorize regions with a static background. Compared with traditional SLAM methods, it is more robust in dynamic scenes. VDO-SLAM [26] exploits image-based semantic information in the scene without prior knowledge of the pose or geometry of the object, enabling simultaneous localization, map building, and tracking of dynamic objects. However, due to the problem of the algorithm optimization function, large errors will occur, and the operation efficiency needs to be improved. In DP-SLAM [27], the probability tracking of dynamic feature points based on Bayesian theory is proposed. This method has achieved good results, but it does not track line features and is too reliant on the priors of semantic segmentation when differentiating their dynamic characteristics.

Traditional visual SLAM usually uses random sampling consistency (RANSAC) and other methods to eliminate dynamic features that do not conform to static models. This method achieves good accuracy and stability when static features dominate the field of view. On the contrary, when the scene is filled with a large number of moving objects, this method is prone to tracking drift and localization failure. In the algorithm research based on dynamic object segmentation, many methods use dynamic segmentation method to divide features into dynamic collection and static collection for tracking and positioning. However, they are too reliant on the instance segmentation results of keyframes, and they do not design corresponding selection strategies for the characteristics of dynamic environments. To solve these problems, we propose a dynamic SLAM system PLDS-SLAM based on point-line features.

3. Method

We propose a stereo dynamic SLAM system based on point-line feature fusion. A priori dynamic region features are obtained by detecting and segmenting dynamic regions, and richer static features are obtained for a priori dynamic objects by using geometric constraints. Different from the traditional geometric constraint method based on point features, we propose a geometric constraint algorithm based on line segment features to solve the problem of mismatching line features and obtain potential dynamic and mismatched line segment features through geometric constraints on line segments, to improve the accuracy and robustness of line segment feature extraction and data management. A set of prior dynamic object recognition algorithms is designed based on semantic segmentation. We use the geometric constraint algorithm to solve the problem of feature deviation caused by the existing algorithm not distinguishing between dynamic and static objects and insufficient map matching and motion tracking, which lead to tracking failure and trajectory deviation. In addition, for existing methods, dynamic noise removal is often performed on feature points at a single moment or in the current frame. The result of dynamic noise removal depends heavily on the prior dynamic object segmentation results, especially at the edge of the segmentation, which is prone to wrong dynamic noise classification. Different from existing methods, we propose an outlier removal algorithm constrained by point-line features based on Bayesian theory. This method simultaneously removes dynamic point and dynamic line feature noise in complex environments, and it improves the accuracy of dynamic noise removal through continuous frame tracking of dynamic noise, thereby improving the accuracy and stability of the SLAM system.

3.1. Overall Framework

Our proposed method obtains a priori dynamic region features by detecting and segmenting dynamic regions, obtains potential dynamic noise features, and removes them by proposing a geometric constraint method for matching line segments and combining epipolar constraints on feature points. At the same time, an outlier elimination method based on Bayesian theory point and line feature constraints is proposed to improve the robustness and accuracy of the SLAM system. As shown in Figure 1, our proposed method includes four modules: visual odometry, local mapping, loop closure detection, and map.

3.2. Geometric Constraint and Representation of Line Segment

In this paper, we adopt the LSD [28] algorithm for line segment detection and compute LBD descriptors [29] to represent each line feature. Similar to the ORB descriptor of a point feature, the LBD descriptor contains the geometric properties and appearance description of the corresponding line feature. For two frames of images, the similarity of line features is measured by computing the consistency of LBD descriptors between line pairs.

Since a three-dimensional straight line can be initialized by two points in its space, as shown in Figure 2, we set the coordinates of their two endpoints as $\mathit{S}={\left(x_1,y_1,z_1\right)}^T$, $\mathit{E}={\left(x_2,y_2,z_2\right)}^T$. Then, the Plücker coordinates of the straight line in space can be expressed as:

$$\mathit{L}=\left[\begin{array}{c}\mathit{S}\times \mathit{E}\\ \mathit{S}-\mathit{E}\end{array}\right]=\left[\begin{array}{c}\mathit{n}\\ \mathit{v}\end{array}\right]\subset {ℝ}^6$$

(1)

where $\mathit{n}\in {ℝ}^3$ is the normal vector of the plane determined by the line and the origin, and $\mathit{v}\in {ℝ}^3$ is the direction vector of the line.

Corresponding to the triangulation processing of feature points, the triangulation of line segments is used in visual odometry to add line segments to map landmarks to realize multi-element feature landmarks. Let the plane line segments corresponding to the space straight line L on the two images be l₁ and l₂, respectively, then:

$${\mathit{l}}_1=({\mathit{s}}_1,{\mathit{e}}_1)$$

(2)

$${\mathit{l}}_2=({\mathit{s}}_2,{\mathit{e}}_2)$$

(3)

The starting point of line segment l₁ is ${\mathit{s}}_1\left(u_{s1},v_{s1}\right)$, the ending point is ${\mathit{e}}_1\left(u_{e1},v_{e1}\right)$, the starting point of line segment l₂ is ${\mathit{s}}_2\left(u_{s2},v_{s2}\right)$, and the ending point is ${\mathit{e}}_2\left(u_{e2},v_{e2}\right)$. The intersection line between the plane determined by the camera optical center position O₁ and the line segment l₁ in the image I₁ and the plane determined by the next camera position optical center O₂ and the line segment l₂ in the image I₂ is the spatial coordinate position of the straight line.

According to the basic principle of the space plane equation, the equation of the plane π_i is set as $A_{i}x+B_{i}y+C_{i}z=D_i$, a plane is uniquely determined by 3 points, and the plane equation π_i determined by the camera optical center O_i and the line segment endpoint s_i, e_i can be obtained using the following two formulas:

$$\left[\begin{array}{c}{A}_i\\ B_i\\ C_i\end{array}\right]={\left[{\mathit{s}}_i\right]}_{\times }{\mathit{e}}_i$$

(4)

$$D_i=\left[\begin{array}{c}{A}_i\hspace{0.25em}\hspace{0.25em}\hspace{0.25em}\hspace{0.25em}{B}_i\hspace{0.25em}\hspace{0.25em}\hspace{0.25em}\hspace{0.25em}{C}_i\end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]$$

(5)

Of these, ${[\ast ]}_{\times }$ represents the antisymmetric matrix of the vector. The plane π₁(O₁, s₁, e₁) and π₂(O₂, s₂, e₂) are obtained by the above two formulas, then the spatial linear coordinates can be obtained according to the dual Plücker matrix:

$${\mathit{L}}_w=\left[\begin{array}{c}{[{\mathit{v}}_w]}_{\times }{\mathit{n}}_w\\ -{\mathit{n}}_{\mathit{w}}^T\hspace{0.25em}\hspace{0.25em}\hspace{0.25em}\hspace{0.25em}0\end{array}\right]={\mathbf{\pi }}_1{\mathbf{\pi }}_2^T-{\mathbf{\pi }}_2{\mathbf{\pi }}_1^T\in {ℝ}^4$$

(6)

Let ${\mathit{T}}_{cw}\in SE\left(3\right)$ be the transformation from the world coordinate system to the camera coordinate system, which consists of a rotation transformation ${\mathit{R}}_{cw}\in SO\left(3\right)$ and a translation ${\mathit{t}}_{cw}\in {ℝ}^3$. The transformation process of the space straight line ${\mathit{L}}_w$ from the coordinates of the world coordinate system to the coordinates of the camera coordinate system ${\mathit{L}}_c$ is shown in the following formula:

$${\mathit{T}}_{cw}=\left[\begin{array}{cc}{\mathit{R}}_{cw}& {\mathit{t}}_{cw}\\ \mathbf{0}& 1\end{array}\right]$$

(7)

$${\mathit{L}}_c=\left[\begin{array}{c}{\mathit{n}}_c\\ {\mathit{v}}_c\end{array}\right]={\mathit{H}}_{cw}{\mathit{L}}_w=\left[\begin{array}{cc}{\mathit{R}}_{cw}& {[{\mathit{t}}_{cw}]}_{\times }{\mathit{R}}_{cw}\\ \mathbf{0}& {\mathit{R}}_{cw}\end{array}\right]{\mathit{L}}_w$$

(8)

The representation of the space line in the camera coordinate system, the process of projecting ${\mathit{L}}_c$ onto the image plane is shown in the following formula:

$$\mathit{l}={\mathit{K}}^{\prime }{\mathit{n}}_c=\left[\begin{array}{ccc}{f}_y& 0& 0\\ 0& f_x& 0\\ -f_y{c}_x& -f_x{c}_y& f_x{f}_y\end{array}\right]{\mathit{n}}_c$$

(9)

Of these, K′ represents the projection matrix of the line. Because there are mismatches and matching deviations in the matching of line segments, we propose a geometric constraint method for line segments. As shown in Figure 2, according to the projection process of Formula (9), the space straight line L corresponding to the projection line segments on the two pictures is l₁ and l₂, respectively. According to the triangular geometric relationship, the line connecting the endpoints of the projected line segments l₁ and l₂ has the constraint of a parallel relationship; the analytical description is as follows:

$${\mathit{l}}_1={\mathit{K}}^{\prime }{\mathit{n}}_{c1}$$

(10)

$${\mathit{l}}_2={\mathit{K}}^{\prime }{\mathit{n}}_{c2}={\mathit{K}}^{\prime }\left[\begin{array}{cc}{\mathit{R}}_{c2c1}& {\left[{\mathit{t}}_{c2c1}\right]}_{\times }{\mathit{R}}_{c2c1}\end{array}\right]{\mathit{n}}_{c1}$$

(11)

Among them, ${\mathit{R}}_{c2c1}$ and ${\mathit{t}}_{c2c1}$ represent the rotation and translation from camera 1 to camera 2, respectively, obtained from Formulas (10) and (11):

$${\mathit{n}}_{c2}=\left[\begin{array}{cc}{\mathit{R}}_{c2c1}& {\left[{\mathit{t}}_{c2c1}\right]}_{\times }{\mathit{R}}_{c2c1}\end{array}\right]{\mathit{n}}_{c1}$$

(12)

$${\left[{\mathit{t}}_{c2c1}\right]}_{\times }{\mathit{n}}_{c2}=\left[\begin{array}{cc}{\left[{\mathit{t}}_{c2c1}\right]}_{\times }{\mathit{R}}_{c2c1}& {\left[{\mathit{t}}_{c2c1}\right]}_{\times }{\left[{\mathit{t}}_{c2c1}\right]}_{\times }{\mathit{R}}_{c2c1}\end{array}\right]{\mathit{n}}_{c1}$$

(13)

Since ${\mathit{t}}_{c2c1}$ and ${\mathit{t}}_{c2c1}$ are parallel vectors, the outer product of parallel vectors is 0, so there are:

$${\left[{\mathit{t}}_{c2c1}\right]}_{\times }{\mathit{n}}_{c2}=\begin{array}{c}{\left[{\mathit{t}}_{c2c1}\right]}_{\times }{\mathit{R}}_{c2c1}\end{array}{\mathit{n}}_{c1}$$

(14)

Multiply both sides of the equal sign by ${\mathit{n}}_{c2}^{\mathit{T}}$ at the same time:

$${\mathit{n}}_{c2}^{\mathit{T}}{\left[{\mathit{t}}_{c2c1}\right]}_{\times }{\mathit{n}}_{c2}=\begin{array}{c}{\mathit{n}}_{c2}^{\mathit{T}}{\left[{\mathit{t}}_{c2c1}\right]}_{\times }{\mathit{R}}_{c2c1}\end{array}{\mathit{n}}_{c1}$$

(15)

Since ${\mathit{n}}_{c2}^{\mathit{T}}$ and ${\left[{\mathit{t}}_{c2c1}\right]}_{\times }{\mathit{n}}_{c2}$ are perpendicular to each other, there are:

$$\begin{array}{c}{\mathit{n}}_{c2}^{\mathit{T}}{\left[{\mathit{t}}_{c2c1}\right]}_{\times }{\mathit{R}}_{c2c1}\end{array}{\mathit{n}}_{c1}=0$$

(16)

Therefore, the line segments l₁ and l₂ satisfy the geometric constraints determined by the above formula.

The straight line in space has four degrees of freedom, and Plücker parameterization requires six parameters to represent the straight line, which leads to over-parameterization. This requires constrained optimization, creating further difficulties. We use these two parameterization forms in the SLAM system at the same time and use Plücker coordinates when initializing and performing space transformation. In the back-end graph optimization, the extra degrees of freedom will increase the computational cost and cause numerical instability. Therefore, the smallest four-parameter orthonormal representation is used. The orthogonal representation (R, D)∈SO(3) × SO(2) can be obtained from the Plücker coordinates:

$$\begin{array}{c}\mathit{L}=\left[\mathit{n}|\mathit{v}\right]=\left[\begin{array}{ccc}\frac{\mathit{n}}{\parallel \mathit{n}\parallel }& \frac{\mathit{v}}{\parallel \mathit{v}\parallel }& \frac{\mathit{n}\times \mathit{v}}{\parallel \mathit{n}\times \mathit{v}\parallel }\end{array}\right]\left[\begin{array}{cc}\parallel \mathit{n}\parallel & 0\\ 0& \parallel \mathit{v}\parallel \\ 0& 0\end{array}\right]\\ =\mathit{R}\left[\begin{array}{cc}{t}_1& 0\\ 0& t_2\\ 0& 0\end{array}\right].\end{array}$$

(17)

Further, the orthonormal representation of straight line $\mathit{L}=\left(\mathit{R},\mathit{D}\right)$ is:

$$\begin{array}{c}\mathit{R}=\mathit{R}(\stackrel{\rightharpoonup }{\mathbf{\theta }})=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\\ \mathit{D}=\mathit{D}\left(d\right)=\left[\begin{array}{cc}{t}_1& -t_2\\ t_2& t_1\end{array}\right].\end{array}$$

(18)

We use the least four parameters $\mathit{L}={\left[{\stackrel{\rightharpoonup }{\mathbf{\theta }}}^T,d\right]}^T\in {ℝ}^4$ orthonormal to represent a straight line, update R with vector $\stackrel{\rightharpoonup }{\mathbf{\theta }}\in {ℝ}^3$ to represent the direction of the straight line, and update D with d representing the distance from the straight line to the coordinate origin.

3.3. A Priori Dynamic Object Static

For the prior staticization of dynamic objects, we begin by detecting and describing the point-line features of non-priority dynamic objects. At the same time, we obtain the point-line features on the prior dynamic objects generated via instance segmentation and calculate the descriptors of point-line features. Then, we match the point-line features of the non-priority dynamic object to obtain the matching pairs of point-line features of the non-priority object region. Additionally, by adopting the random sampling consensus algorithm, the fundamental matrix is solved by using the point-line feature matching pairs of the non-priority dynamic object area.

Descriptor matching is performed on the point-line features of the prior dynamic object area, and a matching pair of point-line features located on the prior dynamic object area is obtained. Finally, we iteratively calculate each prior dynamic object and obtain the average value of the reprojection error of the fundamental matrix. If the average value is less than the threshold, it is a static object. The algorithm processing method used to determine the static object is shown in Algorithm 1.

Algorithm 1: A priori dynamic object static

Input: A priori dynamic object set, D; camera 1 point-line features P1, L1 and descriptors; camera 2 point-line features P2, L2 and descriptors; Output: The static object set, S; 1 Obtain matching keypoints and matching keylines in non-priority dynamic object regions; 2 Calculate the fundamental matrix $F=K^{-T}{\left[t\right]}_{\times }R{K}^{-1}$; 3  Matching keypoints and keylines for a priori dynamic object regions are extracted; 4 for each dynamic object Oi do 5 for each keypoint matching pair $p_i,p_i^{\prime }$ do 6 Calculate keypoint matching pair error $d\left(p_i,p_i^{\prime }\right)=\frac{\left|p_{i}F{\left(p_i^{\prime }\right)}^T\right|}{\sqrt{{\parallel X_{p_i^{\prime }}\parallel }^2+{\parallel Y_{p_i^{\prime }}\parallel }^2}}$, where $X_{p_i^{\prime }}$, $Y_{p_i^{\prime }}$ denote coordinates of epipolar line vector; 7 end for 8 for each keyline matching pair ${\mathit{l}}_{\mathit{i}},{{\mathit{l}}_{\mathit{i}}}^{\prime }$ do 9 Calculate keyline matching pair error $d\left({\mathit{l}}_{\mathit{i}},{{\mathit{l}}_{\mathit{i}}}^{\prime }\right)=\left[\frac{{{\mathit{s}}^{\mathrm{T}}}_{\mathit{i}}{{\mathit{l}}^{\prime }}_i}{\sqrt{l_1^2+l_2^2}},\frac{{{\mathit{e}}^{\mathrm{T}}}_{\mathit{i}}{{\mathit{l}}^{\prime }}_i}{\sqrt{l_1^2+l_2^2}}\right]$, among them, s and e are the start point and end point of the line segment, and l1 and l2 are the coefficients of the line segment; 10 end for 11 if $\frac{1}{n}\sum d\left(p_i,p_i^{\prime }\right)<1$ and $\frac{1}{m}\sum d\left({\mathit{l}}_{\mathit{i}},{{\mathit{l}}_{\mathit{i}}}^{\prime }\right)<1$ then 12 Append Oi to S; 13 end if 14 end for

3.4. Dynamic Noise Tracking

The accuracy of SLAM positioning and mapping is sharply reduced due to the scattering of moving object features. To tackle this, a Bayesian fusion algorithm model oriented to semantics and point-line constraints is adopted. As shown in Figure 3, firstly, the convolutional neural network Mask-RCNN instance segmentation method is used to obtain a priori potential dynamic region masks. Then, a priori dynamic object contours are obtained using masks. Second, according to the outline, the point-line features at the edge are obtained. Finally, Bayesian theory is used for information fusion for semantic mask contours to obtain the dynamic probability of object features and realize dynamic tracking of edge features, which is used to solve the problem of sharp decline in the accuracy of SLAM positioning and mapping caused by dynamic object feature scattering.

The distance from feature f_i to segmentation boundary b is defined as:

$$\mathrm{dist}\left(f_i,b\right)=min{\parallel f_i-b\parallel }_2$$

(19)

where b is the object segmentation edge, and the coordinates are expressed as pixel coordinates.

A meta-logistic regression model is defined to estimate the semantically dynamic segmentation probability of feature f_i:

$$P\left(D_k\left(f_i\right)\mid s_{mask}\right)=\frac{1}{\exp \left(\mathrm{dist}\left(f_i,b_j\right)\right)}$$

(20)

The dynamic probability of semantics means that the closer the point-line feature near the mask is to the edge, the higher the probability of misclassification. Therefore, if a feature f_i has an unusually high probability, it is more likely to be a dynamic feature despite being outside the mask.

Based on Bayesian theory, the state of the matched feature p_i in the current frame is defined as $D_k\left(f_i\right)$. If the feature f_i is in the region of the moving object, it is considered a dynamic feature; then, $D_k\left(f_i\right)=1$. Others are treated as static: $D_k\left(f_i\right)=0$. The movement probability of the feature in the history frame is used as the prior probability. Then, the posterior probability is expressed as:

$$\begin{array}{c}P\left(D_k\left(f_i\right)=1\mid \left(D_k\left(f_i\right)\mid s_{mask}\right)\right)=\frac{P\left(D_{k-1}\left(f_i\right)=1\right)P\left(\left(D_k\left(f_i\right)\mid s_{mask}\right)\mid D_{k-1}\left(f_i\right)=1\right)}{P\left(D_k\left(f_i\right)\mid s_{mask}\right)}\end{array}$$

(21)

The semantic dynamic probability, that is, the observation probability, is $P\left(D_k\left(f_i\right)\mid s_{mask}\right)$. Then, using the observation probability $\left(D_k\left(f_i\right)\mid s_{mask}\right)$ and the prior probability $P\left(D_{k-1}\left(f_i\right)=1\right)$, as well as the current movement probability $P\left(D_k\left(f_i\right)=1\right)$, are updated. The features with high moving probability are regarded as dynamic features, and the tracking of dynamic map features is eliminated to realize a robust SLAM algorithm system.

The algorithm processing method used for dynamic noise tracking to eliminate outliers is shown in Algorithm 2. The input of the algorithm is the prior dynamic object mask and the map feature, and the output is the interior point and the dynamic probability, which is used to realize dynamic denoising.

Algorithm 2: Outlier elimination algorithm of dynamic noise tracking

Input: Priori dynamic object masks, M and map features f; Output: Inlier and dynamic probability set, F; 1 Obtain priori dynamic object contours using masks M, b; 2 for each feature fi do 3 Calculate the distance from feature fi to segmentation boundary b, $\mathrm{dist}\left(f_i,b\right)=min{\parallel f_i-b\parallel }_2$; 4 Estimate the semantically dynamic segmentation probability of feature fi: $P\left(D_k\left(f_i\right)\mid s_{mask}\right)=\frac{1}{\exp \left(\mathrm{dist}\left(f_i,b_j\right)\right)}$; 5 Calculate the posterior probability $\begin{array}{c}P\left(D_k\left(f_i\right)=1\mid \left(D_k\left(f_i\right)\mid s_{mask}\right)\right)=\frac{P\left(D_{k-1}\left(f_i\right)=1\right)P\left(\left(D_k\left(f_i\right)\mid s_{mask}\right)\mid D_{k-1}\left(f_i\right)=1\right)}{P\left(D_k\left(f_i\right)\mid s_{mask}\right)}\end{array}$; 6 using the observation probability $\left(D_k\left(f_i\right)\mid s_{mask}\right)$ and the prior probability $P\left(D_{k-1}\left(f_i\right)=1\right)$, update the current movement probability $P\left(D_k\left(f_i\right)=1\right)$; 7 if $P\left(D_k\left(f_i\right)=1\right)$ then 8 Append fi to F; 9  end if 10 end for

3.5. Optimize Error Function Construction

Let us assume a keyframe’s keypoint u_i and a map point P_i are the same points. The map point measurement error E_points refers to the difference between the keypoint u_i in the current image and the corresponding point projected from the map point P_i to the image. According to the standard feature point-based SLAM three-dimensional point re-shooting error [30], the point measurement error is:

$$\begin{array}{c}{E}_{\mathrm{points}}={\displaystyle {\displaystyle \sum }_{k\in K}}{\displaystyle {\displaystyle \sum }_{\left(u_i,P_i\right)\in X_p^k}}{\left\{u_i-\pi \left(T_{cw}\left(P_i\right)\right)\right\}}^T{\Sigma }^{-1}\left\{u_i-\pi \left(T_{cw}\left(P_i\right)\right)\right\}\\ \end{array}$$

(22)

Among them, $X_p^k$ represents the set of all keypoint matches in the kth keyframe, $\pi$ is the projection transformation, and $T_{cw}$ is the transformation matrix from the world coordinate system to the camera coordinate system. According to the projection equation of Formulas (8) and (9), the projection of the space straight line on the image plane is l′, then:

$${\mathit{l}}^{\prime }={\mathit{K}}^{\prime }{\mathit{n}}_c=\left[\begin{array}{c}{l}_1\\ l_2\\ l_3\end{array}\right]$$

(23)

Therefore, the reprojection error ${\mathit{e}}_l$ between the matching line segment l and the projected line segment l′ in the image is expressed as:

$${\mathit{E}}_{lines}={\displaystyle \sum }_{k\in K}{\displaystyle \sum }_{\left(l_i,{{\mathit{l}}^{\prime }}_i\right)\in Y_l^k}d\left({\mathit{l}}_{\mathit{i}},{{\mathit{l}}_{\mathit{i}}}^{\prime }\right)={\displaystyle \sum }_{k\in K}{\displaystyle \sum }_{\left(l_i,{{\mathit{l}}^{\prime }}_i\right)\in Y_l^k}{\left[\frac{{{\mathit{s}}^{\mathrm{T}}}_{\mathit{i}}{{\mathit{l}}^{\prime }}_i}{\sqrt{l_1^2+l_2^2}},\frac{{{\mathit{e}}^{\mathrm{T}}}_{\mathit{i}}{{\mathit{l}}^{\prime }}_i}{\sqrt{l_1^2+l_2^2}}\right]}^{\mathrm{T}}$$

(24)

where, ${\mathit{s}}_{\mathit{i}}\left(u_s,v_s\right)$ and ${\mathit{e}}_{\mathit{i}}\left(u_e,v_e\right)$ represent the starting point and the ending point of the line segment l_i in the image plane, respectively.

The objective function of the corresponding point line feature is as follows:

$$E_{sum}=\underset{\mathit{E}}{\mathrm{argmin}}{\displaystyle \sum }_{i\in K}\left[{\displaystyle \sum }_{j\in P}{e}_{ij}^{\top }{\Sigma }_{e_{ij}}^{-1}{e}_{ij}+{\displaystyle \sum }_{k\in L}{e}_{ik}^{\top }{\Sigma }_{e_{ik}}^{-1}{e}_{ik}\right]$$

(25)

Among them, ${\Sigma }_{e_{ij}}^{-1}$ and ${\Sigma }_{e_{ik}}^{-1}$ are information matrices.

4. Experimental Results

To verify the proposed PLDS-SLAM system, in this section, we present some experimental details. To test and analyze the proposed PLDS-SLAM, we conduct experimental tests using an Intel i9-7900X CPU @ 3.30GHz×20, 64GB RAM, and 2080TIGPU. The proposed PLDS-SLAM algorithm is compared with the state-of-the-art ORB-SLAM2 [5] and PL-SLAM [13]. In the HPSequences dataset [31] and the KITTI stereo dataset [32], which provides challenging dynamic environment image sequences, comparative experiments on line segment matching, object static geometric constraints, dynamic object feature tracking, and trajectory measurement are carried out. Additionally, we use the EVO [33] evaluation tool to evaluate the absolute trajectory error between the estimated value of the SLAM system and the real value given by the data set. Through experimental comparison, the effectiveness of our proposed method is demonstrated.

4.1. Line Feature Matching Geometry Constraint Improvements

In this work, the comparison experiments of improved line matching and unimproved line matching are performed to determine the effects on the lighting and viewing angle. The comparison experiment results are shown in Figure 4. From the experimental results, compared with the matching results without geometric constraints, it can be seen that the line-matching results after geometric constraints effectively filter out the wrong matches. The effectiveness of the proposed method in line matching is demonstrated. We have added a table to compare the total matched line segments, reprojection error of k-nearest neighbor matching, and reprojection error results of our proposed method, as shown in

Table 1. Outlier removal experiments of line segments.

Frame	01	02	03	04	05	06	07	08	09
Total Lines	50	547	363	419	358	490	807	405	682
Projection Errors of K-Nearest Neighbors	77.57	16.78	63.34	75.39	57.23	62.62	50.45	59.50	50.56
Projection Errors of Ours	1.16	1.06	1.25	0.95	1.46	1.06	1.16	0.87	1.97

. The bold font in the table represents the optimal result. From the experimental results in Table 1, we can see that our proposed method can effectively remove outliers in line segments, reduce the reprojection error of matching, and thus improve the accuracy and precision of line segment matching.

In addition, we also compare the point-line feature maps before and after geometric constraints on the KITTI dataset. As shown in Figure 5, in the comparison results of Figure 5a–d, such as the local detail map marked by the ellipse in the figure, it can be seen that geometric constraints can effectively filter out some erroneous line segment map features. The proposed method in filtering out-of-line segments can be concluded based on the experimental results.

4.2. Dynamic Noise Removal Experiment

In this part of the experiment, we first use mask-RCNN to segment the input image to determine the priority of the dynamic object, obtain the contour boundary of the prior dynamic object, and we perform Bayesian dynamic feature tracking on the point line features near the boundary, which is known as dynamic tracking. The comparison results before and after are shown in Figure 6. Figure 6a is the object segmentation masks, where the pink area is the prior dynamic object segmentation result. Figure 6b shows the experimental results of point-line feature detection without tracking the edge of the dynamic object, where the red point and line is the feature on the prior dynamic object, and the green point and line is the feature of the non-dynamic prior area. Figure 6c is the result of tracking the edge point and line features of the dynamic object. From the comparison results of the point-line features of the object edge in Figure 6b,c, it can be seen that the classification of the edge dynamic point-line features after dynamic probability tracking is more accurate. In addition, as shown in Figure 7a,b, we counted the number of points and line features and the number of dynamic noise points and line features, respectively. The experimental results demonstrate that our proposed method can effectively remove dynamic point-line feature noise; the experimental results also demonstrate the effectiveness of the method in line matching.

4.3. Dynamic and Static Object Feature Separation

In this section, we conduct experiments on the separation of dynamic object features and static object features. As shown in Figure 8, the red point and line feature are the separated dynamic object features, and the blue point and line are the separated static object features. The experimental results show that the proposed method effectively separates the highly dynamic car object features and the static car object features, which proves the effectiveness of the proposed method.

4.4. Dynamic Environment Trajectory Accuracy Verification Experiment

In this part, we compared the proposed method with the excellent feature point-based ORB-SLAM2, ORB-SLAM3 and the excellent point-line feature-based PL-SLAM on the KITTI dataset. To demonstrate the effectiveness of our proposed method on dynamic datasets, we conducted comparative experiments on sequence 03, sequence 04, and sequence 10 of the KITTI dataset, which contain typical dynamic scenes. As shown in Figure 9a–f from the global map and the large map that our proposed method has the smallest error with the ground truth distance compared with ORB-SLAM2, ORB-SLAM3, and PL-SLAM. This highlights the superior performance of our proposed method on dynamic datasets.

We conducted comparative experiments on the absolute trajectory errors of our proposed method and the ORB-SLAM2, ORB-SLAM3, and PL-SLAM algorithms on the dynamic dataset.

Table 2. Absolute trajectory error comparison on the KITTI dataset.

Dataset	Ours	ORB-SLAM2	ORB-SLAM3	PL-SLAM
Sequence00	3.578	1.697	1.994	2.551
Sequence01	2.310	3.268	8.847	2.423
Sequence02	3.799	3.679	3.601	6.635
Sequence03	2.740	2.900	3.323	4.410
Sequence04	1.180	1.260	1.692	2.010
Sequence05	2.888	1.732	1.961	2.572
Sequence06	2.226	1.959	2.165	6.491
Sequence07	1.579	0.907	1.101	2.211
Sequence08	3.948	3.350	3.075	3.317
Sequence09	3.087	4.219	3.411	4.023
Sequence10	2.240	2.290	2.242	3.190

shows the absolute trajectory accuracy errors of these algorithms, where the bold ones are the optimal results. Based on the experimental results, it can be seen that the accuracy of our proposed method is better than that of ORB-SLAM2, ORB-SLAM3, and PL-SLAM methods on dynamic datasets.

5. Conclusions

In this paper, we explore the multi-element coupling mechanism of semantics and multi-view geometric features in a dynamic environment and propose a new stereo point-line SLAM system based on a dynamic environment. Our method obtains a priori dynamic region features by detecting and segmenting the dynamic region. It also proposes a geometric constraint method for matching line segments, and it combines the pole-constrained method of point features to separate dynamic and static objects. The dynamic feature tracking method based on Bayesian eliminates outliers to improve the robustness and accuracy of the SLAM system. The effectiveness of our proposed method in dynamic scenes is demonstrated by comparative experiments on the KITTI dataset and HPSequences dataset.