D-VINS: Dynamic Adaptive Visual–Inertial SLAM with IMU Prior and Semantic Constraints in Dynamic Scenes

Abstract

Visual–inertial SLAM algorithms empower robots to autonomously explore and navigate unknown scenes. However, most existing SLAM systems heavily rely on the assumption of static environments, making them ineffective when confronted with dynamic objects in the real world. To enhance the robustness and localization accuracy of SLAM systems in dynamic scenes, this paper introduces a visual–inertial SLAM framework that integrates semantic and geometric information, called D-VINS. This paper begins by presenting a method for dynamic object classification based on the current motion state of features, enabling the identification of temporary static features within the environment. Subsequently, a feature dynamic check module is devised, which utilizes inertial measurement unit (IMU) prior information and geometric constraints from adjacent frames to calculate dynamic factors. This module also validates the classification outcomes of the temporary static features. Finally, a dynamic adaptive bundle adjustment module is developed, utilizing the dynamic factors of the features to adjust their weights during the nonlinear optimization process. The proposed methodology is evaluated using both public datasets and a dataset created specifically for this study. The experimental results demonstrate that D-VINS stands as one of the most real-time, accurate, and robust systems for dynamic scenes, showcasing its effectiveness in challenging real-world scenes.

1. Introduction

Simultaneous localization and mapping (SLAM) [1] is a crucial technology for advanced robotics applications, enabling collision-free navigation and environment exploration [2]. SLAM heavily relies on the sensors carried by robots to simultaneously achieve high-precision localization and environment mapping. Visual SLAM (VSLAM) [3,4] utilizes cameras to estimate the robot’s position, offering several advantages such as cost-effectiveness, lower energy consumption, and reduced computational requirements. Over the last decade, the VSLAM framework has witnessed rapid development, with notable frameworks such as SOFT2 [5], VINS-Mono [6], ORB-SLAM3 [7], and DM-VIO [8]. Most of these algorithms employ optimization-based methods to construct epipolar constraints, BA, or minimum photometric constraints with features in the environment. VINS-Fusion [9] leverages optical flow to track feature points in the front end and optimizes the minimum reprojection error to solve the poses with BA in the back end. ORB-SLAM2 [10] uses ORB feature points to improve tracking and incorporates a loop closure thread to achieve higher accuracy in global pose estimation. Building upon ORB-SLAM2, ORB-SLAM3 integrates an IMU to enhance the system robustness and stands as one of the most advanced VSLAM solutions to date.

The essence of SLAM pose estimation lies in the robot’s perception of its relative movement in the environment. In terms of the localization aspect of SLAM, the accuracy of pose estimation is greatly affected by the proportion of dynamic feature points being tracked in the field of view. When the proportion of dynamic feature points is relatively small, non-dynamic SLAM algorithms can utilize statistical methods like RANSAC [11] to identify and discard these few dynamic points as outliers. However, when dynamic objects occupy more than half or the majority of the field of view, there are limited static feature points available for tracking. This presents a significant challenge that needs to be addressed using dynamic SLAM algorithms. In such cases, the accuracy of SLAM pose estimation significantly decreases and can even lead to failure, especially for feature-based VSLAM approaches [5,6,7,8]. Consequently, these open-source algorithms often experience a loss in accuracy or even failure when deployed in dynamic environments such as city streets or rural roads with numerous dynamic objects.

To improve the system accuracy and robustness in highly occluded environments, we extend the work of VINS-Fusion and propose a robust dynamic VSLAM framework, called D-VINS. D-VINS proposes a perceptual classification algorithm with missed detection compensation and assigns different weights to feature points during BA based on their geometric constraints. The main contributions of this paper are summarized as follows:

• A feature classification using the YOLOV5 [12] object detection algorithm is proposed in the front end, which divides dynamic feature points into three categories: absolute static points, absolute dynamic points, and temporary static points. Then, the dynamic factors of temporary static features are calculated based on the IMU pre-integration prior constraint and the epipolar constraint. The temporary static features are classified again according to the dynamic factors.
• A robust BA optimization method based on the dynamics factor is proposed in the back end. If the object is more dynamic, its feature weights are decreased, and vice versa.
• Extensive experiments are carried out on public datasets like TUM, KITTI, and VIODE, and our dataset. These datasets include extensive occlusion scenes, and the results from these datasets are representative. The experiment results demonstrate the accuracy and robustness of our proposed D-VINS.

The rest of this paper is organized as follows: Section 2 discusses related work. Section 3 introduces our proposed system in detail. Section 4 details the experimental process and comparative analysis of the experimental results. Finally, the conclusion is presented in Section 5.

2. Related Work

Dynamic SLAM methods can be classified into two categories: geometry-based methods and semantic-based methods. Geometry-based methods rely on the geometric constraints derived from the inter-frame camera motion, with a potential decrease in the segmentation accuracy. Semantic-based methods leverage deep-learning networks to achieve the highly precise segmentation of known objects. However, the implementation of these methods often requires high-performance computing platforms.

2.1. Geometry-Based Dynamic SLAM

Geometry-based methods rely on geometric constraints between camera frames to eliminate outliers. Dynamic objects can be identified as they deviate from the geometric motion consistency observed between frames. Additionally, statistical analysis allows for the differentiation of inner points (static points) from outliers (dynamic points). Most SLAM systems, like VINS-Mono, use RANSAC with epipolar constraints to remove outliers by calculating the fundamental matrix using the eight-point method. However, RANSAC becomes less effective when outliers dominate the dataset.

DGS-SLAM [13] presents an RGB-D SLAM approach specifically designed for dynamic environments. It addresses outlier impacts during optimization by introducing new robust kernel functions.

DynaVINS [14] introduces a novel loss function that incorporates IMU pre-integration results as priors in BA. In the loop closure detection module, loops from different features are grouped for selective optimization.

PFD-SLAM [15] utilizes GMS (grid-based motion statistics) [16] to ensure accurate matching with RANSAC. Subsequently, it calculates the homography transformation to extract the dynamic region, which is accurately determined using particle filtering.

ClusterSLAM [17] clusters feature points based on motion consistency to reject dynamic objects.

In general, geometry-based methods offer higher accuracy and lower computational costs compared to semantic-based methods. However, they lack the semantic information required for precise segmentation. Moreover, geometry-based methods heavily rely on experience-based hyperparameters, which can significantly limit algorithm feasibility.

2.2. Semantic-Based Dynamic SLAM

Currently, deep-learning networks have achieved remarkable advancements in speed and accuracy in various computer vision tasks, including object detection, semantic segmentation, and optical flow. These networks can provide object detection results, such as bounding boxes, which can be utilized in dynamic SLAM systems. To accurately detect dynamic objects, deep-learning-based methods often incorporate geometric information to capture the real motion state in the current frame.

For example, DynaSLAM [18] is an early dynamic SLAM system that combines multi-view geometry with deep learning. It utilizes MASK R-CNN, which offers pixel-level semantic priors for potential dynamic objects in images.

Dynamic-SLAM [19] detects dynamic objects using the SSD (single shot multi-box detector) [20] object detection network and addresses missed detections by employing a constant velocity motion model. Moreover, it sets a threshold for the average parallax of features within the bounding box area to further reject dynamic features. However, this method’s reliance on bounding boxes may incorrectly reject static feature points belonging to the background.

DS-SLAM [21] employs the SegNet network to eliminate dynamic object features, which are then tracked using the Lucas–Kanade (LK) optical flow algorithm [22]. The fundamental matrix is calculated using RANSAC. The distance between the matched points and their epipolar line is computed, and, if the distance exceeds a certain threshold, the point is considered dynamic and subsequently removed. Additionally, depth information from RGB-D cameras is often employed for dynamic object detection.

RS-SLAM [23] detects dynamic features with semantic segmentation, and a Bayesian update method based on the previous segmentation results is used to refine the current coarse segmentation. It also utilizes depth images to compute the Euclidean distance between such two movable regions.

Dynamic-VINS [24] proposes an RGB-D-based visual–inertial odometry approach specifically designed for embedded platforms. It reduces the computational burden by employing grid-based feature detection algorithms. Semantic labels and the depth information of dynamic features are combined to separate the foreground and background. A moving consistency check based on IMU pre-integration is introduced to address missed detection issues.

YOLO-SLAM [25] is an RGB-D SLAM system that obtains an object’s semantic labels using Darknet19-YOLOv3. The drawback is that it cannot be run in real time.

SG-SLAM [26] is a real-time RGB-D SLAM system that adds a dynamic object detection thread and semantic mapping thread based on ORB-SLAM2 for creating global static 3D reconstruction maps.

In general, geometry-based methods offer faster processing times but lack semantic information. In contrast, deep-learning-based methods excel in dynamic object detection by detecting potential dynamic objects with semantic information. However, it is challenging to run deep-learning algorithms in real time on embedded platforms. Their accuracy heavily relies on the training results. Moreover, most of these methods use RGB-D cameras, which tightly couple geometric and depth information, making them more suitable for indoor environments. Few algorithms are specifically designed for outdoor dynamic scenes.

Although current dynamic SLAM methods can detect dynamic objects, they often oversimplify their treatment, limited to either discarding or preserving them. Additionally, challenges arise when the camera is heavily obscured by moving objects. To address these limitations, we propose a dynamic SLAM method that integrates both geometric and semantic information, tightly incorporating IMU prior constraints into the dynamic feature check and optimization process.

3. Methods

The system is implemented based on VINS-Fusion [9], an optimization-based estimation framework that supports multiple sensors, including stereo, monocular, and IMU sensors. The system initialization follows a loosely coupled approach, where Harris corners are extracted in the front end and tracked using the LK optical flow. In the back end, the system state is marginalized and optimized using a sliding window approach. Figure 1 illustrates the system workflow. For object detection, we utilize the Common Objects in COntext (COCO) dataset, which comprises 80 object categories commonly found in daily life, allowing algorithms trained on this dataset to recognize the majority of objects in everyday scenes.

In dynamic object classification, the color image is initially processed by the fast convolutional neural network You Only Look Once (YOLO) [12], which provides semantic labels of the COCO dataset [27] and bounding boxes of objects. The Harris [28] keypoints are then extracted, and only those points from absolute static objects and temporary static objects are tracked using the LK optical flow in the front end. The IMU sensor provides feature states, like translation, rotation, and velocity with prior motion constraints. The feature’s dynamic factor is determined by the root mean square of the IMU pre-integration error and epipolar constraint. During the feature dynamic check, the dynamic factors of the temporary static points are calculated, indicating the movement of the feature points. A larger numerical value suggests that the feature points are likely to be dynamic. To preserve potential points in dynamic scenes, the feature rejection strategy focuses on retaining points identified by dynamic factors rather than removing all movable points.

In the back end, we propose a novel loss function with adaptive weights based on dynamic factors in BA, a process used to refine camera poses and spatial point positions. The typical approach involves minimizing the reprojection error, which quantifies the difference between landmarks and their observed pixels in the image. To optimize the cost function, feature weights are incorporated as parameters. D-VINS divides the conventional optimization process into two steps. Firstly, the feature weights are fixed to independently optimize the system states. Then, the system states are fixed to optimize the weights. This process is iterated until the required number of iterations is reached, or until the weights converge. Additionally, dynamic factors are introduced to adjust the feature weights.

3.1. Dynamic Object Classification

In dynamic environments, tracking static features is crucial for SLAM systems to maintain localization accuracy. However, deep-learning models struggle to clearly define the boundary between dynamic and static objects. For instance, while books are typically considered static objects, they become dynamic when a person holds them, similar to cars in a parking lot or on a highway. Therefore, relying solely on semantic information is insufficient for robots to detect moving objects in such environments.

3.1.1. Semantic Label Incremental Updating with Bayes’ Rule

To recognize a wide range of objects, we utilized the COCO dataset to train the YOLOV5 model, which includes 80 common object categories. However, not all objects need to be detected, so we focused on the 17 most common categories. The process involves inputting color images to YOLOV5, accelerated with TensorRT [29], accelerating to obtain semantic labels of the COCO categories. Bounding boxes help approximate the region of dynamic objects in the images, and feature points within these boxes are assigned semantic labels. As the object detection network can sometimes have missing or incorrect detections, D-VINS employs Bayes’ rule to update the feature’s semantic label and mitigate errors in specific frames. This approach transforms the labeling problem into a maximum posterior problem, enhancing the accuracy of semantic labeling.

The $m$th map point in the given world co-ordinates $W$ observed in the $k$th frame can be written as ${\mathit{P}}_k^W\left(x,y,z,1\right)$.${\mathit{p}}_m^k\left(u,v,1\right)$ denotes the pixels in camera co-ordinates that correspond to the $m$th map point. The projection process of the feature points is as follows:

$$s{\mathit{p}}_m^k=\mathit{K}\mathit{T}{\mathit{P}}_k^W$$

(1)

where$s$is the depth of the map points, $\mathit{K}$is the extrinsic matrix, and $\mathit{T}$denotes the transformation matrix from the world co-ordinates to the observation frame. $l_{true}=\left\{l_i^k|k\in \left(1,2,3,\dots ,N\right),i\in \left(1,2,3,\dots ,\mathrm{M}\right)\right\}$ and $b_{observe}=\left\{b_i^k|k\in \left(1,2,3,\dots ,N\right),i\in \left(1,2,3,\dots ,\mathrm{M}\right)\right\}$ denote the ground truth of the semantic labels of the $m$th feature points from the beginning frame to the $k$th frame and the measurements of the deep-learning bounding boxes. According to Bayes’ rule, there are

$$P\left(l_{true}|b_{observe}\right)=\frac{P\left(b_{observe}|l_{true}\right)P\left(l_{true}\right)}{P\left(b_{observe}\right)}\propto P\left(b_{observe}|l_{true}\right)P\left(l_{true}\right)$$

(2)

This is a maximum posterior problem, shown as

$$arg maxP\left(l_{true}|b_{observe}\right)=arg maxP\left(b_{observe}|l_{true}\right)\times P\left(l_{true}\right)$$

(3)

The semantic labels of feature points in the current frame can be influenced by dynamic objects detected in previous frames. Consequently, the semantic label probability distribution of the $m$th feature point in the $k$th frame is affected by the multi-frame history.

$$P\left(l_k^m|b_{1:k}\right)={\displaystyle \prod }_{\mathrm{i}=1}^{\mathrm{k}}P\left(l_i^m|l_{i-1}^m,b_i\right)$$

(4)

Upon obtaining the semantic information of the current frame, D-VINS checks its consistency with the previous frames. If the previous semantic label matches the one in the current frame, the detection result of the current frame is considered more reliable, and vice versa. The detailed algorithm steps are presented in Algorithm 1.

Algorithm 1: Semantic label updating with Bayesian rule

Input: Current frame bounding box $A_{mask}$; current frame’s feature points $P$; previous frame’s dynamic label $l_{k-1}$; non-updated current frame’s dynamic label$b_k$; threshold of the dynamic label${\epsilon }_{yolo}$; frequency of feature point being observed$N_{observe}$ Output: Current frame’s dynamic label$l_k$.

1: for each $P_j$ in this Frame $P$ do: 2:         for each bounding box in this Frame do: 3:                if (InThisBoundingBox($P_j$,$A_{mask}$)) && ($b_k\ge {\epsilon }_{yolo}$)) then, 4:                    $N_{observe}$++; 5:                   $ratio=\frac{\left|l_{k-1}-b_k\right|}{l_{k-1}}$; 6:                   $l_k^{temp}=\frac{N_{observe}}{ratio\times size\left(P\right)}\times b_k+\left(1-\frac{N_{observe}}{size\left(P\right)}\right)\left(1-\frac{1}{ratio}\right)\times l_{k-1}$; 7:                 end if 8:        end for 9:       $l_k=\max (l_k^{temp})$; 10:     end for

3.1.2. Feature Point Motion State Classification

The feature point classification is divided into two parts. The first part is to classify the COCO categories according to the possibility of motion based on life experience. The classification is divided into five levels (I to V), with higher levels indicating a higher possibility of movement. Level I includes public facilities (e.g., traffic light and bench), Level II includes furniture (e.g., chair, sofa, and bed), Level III includes transportation (e.g., bicycle, car, motorbike, bus, truck, and boat), Level IV includes sports (e.g., football and basketball) and people (e.g., person), and Level V includes animals (e.g., cat, dog, and bird), as depicted in Figure 2.

The second part involves dividing feature points into three categories based on their movement in the current frame: absolute stationary points, temporary stationary points, and absolute dynamic points.

Generally, if an object is detected as a bench, sofa, or potted plant, its feature points are likely to be static, making them suitable for pose estimation and mapping. If the semantic label indicates animals such as birds, cats, or dogs, these features are considered dynamic. Animals typically move continuously and occupy a small area in an image, exerting less impact on the SLAM system. As shown in Figure 2, objects of Level I and Level II are classified as absolute static objects, objects of Level III and Level IV are temporary static objects, and objects of Level V are absolute dynamic objects.

3.2. Feature Dynamic Check with IMU Prior and Epipolar Constraints

To check the current motion state of movable objects, D-VINS calculates dynamic factors to find the absolute dynamic points. For temporary static points, the dynamic factors are determined in two steps. Firstly, the IMU pre-integration calculates the initial pose of the current frame, and the reprojection error is obtained by projecting the 3D feature points onto the image plane. Secondly, the foundational matrix is calculated based on epipolar geometry to determine the distance from the feature points to their respective epipolar lines, constituting the second part of the dynamic factor.

If the dynamic factor of a feature point falls below a certain threshold, the point is labeled as an absolute dynamic point.

3.2.1. Dynamic Factor of Reprojection Error Based on IMU Prior Constraint

Conventional visual reprojection involves projecting a feature point from its previous observed frame onto the pixel plane of the current frame. However, the reprojection error cannot be calculated if the camera pose of the current frame is unknown. To address this limitation, IMU pre-integration offers an initial estimate for the current frame’s camera pose, enabling the calculation of the reprojection error and the rejection of dynamic objects using the IMU sensor.

There must be errors between the estimation pose and the real camera pose, which means the reprojection points and the observation points are usually not coincident. For the map point in the world co-ordinates $\mathit{P}={\left[X_p,Y_p,Z_p,1\right]}^T$, its pixel co-ordinates projected onto the $j$th frame are ${\mathit{p}}^{C_j}={\left[u^j,v^j,1\right]}^{\mathrm{T}}$. According to Equation (1), a relationship between the map points and pixel points according to the camera projection model exists, as follows:

$$s\left[\begin{array}{c}{u}^j\\ v^j\\ 1\end{array}\right]=\mathit{K}\exp \left({\mathsf{\xi }}^{\wedge }\right)\left[\begin{array}{c}{X}_p\\ Y_p\\ Z_p\\ 1\end{array}\right]$$

(5)

Equation (5) can be written in matrix form, ${\mathit{p}}^{C_j}=\mathit{K}\exp \left({\mathsf{\xi }}^{\wedge }\right)\mathit{P}$, and the residual ${\mathit{r}}_C$ of the reprojection error is as follows:

$${\mathit{r}}_C={\Vert {\mathit{p}}^{C_j}-\frac{1}{s}\mathit{K}\exp \left({\mathsf{\xi }}^{\wedge }\right)\mathit{P}\Vert }_2^2$$

(6)

where ${\mathsf{\xi }}^{\wedge }$ is the Lie algebra of the $j$th frame in the body frame, and $\mathit{K}$ is the intrinsic matrix obtained via camera calibration [30]. The camera poses of the$j$th frame are obtained via IMU pre-integration:

$$\begin{array}{c}{R}_w^{b_{j-1}}{p}_{b_j}^w=R_w^{b_{j-1}}\left(p_{b_{j-1}}^w+v_{b_{j-1}}^w\mathsf{\Delta }\mathrm{t}-\frac{1}{2}{g}^w{\mathsf{\Delta }\mathrm{t}}^2\right)+{\mathsf{\alpha }}_{b_j}^{b_{j-1}}\\ R_w^{b_{j-1}}{v}_{b_j}^w=R_w^{b_{j-1}}\left(v_{b_{j-1}}^w-g^w\mathsf{\Delta }\mathrm{t}\right)+{\mathsf{\beta }}_{b_j}^{b_{j-1}}\\ q_w^{b_{j-1}}\otimes q_{b_j}^w\&={\mathsf{\gamma }}_{b_j}^{b_{j-1}}\end{array}$$

(7)

where ${\mathsf{\alpha }}_{b_j}^{b_{j-1}}$, ${\mathsf{\beta }}_{b_j}^{b_{j-1}}$, and ${\mathsf{\gamma }}_{b_j}^{b_{j-1}}$ are the pre-integration terms of position, velocity, and pose, which change the reference frame from the world frame to the local body frame$b_{j-1}$. $p_{b_j}^w$, $v_{b_j}^w$, and $q_{b_j}^w$ are the system states of the$j$th body frame. From Equation (7), the pose of the jth frame is obtained. The pixel co-ordinates in the jth frame projected from the ith frame are ${\mathit{P}}^{C_j}={\left[u_i^j,v_i^j,1\right]}^T$. And the observation in the$j$th frame is ${\overline{\mathit{P}}}^{C_j}={\left[{\widehat{u}}_i^j,{\widehat{v}}_i^j,1\right]}^T$. Through Equation (6), the new visual reprojection residual$r_{project}\left(\mathit{P}\right)$of the map point $\mathit{P}$ can be established using the camera projection model:

$$\left\{\begin{array}{c}{\mathit{P}}^{C_j}=\pi \left(T_b^c{T}_w^{b_j}{T}_{b_i}^w{T}_c^b{\mathit{P}}^{C_j}\right)\\ {\mathit{P}}^{C_i}=\pi \left(T_b^c{T}_w^{b_i}{T}_{b_j}^w{T}_c^b{\mathit{P}}^{C_i}\right)\\ r_{\mathit{P}roject}\left(\mathit{P}\right)=\Vert {\overline{\mathit{P}}}^{C_j}-{\mathit{P}}^{C_j}{\Vert }_2^2+\Vert {\overline{\mathit{P}}}^{C_i}-{\mathit{P}}^{C_i}{\Vert }_2^2\end{array}\right.$$

(8)

where $T_b^c$ is the transformation matrix from the body frame to the camera frame, which is obtained using Kalibr [31].$T_w^{b_j}$ and $T_{b_i}^w$ represent the transformation matrix between IMU frames and the world co-ordinates.${\mathit{p}}_{b_i}^w$ and ${\mathit{p}}_{b_i}^w$ represent the translation matrix between the body frame and world frame.$\pi \left(·\right)$ represents the pinhole camera projection model.

As shown in Figure 3, the red distance represents the dynamic factor of the IMU reprojection error. It shows the observation and projection of the static map point P and the dynamic point P′ in two camera frames. O denotes the camera’s optical center. $x_1$ and $x_4$ are feature points matched for the two frames with optical flow. $x_2$ is the feature point projected by the static point P in the $j$th camera frame. $x_3$ is the feature point projected by the dynamic point P′ in the (j − 1)th camera frame.

Generally, determining the dynamics of an object through feature points’ reprojection error is effective. However, this method may fail when the dynamic object is moving along the camera’s optical center, either towards or away from the camera, as shown in Figure 3. In such cases, the reprojection error becomes close to 0, even if point P is not a dynamic point. To address this limitation, we propose an additional reprojection error on the previous frame, alongside the conventional visual reprojection in Equation (8). By incorporating both reprojection processes, the method becomes complementary, minimizing the impact on the dynamic judgment of feature points with special object motion directions. Consequently, the first part of the dynamic factors ${\lambda }_p$is obtained as shown below:

$${\lambda }_p=r_{project}\left(\mathit{P}\right)$$

(9)

3.2.2. Dynamic Factor of Epipolar Constraints

An epipolar constraint is a critical property that restricts the position of feature points, commonly used in various SLAM systems to accelerate the matching process in the front end. In D-VINS, data association between feature points is achieved using the pyramidal iterative Lucas–Kanade optical flow. Subsequently, the fundamental matrix between two camera frames is computed using the seven-point method based on RANSAC.

Using the fundamental matrix, the epipolar lines of feature points in the current frame are calculated. The distance from a point to its epipolar line is defined as the second part of the dynamic factor. This distance is then utilized to determine whether the point is dynamic or not. According to the pinhole camera model, the map point P is observed in different camera frames, which is shown in Figure 4. x₁ and x₃ are matched feature points in different frames, and x₂ is the feature point projected to the$j$th frame by the map point P. The short dashed lines I and I’ are the epipolar lines of the two frames.

${\mathit{x}}_1=\left[u_1,v_1,1\right], {\mathit{x}}_2=\left[u_2,v_2,1\right]$ are the homogeneous co-ordinate forms of the two matched feature points, belonging to the (j − 1)th frame and$j$th frame, respectively. Then, the epipolar line${\mathit{I}}^{\prime }$ of ${\mathit{x}}_2$ in the jth frame is as follows:

$${\mathit{I}}^{\prime }=\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\mathit{F}{\mathit{x}}_1{}^T=\mathit{F}\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]$$

(10)

where$X$, $Y$, and$Z$denote the real constants in the general form of a straight line (Xu + Yv + Z = 0), and F denotes the fundamental matrix. Then, for the feature point ${\mathit{x}}_2$, the epipolar constraint is as follows:

$${\mathit{x}}_2\mathit{F}{\mathit{x}}_1{}^T={\mathit{x}}_2{\mathit{I}}^{\prime }=\mathbf{0}$$

(11)

In Figure 4, the distance from the point to the epipolar line is marked by the blue line. For the matched feature point ${\mathit{x}}_i\left(i=2,3\right)$ of ${\mathit{x}}_1$, the residual of the epipolar constraint $r_{epipolar}\left(\mathit{P}\right)$ can be described as follows:

$$r_{epipolar}\left(\mathit{P}\right)=\frac{\left|{\mathit{x}}_i\mathit{F}{\mathit{x}}_{\mathit{1}}{}^T\right|}{\sqrt{{\Vert X\Vert }^2+{\Vert Y\Vert }^2}}$$

(12)

Then, the second part of the dynamic factor is obtained as shown below:

$${\lambda }_e=r_{epipolar}\left(\mathit{P}\right)$$

(13)

For the features of static objects, $r_{epipolar}\left(\mathit{P}\right)$ should be 0 or close to 0. But, for the features of dynamic objects, like P′, there is an offset between the real pixel co-ordinates and their observation. However, when the feature point moves toward the optical center of the (j − 1)th frame, the feature point is still on its epipolar line. Therefore, it is hard to determine whether the object is in motion or not. Therefore, when determining whether features are moving, the two distances of ${\lambda }_p$and${\lambda }_e$ need to be combined.

The threshold ${\epsilon }_{reproject}$ of the reprojection dynamic factor is set to 4 pixels, and the ${\epsilon }_{epipolar}$ of the epipolar dynamic factor is set to 3 pixels. If the errors exceed those thresholds, then the feature points are considered absolute dynamic points and rejected. Then, the feature is marked as ADP. Thus far, the two dynamic factors ${\lambda }_p$ and ${\lambda }_e$ have been obtained.

This method enables a more accurate classification of temporary static objects and finds the dynamic feature points. In addition, the feature points of dynamic objects with small movements can be fully utilized by the SLAM system. The specific algorithm steps are shown in Algorithm 2.

Algorithm 2: Dynamic feature rejection algorithm.

Input: Previous frame $F_i$; current frame$F_j$; previous frame’s feature points $P_l^{C_i}$; current frame’s feature points $P_l^{C_j}$; the threshold of the reprojection dynamic factor${\epsilon }_{reproject}$; the threshold of the epipolar dynamic factor ${\epsilon }_{epipolar}$. Output: Current frame’s feature points’ dynamic factors ${\lambda }_p$and${\lambda }_e$; current frame’s feature points’ dynamic label.

1: for each $P_l^{C_j}$ in this Frame$F_j$ do: 2:         if ($P_l^{C_j}$.dynamics_lable == Temporary Static Point) do: 3:             F_Maxtrix = cv::FindFundamentalMat($F_i$, $F_j$, CV_FM_RANSAC); 4:             $P_l^{C_j}$.A = CalIMUProjectDis($F_i$,$F_j$); 5:             $P_l^{C_j}$.B = CalEpipolarDis($F_i$,$F_j$, F_Maxtrix); 6:               if ((A >${\epsilon }_{reproject}$)&&(B >${\epsilon }_{epipolar}$)) do: 7:                  $P_l^{C_j}$.dynamics_lable = ADP1; 8:              end if 9:   end for

3.3. Dynamic Adaptive Bundle Adjustment

The current conventional bundle adjustment optimization treats all feature points with the same weight. This study designs a novel bundle adjustment optimization algorithm based on the dynamic factor.

3.3.1. Conventional Bundle Adjustment Optimization

In the conventional visual–inertial state estimator, the bundle adjustment optimization equation is as follows:

$$\underset{x}{\mathit{min}}\{{\Vert r_p-H_p\mathcal{X}\Vert }^2+\sum _{k\in \mathcal{B}}{\Vert r_{\mathcal{B}}\left({\widehat{z}}_{b_{k+1}}^{b_k},\mathcal{X}\right)\Vert }_{P_{b_{k+1}}^{b_k}}^2+\sum _{\left(l,j\right)\in \mathcal{C}}\rho \left({\Vert r_{\mathcal{C}}\left({\widehat{z}}_l^{c_j},\mathcal{X}\right)\Vert }_{P_l^{c_j}}^2\right)\}$$

(14)

where ${\rho }_H\left(·\right)$ denotes the Huber kernel function; $r_p$ represents the marginalization residuals; $r_{\mathcal{I}}$ represents the IMU pre-integration residuals; $r_{\mathcal{P}}$ represents the visual reprojection error; $H_p$ represents the marginalization of the measurement state estimation matrix; ${\widehat{z}}_{b_{k+1}}^{b_k}$ represents an IMU observation; ${\widehat{z}}_j^{c_i}$ represents a visual observation; $P_{b_{k+1}}^{b_k}$ denotes the covariance of the IMU measurement; $P_j^{c_i}$ denotes the visual covariance; $\mathcal{B}$ represents the set of all IMU observations; $\mathcal{C}$represents the set of tracked features in the sliding window; and $\mathcal{X}$ denotes the estimated states to be optimized.

The traditional bundle adjustment formulation cannot reject or adjust the weights of dynamic feature points. Eliminating all temporary static points would result in insufficient visual observations for optimization, leading to unstable or erroneous BA optimization results. Thus, a more robust approach to BA optimization needs to be implemented.

3.3.2. Dynamic Adaptive Cost Function with Dynamic Factors

The novel cost function introduced in this study possesses two main features. Firstly, it facilitates the rejection of dynamic features. Secondly, it adjusts the weights of feature points during optimization based on the dynamic factors. Drawing inspiration from DynaVINS, we propose the form of the dynamic adaptive loss function as follows:

$${\rho }_D\left({\lambda }_p,{\lambda }_e,W_j,{\mathrm{L}}_j,r_{\mathcal{P}}^j\right)=\left(1-L_j\right)\times \left[W_j{}^2{\rho }_H\left(\lambda \right)+{\left(1-W_j\right)}^2{\lambda }_w\right]+L_j\times {\rho }_H\left(r_{\mathcal{P}}^j\right)$$

(15)

$${\rho }_D\left({\lambda }_p,{\lambda }_e,W_j,{\mathrm{L}}_j,r_{\mathcal{P}}^j\right)=\left\{\begin{array}{c}{W}_j{}^2{\rho }_H\left(\lambda \right)+{\left(1-W_j\right)}^2{\lambda }_w,{\mathrm{L}}_j=1\\ {\rho }_H\left(r_{\mathcal{P}}^j\right),{\mathrm{L}}_j=0\end{array}\right.$$

(16)

$${\lambda }_w=\frac{1}{\sqrt{\frac{{\lambda }_p{}^2+{\lambda }_e{}^2}{2}}}\in \left[1,12\right]$$

(17)

where ${\lambda }_p$ and${\lambda }_e$ denote two dynamic factors of a feature point in the frame $j$; $W_j\in \left[0, 1\right]$ denotes the weights of feature points, where the weight is fixed to 1 for absolute static points; and $L_j\in \left\{0, 1\right\}$ represents the dynamic label, which will be 1 for absolute static points and 0 for temporary static points. Equation (17) presents the dynamic factors. For absolute static points, $L_j$is 1, and the back-end optimization loss function is the same as the conventional one. For temporary static points, the loss function will switch to $W_j{}^2{\rho }_H\left(\lambda \right)+{\left(1-W_j\right)}^2{\lambda }_w$ shown in Equation (16). As the loss function is designed to have a nonlinear quadratic form, the optimal weights $W_j$ can be derived as follows:

$$W_j=\frac{{\lambda }_w}{{\rho }_H\left(\lambda \right)+{\lambda }_w}$$

(18)

After optimizing the weights, the features with higher dynamic factors${\lambda }_w$ will have lower weights. The losses’ gradient of those features will be close to zero, which has no impact on BA, as shown in Figure 5a. The higher the weight assigned to feature points, the steeper the slope of the curve, indicating a greater impact on the nonlinear optimization process.

Different dynamic factors ${\lambda }_w$ affect the gradient value and adjust the BA residuals with marginalization and IMU pre-integration residuals. Equation (16) in quadratic form is designed for nonlinear optimization. As ${\lambda }_w$ increases, the gradient value of the converged loss function decreases, resulting in a smaller weight assigned to the corresponding features during the optimization process, as shown in Figure 5b.

When a feature point moves toward the optical center, the value of the reprojected dynamic factor ${\lambda }_p$ is typically small. However, if the feature point exhibits a larger dynamic factor ${\lambda }_e$ due to epipolar constraints, ${\lambda }_w$ will be smaller, resulting in a smaller weight, as shown in Figure 5c.

Different from DynaVINS, the weight momentum factor does not need the help of semantic labels, and the point weights are delivered with bounding boxes. The total cost function based on the dynamic factors is as follows:

$$\underset{\mathcal{X},W}{min}\left\{\Vert r_p-H_p\mathcal{X}{\Vert }^2+\sum _{k\in \mathcal{B}}\Vert r_{\mathcal{I}}^k{\Vert }^2+\sum _{j\in {\mathbf{F}}_{\mathcal{P}}}{\rho }_D\left(A_j,B_j,W_j,L_j,r_{\mathcal{P}}^j\right)\right\}$$

(19)

The strategy is to increase the weight of absolute static points and decrease the weight of temporary static points in BA optimization, completely discarding absolute dynamic points in the optimization. The weights of feature points are optimized to obtain more robust localization results. In optimizing the current state $\mathcal{X}$, the weights of each feature point are fixed. After that, the current state $\mathcal{X}$ is fixed, and the feature points’ weights $W$are optimized according to the dynamic adaptive cost function, which is as follows:

$$\underset{W_j}{min}\left\{\sum _{j\in F_{\mathcal{P}}}{\rho }_D\left(A_j,B_j,W_j,L_j,r_{\mathcal{P}}^j\right)\right\}$$

(20)

Since the feature points’ weights $W_j$ are independent from each other, the overall loss function is obtained by accumulating the cost equations of each feature point:

$$\underset{W_j\in \left[0,1\right]}{min}\left\{W_j{}^2\left(\sum _{i\in \mathrm{TSP}}{\rho }_H\left(\Vert r_{\mathcal{P}}^{j,i}{\Vert }^2\right)\right)+{\left(1-W_j\left({\lambda }_w\right)\right)}^2{\lambda }_w\right\}$$

(21)

Ultimately, different optimization weights can be used based on different dynamic factors of the feature points.

4. Experimental Results

In this section, we validate the effectiveness of D-VINS by conducting experiments on publicly available datasets, including TUM RGB-D [32], KITTI [33], and VIODE [34]. These datasets offer diverse dynamic scenes for testing the algorithm’s performance.

TUM RGB-D is a widely used dynamic SLAM measurement dataset, containing monocular camera and RGBD information. VIODE is a simulation dataset specifically designed for algorithm performance testing. It is generated using a drone equipped with a stereo camera and IMU. The KITTI dataset is utilized to evaluate the algorithm’s performance in urban vehicle platforms.

In this paper, the root mean square error (RMSE) of the absolute trajectory error (ATE) [35] and the root mean square value of the relative pose error (RPE) were selected as evaluation metrics. The unit of the ATE is m. The unit of translational drift in the RPE is m/s. The ATE is well-suited to measuring the global consistency of the trajectory, while the RPE is well-suited to measuring the translation and rotation drift. They represent the global consistency of the trajectory and the drift of the odometer per unit time, respectively. Given the estimation state ${\mathit{X}}_{i }$ and the ground truth ${\widehat{\mathit{X}}}_i$, ATE-RMSE and RPE-RMSE are calculated as follows:

$${\mathrm{ATE}}_{\mathrm{RMSE}}=\sqrt{\frac{1}{n}{\displaystyle \sum }_{i=1}^n \Vert \mathrm{translation}\left({\widehat{\mathit{X}}}_i\right)-\mathrm{translation}{\left({\mathit{X}}_i\right)\Vert }^2}$$

(22)

$${\mathrm{RPE}}_{\mathrm{RMSE}}=\sqrt{\frac{1}{n}{\displaystyle \sum }_{i=1}^n{ }_{}\Vert \mathrm{translation}{\left({\left({\widehat{\mathit{X}}}_i^{-1}{\widehat{\mathit{X}}}_{i+\mathsf{\Delta }}\right)}^{-1}\left({\mathit{X}}_i^{-1}{\mathit{X}}_{i+\mathsf{\Delta }}\right)\right)\Vert }^2}$$

(23)

where n is the number of frames in the data sequence, and ∆ is a time interval. The experimental results were analyzed both qualitatively and quantitatively. To facilitate the experiments, we integrated D-VINS with ROS, and all tests were conducted on a laptop equipped with 16 GB RAM (CPU: AMD Ryzen7 5800H, made by AMD USA, GPU: NVIDIA GEFORCE RTX 3050TI, made by NVIDIA Corporation USA). We evaluated the improvement of our proposed system, D-VINS, compared to the original algorithms ORB-SLAM2, and ORB-SLAM3. Additionally, we compared D-VINS with the state-of-the-art dynamic VSLAM algorithm, DynaVINS, as well as other similar algorithms such as DS-SLAM, RS-SLAM, and Dynamic-VINS, to further assess its effectiveness.

4.1. TUM RGB-D, VIODE, and KITTI Dataset Evaluation

4.1.1. TUM RGB-D Dataset

The TUM RGB-D dataset was captured using a Microsoft Kinect camera, made by Microsoft USA, at a frame rate of 30 Hz. It consists of 39 image sequences containing both color and depth images. This dataset has become widely used for evaluating visual odometry in dynamic scenes. The ground truth is obtained through a high-precision motion capture system. This dataset provides nine sequences for dynamic scenes, where dynamic objects can be divided into low- and high-dynamic sequences. The low-dynamic sequences are labeled “sitting” (fr3/sitting_static, fr3/sitting_xyz, fr3/sitting_halfsphere, fr3/sitting_rpy); and the high-dynamic sequences are labeled “walking” (fr3/walking_static, fr3/walking_xyz, fr3/walking_halfsphere, fr3/walking_rpy).

In this paper, we used the open-source trajectory evaluation tool Evo (available online: https://github.com/MichaelGrupp/evo (accessed on 25 April 2023)) to visualize the trajectory differences between D-VINS and ORB-SLAM2. In addition, the TUM dataset contains no IMU data, and VINS does not support monocular VO mode, so modules containing IMUs were excluded from D-VINS for the experiment. The data were obtained from the actual experiments on the dataset. There are the hyperparameters used in D-VINS, the number of feature points (150), and the pixel spacing of feature points (25); and the initial value of the dynamic factor of the feature point is ${\lambda }_p=2, {\lambda }_e=2$.

Firstly, Figure 6 shows the plotted trajectories of the two algorithms. The black dashed line represents the ground truth, provided by the dynamic capture. The blue solid line represents the trajectory generated by D-VINS, and the green solid line represents the trajectory generated by ORB-SLAM2 for comparative analysis. The quantitative analysis of this figure fully demonstrates the necessity of dynamic object rejection and the effectiveness of D-VINS. According to Figure 6h, it can be seen that, when dynamic objects appear, the trajectory accuracy is seriously impaired.

Secondly,

Table 1. The ATE- and RPE-RMSE (m) of ORB-SLAM2, ORB-SLAM3, and D-VINS on the TUM RGB-D dataset.

Sequences	ORB-SLAM2		ORB-SLAM3		D-VINS * (Ours)		Improvement
	ATE	RPE	ATE	RPE	ATE	RPE	ATE	RPE
fr3_sitting_static	0.0116	0.0152	0.0097	0.0060	0.0080	0.0114	17.53%	-
fr3_sitting_xyz	0.0133	0.0199	0.0098	0.0086	0.0153	0.0179	-	-
fr3_sitting_halfs	0.0336	0.0124	0.0208	0.0080	0.0252	0.0122	-	-
fr3_walking_static	0.4121	0.0299	0.2450	0.0163	0.0069	0.0101	97.18%	38.04%
fr3_walking_xyz	0.8856	0.1255	0.5617	0.0267	0.0155	0.0182	97.24%	31.84%
fr3_walking_rpy	0.5987	0.0528	0.6841	0.0289	0.0422	0.0432	92.95%	-
fr3_walking_ half	0.4227	0.0338	0.3212	0.0202	0.0216	0.0234	93.27%	-

Note: Bold data indicate the best results. The symbol “*” indicates that D-VINS removed the module containing the IMU. The symbol “-” indicates that the algorithm showed no improvement.

and

Table 2. The ATE- and RPE-RMSE (m) of D-VINS and the compared methods on the TUM RGB-D dataset.

Sequences	DS-SLAM		RS-SLAM		Dynamic-VINS		D-VINS * (Ours)
	ATE	RPE	ATE	RPE	ATE	RPE	ATE	RPE
fr3_walking_static	0.0081	0.0102	0.0067	0.0099	0.0077	0.0095	0.0069	0.0101
fr3_walking_xyz	0.0247	0.0333	0.0146	0.0210	0.0486	0.0578	0.0155	0.0182
fr3_walking_rpy	0.4442	0.1503	0.1869	0.2640	0.0629	0.0595	0.0422	0.0432
fr3_walking_half	0.0303	0.0297	0.0425	0.0609	0.0608	0.0665	0.0216	0.0234

Note: Bold data indicate the best results. The symbol “*” indicates that D-VINS removed the module containing the IMU.

summarize the quantitative experimental results, showing a comparison with several outstanding SLAM algorithms, like ORB-SLAM3 [7], RS-SLAM [23], and Dynamic-VINS [24]. Among these contrasting SLAM methods, ORB-SLAM3 is the most accurate VSLAM open-source algorithm for evaluating the positioning accuracy of D-VINS in non-dynamic environments. RS-SLAM is a robust semantic RGB-D SLAM system that can achieve real-time high-accuracy localization based on semantic segmentation. The horizontal line in (h) represents loopback detection. Dynamic-VINS is an optimization-based RGB-D inertial odometry system providing real-time state estimation results for resource-restricted platforms.

According to Table 1, D-VINS outperformed ORB-SLAM2 in six sequences and ORB-SLAM3 in four dynamic sequences. In the sequences fr3_sitting_xyz and fr3_sitting_halfs, even though D-VINS did not achieve the highest positioning accuracy, it obtained sub-optimal accuracy in the camera absolute pose estimation. ORB-SLAM2 was unable to identify dynamic point features, and its camera pose accuracy was the weakest of all compared SLAM algorithms.

In Table 2, the compared methods’ results are presented, as reported in their original published papers. It is not difficult to find that D-VINS obtained the two best results from the four groups of experiments. In the fr3_walking_static and fr3_walking_xyz sequences, D-VINS obtained sub-optimal accuracy compared with the three other SLAM methods. The reliability of dynamic object recognition and rejection with deep learning and geometric constraints was verified with this dataset. RS-SLAM utilizes PSPNet for pixel-level segmentation and depth information, resulting in a higher accuracy in dynamic feature detection. It provides highly precise localization results when the number of dynamic feature points is limited. Dynamic-VINS just simply disables modules relevant to the IMU. Therefore, its robustness and accuracy were weaker in high-dynamic sequences. DS-SLAM exhibited severe accuracy degradation in fr3_walking_rpy. This is attributed to the non-texture and pure rotational motion, which affect the estimation of epipolar lines and camera poses. Additionally, significant occlusions occur when people pass in front of the camera, as the features’ weights are not adjusted. It should be noted that D-VINS does not incorporate pixel-level segmentation results and relies on bounding boxes, which introduces feature point segmentation errors. However, the advantage of D-VINS lies in its efficient processing speed.

4.1.2. KITTI Dataset

The KITTI dataset provides sequences containing stereo color images in the 00 to 10 urban street and highway environments for evaluating the accuracy of odometry localization. The hyperparameters used in D-VINS are: the number of feature points (150), the pixel spacing of feature points (35), the maximum number of optimizations (5), and the initial value of the dynamism factor of the feature point (${\lambda }_p=2, {\lambda }_e=2$). The parameters for DynaVINS in KITTI are: ${\lambda }_w=1.0, {\lambda }_m=0.2$.

Table 3. The ATE-RMSE (m) of VINS-Fusion, DynaVINS, and D-VINS on the KITTI dataset.

Sequences	VINS-Fusion	DynaVINS	D-VINS
KITTI 05	1.913	12.4668	1.7631
KITTI 07	2.1927	3.8006	2.1100

Note: bold data indicate the best results.

shows the results of the 05 and 07 sequences in comparison with VINS-Fusion and DynaVINS, and the bolded data indicate the best performance, as shown in Figure 7. Since dynamic objects on both the 00 and 05 sequence streets are sparse, dynamic object rejection provides a limited improvement to the system’s accuracy. The experimental data were obtained from real measurements, rather than directly from the paper, to compare the generalizability of the algorithms. As shown in Table 3, the localization accuracy and dynamic feature recognition rejection of DynaVINS are highly dependent on the hyperparameters (momentum factor and regularization factor). Therefore, its localization results in different datasets are worse, and it is hard to keep the algorithm localized with high accuracy even after a long period of parameter adjustment. The experimental results on the KITTI dataset show that D-VINS has better generalizability than DynaVINS, as well as a certain accuracy improvement compared to VINS-Fusion.

4.1.3. VIODE Dataset

VIODE is a simulation dataset for testing VIO performance in dynamic environments such as urban areas, filling the gap in dynamic VIO system evaluation. The dataset simulates the UAV localization problem in different dynamic environments (daytime city street environment, dark city street environment, and underground parking environment). Each scene is divided into four sequences according to the number of dynamic objects: 0_none, 1_low, 2_mid, and 3_high have a total of 12 sequences. In the high sequences, the camera field of view is included with the entire occlusion to evaluate the localization accuracy and system robustness of the VIO in extreme situations. The dataset contains time-synchronized stereo color images, IMU data, an instance segmentation mask, and the ground truth of the trajectory.

To validate the accuracy of the algorithm in the dynamic recognition of absolute dynamic points and temporary static points, D-VINS and DynaVINS were compared. VINS-Fusion was also included in the comparison to prove the necessity and effectiveness of the dynamic rejection module. The hyperparameters used in D-VINS are: the number of feature points (150), the pixel spacing of feature points (35), the maximum number of optimizations (5), and the initial value of the dynamism factor of the feature point (${\lambda }_p=2, {\lambda }_e=2$). The hyperparameters in DynaVINS use the same values as in the paper, with regularization factor ${\lambda }_w=1.0$ and momentum factor ${\lambda }_m=2.0$. Figure 8 shows the miss-detection compensation module. These features remain as labels as they are tracked by the optical flow. Figure 9 presents the epipolar constraint results; the points far from the epipolar line are labeled as dynamic points.

Overall, D-VINS demonstrates great pose estimation accuracy in static scenes when incorporating the dynamic factor, as illustrated in

Table 4. The ATE-RMSE (m) of VINS-Fusion, DynaVINS, and D-VINS on the VIODE dataset.

Scenes	Sequences	VINS-Fusion	DynaVINS	D-VINS
Parking_lot	0_none	0.0774	0.0595	0.0538
	1_low	0.1126	0.0826	0.0472
	2_mid	0.1174	0.0630	0.0396
	3_high	0.1998	0.0982	0.0664
City_day	0_none	0.1041	0.1391	0.0882
	1_low	0.2043	0.0748	0.0912
	2_mid	0.2319	0.0520	0.0864
	3_high	0.3135	0.0743	0.0835
City_night	0_none	0.2624	0.1801	0.1561
	1_low	0.5665	0.1413	0.1221
	2_mid	0.3862	0.1192	0.1395
	3_high	0.7611	0.1519	0.1566

Note: bold data indicate the best results.

. It exhibits a better localization accuracy in low-dynamic scenes and comparable accuracy to DynaVINS in high-dynamic scenes, and, in some sequences, it even outperforms DynaVINS. Additionally, the spacing and quantity of feature points in the front end significantly affect the performance of D-VINS. In cases where remote features dominate the field of view or the feature spacing is too small, the accuracy of the stereo system is severely compromised. Thus, for the same set of parameters, D-VINS does not perform as well in the city sequence as it does in the parking lot environment. The localization accuracy of DynaVINS highly depends on the hyperparameters, such as the regularization factor ${\lambda }_w$ and the momentum factor ${\lambda }_m$. However, these hyperparameters are manually tuned and lack generalization across different scenes. While they exhibit high localization accuracy in some scenes, they may not be suitable for other sequences. For example, the localization accuracy of DynaVINS in the City_day dataset is unstable.

In dynamic scenes with low occlusion, D-VINS incorporates a deep-learning module that detects dynamic objects and calculates dynamic factors with geometry. D-VINS achieves higher accuracy in screening dynamic feature points compared to DynaVINS, which only relies on geometric methods for dynamic object rejection, as observed in Figure 10b,d,f. Consequently, D-VINS achieves a better positioning accuracy in low and mid sequences. The green feature points represent absolute static points, the white feature points represent absolute dynamic points, and the purple feature points represent temporary dynamic points. However, in highly occluded environments where dynamic objects are near the camera, D-VINS may fail to detect the objects with the deep-learning network.

4.2. Data Collection Equipment and Real-Environment Dataset Experiments

To demonstrate that D-VINS can be applied to a real project, we built a self-made data acquisition device and created a dataset with it.

4.2.1. Data Collection Devices and Real Datasets

The device integrates GNSS, inertial navigation, LIDAR, and a stereo camera, and is mainly divided into two parts: the handheld part, and the backpack part. As shown in Figure 11, the device has three working modes: handheld, backpack, and vehicle working mode. The handheld part includes a GNSS antenna, Velodyne VLP-32C mechanical LIDAR, Inertial Labs INS-D GNSS/IMU inertial guidance, and a ZED2 color stereo camera, and the resolution is 1280 × 720; the backpack part includes an NVIDIA Jetson AGX Xavier processor, a 12 V DC lithium battery, an MD-649 4G DTU 4G communication module, and an antenna. In this paper, one handheld rural sequence and two city street sequences were selected for experimental validation:

• The 5_SLAM_country_dynamic_loop_1 sequence was collected in a village in Xiangyin County, Yueyang City, Hunan Province, in a relatively open environment, where a pedestrian and child were always present in the image moving in synchronization with the camera. The start and end points of the sequence are close to each other, but there is no loop to detect the drift.
• The 14_SLAM_car_road_1 sequence shows a street in Xiangyin County, Yueyang City, Hunan Province. The sequence is an open environment. This environment is challenging for stereo-visual localization, which causes severe drift. The rural roads are narrow with many vehicles, and there are villagers gathering in the middle of the road. Pedestrians and vehicles are intricate and occupy a large field of view, making positioning difficult and challenging.
• The 18_SLAM_car_road_2 sequence is an urban environment with wider roads, more vehicles, and more pedestrians compared to the 14 rural streets. It is suitable as a dynamic rejection algorithm evaluation sequence. The main data types include GNSS raw data, IMU data, LiDAR point cloud data, and binocular color image data. The ground truth of the trajectory is obtained with GNSS RTK.

4.2.2. Feature Classification Results on the Real Dataset

In the 5_SLAM_country_dynamic_loop_1 sequence, there are two people walking in front of the camera; this represents an easy object detection task for deep learning to accomplish. As shown in Figure 12, when the person with the jacket is moving in the second row in (a), the dynamic features are segmented accurately. When the person is movable but remains static at the current time shown in the first row in (a), those feature points are kept for optimization. In a highly dynamic sequence, the majority of the points are movable and only a few of them are moving. D-VINS is able to reject dynamic features that are close to the camera with higher dynamic factors. In the city street, the cars parked on the roadside and driving on the road are detected with different motion state classifications.

4.2.3. Trajectory Results on the Real Dataset

This paper compared the results of the current state-of-the-art algorithms DynaVINS, D-VINS, and VINS on real dataset sequences. D-VINS obtained better measurement results on the real dataset, effectively overcoming the influence of dynamic objects. As shown in

Table 5. The ATE-RMSE (m) of VINS-Fusion, DynaVINS, and D-VINS on the real dataset.

Sequences	VINS-Fusion	DynaVINS	D-VINS
5_SLAM_dynamic_loop_1	0.657039	2.145493	0.654882
14_SLAM_car_road_1	37.31964	-	27.60877
18_SLAM_car_road_2	299.7889	-	151.2075

Note: “-” indicates that the method failed to estimate the camera pose. Bold data indicate the best results.

, D-VINS obtained a better localization accuracy than DynaVINS in the 5_SLAM_dynamic_loop_1 sequence. Even though the ATE-RMSE of D-VINS was similar to that of VINS, the more detailed results show that D-VINS had a more accurate localization accuracy in the presence of dynamic objects, as well as a lower median and mean. Even after parameter adjustments, DynaVINS had difficulty finding parameters that could achieve good accuracy in localization (the hyperparameters provided in the paper could not accomplish localization, even though they worked well on the VIODE and KITTI datasets). The high reliance on equipment and hyperparameters is also a drawback for geometry-based methods. D-VINS is merely a visual odometry system, and, in the absence of GNSS and loop detection, the drift is significant. In Figure 13, D-VINS achieves excellent positioning results in two road sequences without loop detection. The pure VIO system (no global optimization and loopback detection) can effectively reduce the influence of dynamic objects and substantially exceed the positioning accuracy of VINS. In summary, D-VINS has stronger robustness and scene applicability in dynamic scenes compared to other algorithms.

4.2.4. Time Analysis and Ablation Experiment

As a fundamental component of robot state estimation, SLAM plays a crucial role in the smooth execution of higher-level tasks. Therefore, we conducted tests on the average time cost of processing each frame of various frameworks. The experimental results are presented in Figure 14. All tests were conducted on a laptop equipped with 16 GB RAM (CPU: AMD Ryzen7 5800H, GPU: NVIDIA GEFORCE RTX 3050TI). Due to the inconsistent number of iterations during optimization, the overall time for back-end optimization exhibits significant variance. However, the system can operate in real time with a 10 Hz camera.

D-VINS adopts a combined approach of geometric and semantic information to remove dynamic feature points and improve accuracy, effectively incorporating the strengths of both methods while avoiding their limitations. To validate the effectiveness of the fused algorithm integrating geometric and semantic information, we conducted comparative experiments. D-VINS (G) represents a geometry-based method adjusting feature weights with epipolar constraints and IMU reprojection. D-VINS (S) represents a semantic-based method with YOLOV5. D-VINS (G + S) represents a fusion algorithm based on geometric and semantic information. The experimental results are shown in

Table 6. The ATE-RMSE (m) of D-VINS (G), D-VINS (S), and D-VINS (G + S) on the VIODE dataset.

Scenes	Sequences	D-VINS (G)	D-VINS (S)	D-VINS (G + S)
Parking_lot	0_none	0.1000	0.0568	0.0538
	1_low	0.1196	0.0517	0.0472
	2_mid	0.1126	0.0709	0.0396
	3_high	0.1387	0.0702	0.0664

Note: Bold data indicate the best results.

.

5. Discussion

In the experimental results shown in Table 1, Table 2, Table 3, Table 4 and Table 5, D-VINS and the contrasting SLAM systems were all validated. D-VINS outperformed the other systems in terms of the ATE and RPE in six sequences of the TUM RGB-D dataset. Table 1 and Table 2 demonstrate the effectiveness of D-VINS’ dynamic object removal method, especially in scenarios involving pure rotation and high-occlusion areas, compared to semantic-based methods like RS-SLAM and DS-SLAM. Moreover, RS-SLAM and Dynamic-VINS are limited to indoor usage due to their reliance on depth information, making them unsuitable for outdoor environments.

In the KITTI sequences shown in Table 3, D-VINS demonstrated its effectiveness in outdoor scenes. The state-of-the-art DynaVINS showed high accuracy in the VIODE simulation dataset, as shown in Table 4, but it heavily relied on two hyperparameters, making it challenging to achieve the same high accuracy in different datasets. In the KITTI 05 and 07 sequences, where there were fewer dynamic objects than in VIODE, the impact on position estimation was less significant, resulting in DynaVINS not performing as well as it did in VIODE, as displayed in Figure 7. For the highly occluded scenes in VIODE, deep learning struggled, especially when the objects moved toward the camera’s optical center (shown in the first image of Figure 10b). In contrast, D-VINS demonstrated good dynamic feature segmentation performance and trajectory accuracy, exhibiting better robustness in multiple environments in Figure 10b,d,f. However, the accuracy of DynaVINS decreased in sequences with fewer dynamic objects, such as the 0_none sequences. D-VINS maintains accuracy in highly occluded dynamic environments. However, its improvement in low-occlusion dynamic environments is not significant with abundant features that can be tracked.

Furthermore, D-VINS showed great performance in feature classification and trajectory accuracy in rural and city sequences, as shown in Figure 12. For our real dataset, DynaVINS faced challenges in accomplishing the localization task. Higher reprojection errors could arise from both the camera movement and dynamic features, making it difficult to accurately indicate the feature’s motion state using weights. In sequences with large spaces and without loop closures, VINS-Fusion performed worse compared to D-VINS. However, the segmentation accuracy depends on the bounding boxes obtained from YOLOV5, making it unable to achieve pixel-level precise segmentation. Factors such as the complexity of moving objects and blurry images can influence the results of object detection and geometric calculations. These influences can degrade the segmentation accuracy and, ultimately, impact the camera localization precision.

In summary, while D-VINS may not overcome extreme conditions of complete occlusion in the field of view, it still achieves a high localization accuracy in highly occluded environments, comparable to DynaVINS. D-VINS is more suitable for indoor environments with occlusions and outdoor environments with at a small scale. In large outdoor scenes, it experiences considerable drift without GNSS and loop detection. In cases where the camera’s field of view is completely occluded, the system relies more on IMU pre-integration results. The few static feature points are given higher weights in the iterative optimization process, due to dynamic factors based on epipolar constraints and IMU reprojection errors, ultimately enhancing the accuracy of the pose optimization. Extensive experimental results have proven that D-VINS can improve the accuracy and robustness of pose calculation in dynamic scenes.

6. Conclusions

In this paper, we proposed a novel dynamic VIO system designed for outdoor dynamic scenes, effectively reducing the impact of dynamic objects. The D-VINS system consists of four modules: target identification and data pre-processing, feature point classification and tracking, and back-end dynamic factor BA optimization. The dynamic object recognition relies on YOLOV5 to obtain semantic information about the scene, such as walking people and stopped cars. The semantic labels are continuously updated, classifying dynamic points into absolute dynamic points, absolute static points, and temporary static points. The dynamic factors of temporary static points are calculated using IMU pre-integration and epipolar constraints. Only absolute static points and temporary static points are sent for nonlinear optimization. Feature point weights are adjusted based on the dynamic label and dynamic factors during BA optimization. The system’s effectiveness in dynamic environments was verified using the TUM RGB-D, KITTI, and VIODE datasets, as well as our own dataset. The experimental results demonstrated our system’s superior performance compared to RS-SLAM, Dynamic-VINS, and the state-of-the-art method DynaVINS. In general, D-VINS enhances feature point weights, enabling its use in environments with occluding dynamic objects. However, the accuracy is limited by the number of optimization iterations, as excessive iterations can hinder real-time system operation. Time analysis shows that sacrificing some optimization iterations still allows for achieving a satisfactory localization accuracy while maintaining real-time performance.

Future work should address some limitations and explore potential improvements. Firstly, the algorithm lacks pixel-level segmentation of feature points, even with geometry constraints. Integrating a high-speed semantic segmentation module could help to achieve a more accurate identification of dynamic feature points in the front end. Secondly, during the experimental process, degraded scenes had an impact on the estimation. For instance, the localization accuracy of stereo cameras decreases in open environments. Adjusting the weights of feature points at different distances can improve the localization accuracy. The algorithm also exhibits good scalability, as the strategy of adjusting feature point weights can be incorporated into existing multi-sensor fusion systems. Additionally, to address occlusion issues in dynamic scenes, the target velocity can be used to introduce extra constraints on self-motion estimation.