SD-VIS: A Fast and Accurate Semi-Direct Monocular Visual-Inertial Simultaneous Localization and Mapping (SLAM)

Abstract

In practical applications, how to achieve a perfect balance between high accuracy and computational efficiency can be the main challenge for simultaneous localization and mapping (SLAM). To solve this challenge, we propose SD-VIS, a novel fast and accurate semi-direct visual-inertial SLAM framework, which can estimate camera motion and structure of surrounding sparse scenes. In the initialization procedure, we align the pre-integrated IMU measurements and visual images and calibrate out the metric scale, initial velocity, gravity vector, and gyroscope bias by using multiple view geometry (MVG) theory based on the feature-based method. At the front-end, keyframes are tracked by feature-based method and used for back-end optimization and loop closure detection, while non-keyframes are utilized for fast-tracking by direct method. This strategy makes the system not only have the better real-time performance of direct method, but also have high accuracy and loop closing detection ability based on feature-based method. At the back-end, we propose a sliding window-based tightly-coupled optimization framework, which can get more accurate state estimation by minimizing the visual and IMU measurement errors. In order to limit the computational complexity, we adopt the marginalization strategy to fix the number of keyframes in the sliding window. Experimental evaluation on EuRoC dataset demonstrates the feasibility and superior real-time performance of SD-VIS. Compared with state-of-the-art SLAM systems, we can achieve a better balance between accuracy and speed.

1. Introduction

Simultaneous localization and mapping (SLAM) plays an important role in self-driving cars, virtual reality, unmanned aerial vehicles (UAV), augmented reality and artificial intelligence [1,2]. This technology can provide reliable state estimation for UAV and self-driving cars in GPS-denied environments by relying on its sensors. Various types of sensors can be utilized in SLAM, such as stereo camera, lidar, inertial measurement units (IMU), and monocular camera. However, they have significant disadvantages when used individually: the metric scale of stereo camera can be obtained directly by using fixed baseline length, but it can only be estimated accurately in a limited depth range [3]; lidar has high precision in indoor, but it will encounter the reflection problem of glass surface in outdoor [4]; cheap IMUs are extremely susceptible to bias and noise [5]; monocular camera cannot estimate the absolute metric scale [6]. This paper mainly studies the monocular vision-inertial navigation system (VINS) based on multi-sensor fusion [7], which has the advantages of small size, lightweight, observable scale, roll, and pitch angle, etc.

According to the different methods of image information processing, there are two categories of SLAM: feature-based method and direct method. The standard process of feature-based method proceeds in three steps [8,9,10]. Firstly, a group of sparse point or line features is extracted from each image, and feature matching between adjacent frames is performed by using invariant feature descriptors. Secondly, estimate the camera motion and the 3D position of sparse feature points by using multiple view geometry theory. Finally, optimize the camera motion and the 3D position of sparse feature points by minimizing visual re-projection errors. However, the feature-based method has the following disadvantages: feature extraction and matching are very time-consuming; fewer features extractable in low-texture environments; repeated texture environments will cause incorrect feature matching. Therefore, the accuracy and robustness of the feature-based method depend on the correct feature extraction and matching.

The direct method considers the entire image or some pixels with a large gradient and directly estimates the camera motion and scene structure by minimizing the photometric error [11,12,13,14]. Therefore, in the low-texture environments and repeated texture environments, the performance of the direct method is better than the feature-based method. In addition, without feature extraction and matching, the calculation speed of the direct method is faster than the feature-based method. However, the photometric error function is highly non-convex, it is difficult for the direct method to converge when large baseline motion and image blur occurs. Due to the inability to perform effective loop closure detection, the cumulative drift generated during long-term operation is still an unresolved problem [15].

Academic research on SLAM is very extensive. Well known methods include PTAM, SVO, VINS-Mono, LSD-SLAM, ORB-SLAM, DSO. PTAM was one of the most representative systems in the early stage of the feature-based SLAM algorithm. This algorithm first proposed to divide tracking and mapping into two parallel threads [16]. After that, most feature-based SLAM algorithms have adopted this idea, including ORB-SLAM. ORB-SLAM is the most successful feature-based SLAM, which uses ORB features in tracking, mapping, re-location, and loop closure detection [17]. VINS- Mono is a compelling multi-sensor monocular vision-inertial SLAM based on tight coupling and non-linear optimization [18]. Pl-VIO is an extension of the line feature of VINS-Mono, which can optimize the re-projection error of point and line features in a sliding window [19]. VINS-Fusion is a multi-sensor fusion platform based on non-linear optimization, which is the continuation of VINS-Mono in the direction of multi-sensor fusion [20]. LSD-SLAM is a novel method of real-time monocular SLAM based on direct method and can create large-scale semi-dense maps in real-time on a laptop without GPU acceleration [21]. Based on LSD-SLAM, DSO adds complete photometric calibration and uniformly samples pixels with large gradients throughout the image, thereby improving tracking accuracy and robustness [11]. On the basis of DSO, LDSO adds loop closure detection, which makes up for the shortcomings of DSO incapable of loop closure detection and eliminates the cumulative error generated during long navigation [22]. Stereo DSO is an improved version of DSO, which can realize the real-time estimation of motion and 3D structure with high accuracy and robustness on the moving stereo camera [3]. Lee authors in [7] presents a new implementation method for efficient simultaneous localization and mapping using a forward-viewing monocular vision sensor. The method is developed to be applicable in real time on a low-cost embedded system for indoor service robots.

In recent years, SLAM systems that combine the feature-based method and direct method have become popular. Semi-direct visual odometry (SVO) is an effective hybrid method that combines the advantages of feature-based method and direct method [23]. However, since the algorithm was originally designed for the drone’s look-down camera, it is easy to lose camera tracking in other settings. Lee [24] proposed a loose-coupled method by combining ORB-SLAM and DSO to improve positioning accuracy. However, its frontend and backend are almost independent, which cannot share estimation information to further improve the pose precision. In [25,26], different semi-direct approaches were proposed for stereo odometry. Both methods use feature-based tracking to obtain a motion prior, and then perform direct semi-dense or sparse alignment to refine the camera pose. SVL [27] can be considered a combination of ORB-SLAM and SVO. The method for ORB-SLAM is adopted in keyframes, and SVO is adopted in non-keyframes. Therefore SVL can achieve good balance between speed and accuracy according to camera motion and environment. Our method is motivated by SVL, but we go one step further in real-time performance. Specifically, thanks to the keyframe selection strategy and sliding window-based back-end, we only need to extract new feature points on the keyframes and track them with KLT sparse optical flow algorithm, which can further reduce the calculation complexity while ensuring accuracy.

In this paper, we present SD-VIS, a novel fast and accurate semi-direct visual-inertial SLAM framework, which combines the exactness of feature-based method and quickness of direct method. The keyframes in SD-VIS are tracked by feature-based method, which is used for sliding window-based non-linear optimization and loop closure detection. This strategy solves the problem of drift in the long-term operation and ensures the robustness of the algorithm in case of large baseline motion and image blur. Non-keyframes are tracked by the direct method, and the distance between adjacent non-keyframes is minimal, which ensures the convergence of error function. Compared with the direct method, SD-VIS exhibits the function of loop closure detection and solves the problem of drift in long-term operation. Compared with the feature-based method, SD-VIS can achieve the same accuracy while maintaining a faster speed.

2. System Framework Overview

Figure 1 demonstrates the framework of the semi-direct vision-inertial SLAM system. Sensor data comes from a monocular camera and IMU. IMU measurements between two consecutive images are pre-integrated, and the pre-integration is used as the constraint of IMU between two images (Section 3.2). In the initialization procedure, we detect the feature points on each image and track them with KLT sparse optical flow algorithm [15].

In the following visual-inertial alignment, we align the pre-integrated IMU measurements and visual images and calibrate out the metric scale, initial velocity, gravity vector, and gyroscope bias by using multiple view geometry (MVG) theory based on the feature-based method. (Section 3.3). After initialization, keyframe selection will be performed based on the IMU pre-integration and previous feature matching results. The previous feature matching refers to the feature matching between the last frame and the penultimate frame in the sliding window, and the matching is completed before the current frame arrives. Non-keyframes are used for fast-tracking and localization by direct method [11], and keyframes are tracked by feature-based method [18] and used for non-linear optimization and loop closure detection (Section 4). In the following tight coupling optimization framework, we can get more accurate state estimation by minimizing visual re-projection error, IMU residual, prior information from marginalization, and re-location information from loop closure detection (Section 5).

3. IMU Measurements and Visual-Inertial Alignment

3.1. Definition of Symbols

Figure 2 shows the definition of symbols in the semi-direct visual-inertial SLAM framework. C, B, and W are the camera coordinate system, the IMU body coordinate system, and the world coordinate system, respectively. We define ${\mathrm{T}}_{\mathrm{WB}}=\left({\mathrm{R}}_{\mathrm{B}}^{\mathrm{W}},{\mathrm{P}}_{\mathrm{B}}^{\mathrm{W}}\right)$ as the motion of B relative to W. ${\mathrm{T}}_{\mathrm{BC}}=\left({\mathrm{R}}_{\mathrm{C}}^{\mathrm{B}},{\mathrm{P}}_{\mathrm{C}}^{\mathrm{B}}\right)$ represents the extrinsic parameters between C and B, which can be calibrated in advance. ${\mathrm{T}}_{t,t+1}\in \mathrm{SE}\left(3\right)$ represents the motion from time $\mathrm{t}$ to time $\mathrm{t}+1$ in the coordinate system C, and $Z_{t,t+1}$ represents a pre-integrated IMU measurement between the camera coordinate system ${\mathrm{C}}_t$ and ${\mathrm{C}}_{t+1}$.

A $3\mathrm{D}$ point ${\mathrm{F}}_1,{\mathrm{F}}_2\in {\mathrm{R}}^3$ represents the spatial feature points observed simultaneously by the camera coordinate system ${\mathrm{C}}_t$ and ${\mathrm{C}}_{t+1}$. ${\mathrm{P}}_1,{\mathrm{P}}_2,{\mathrm{P}}_3,{\mathrm{P}}_4\in {\mathrm{R}}^2$ are the projections of feature points on the image coordinate system. We adopt the traditional pinhole camera model to map the ${\mathrm{F}}_1$ in the camera coordinate system to the image coordinate system by the projection function π: ${\mathrm{R}}^3\to {\mathrm{R}}^2$:

$${\mathrm{P}}_1={\mathsf{\pi }\mathrm{F}}_1=\left[\begin{array}{c}{\mathrm{f}}_{\mathrm{u}}\frac{{\mathrm{x}}_{\mathrm{c}}}{{\mathrm{z}}_{\mathrm{c}}}+{\mathrm{c}}_{\mathrm{u}}\\ {\mathrm{f}}_{\mathrm{v}}\frac{{\mathrm{y}}_{\mathrm{c}}}{{\mathrm{z}}_{\mathrm{c}}}+{\mathrm{c}}_{\mathrm{v}}\end{array}\right]{\mathrm{F}}_1={\left[\begin{array}{ccc}{\mathrm{x}}_{\mathrm{c}}& {\mathrm{y}}_{\mathrm{c}}& {\mathrm{z}}_{\mathrm{c}}\end{array}\right]}^{\mathrm{T}}$$

(1)

where ${\left[\begin{array}{cc}{\mathrm{f}}_{\mathrm{u}}& {\mathrm{f}}_{\mathrm{v}}\end{array}\right]}^{\mathrm{T}}$ and ${\left[\begin{array}{cc}{\mathrm{c}}_{\mathrm{u}}& {\mathrm{c}}_{\mathrm{v}}\end{array}\right]}^{\mathrm{T}}$ is camera internal parameters.

3.2. IMU Pre-Integration

In the back-end optimization and visual-inertial alignment, the constraints of vision and IMU need to be optimized in the same frame, so the IMU measurements between two adjacent frames need to be integrated into one constraint.

IMU can output 3-axis angular velocity $\tilde{\mathsf{\omega }}$ and 3-axis acceleration $\tilde{\mathsf{\alpha }}$ including bias and Gaussian white noise:

$$\tilde{\mathsf{\omega }}={\mathsf{\omega }}^{\mathrm{B}}+{\mathrm{b}}_{\mathsf{\omega }}^{\mathrm{B}}+{\mathrm{n}}_{\mathsf{\omega }}^{\mathrm{B}}$$

(2)

$$\tilde{\mathsf{\alpha }}={\mathrm{R}}_{\mathrm{W}}^{\mathrm{B}}\left({\mathsf{\alpha }}^{\mathrm{W}}+{\mathrm{g}}^{\mathrm{W}}\right)+{\mathrm{b}}_{\mathsf{\alpha }}^{\mathrm{B}}+{\mathrm{n}}_{\mathsf{\alpha }}^{\mathrm{B}}$$

(3)

where ${\mathrm{n}}_{\mathsf{\omega }}^{\mathrm{B}}~\mathrm{N}\left(0,{\mathsf{\sigma }}_{\mathsf{\omega }}^2\right)$, ${\mathrm{n}}_{\mathsf{\alpha }}^{\mathrm{B}}~\mathrm{N}\left(0,{\mathsf{\sigma }}_{\mathsf{\alpha }}^2\right)$ represents the Gaussian white noise. ${\mathrm{g}}^{\mathrm{W}}={\left[0,0,\mathrm{g}\right]}^{\mathrm{T}}$ is the gravity vector. ${\mathrm{R}}_{\mathrm{w}}^{\mathrm{B}}$ represents the rotation matrix from W to B. ${\mathrm{b}}_{\mathsf{\omega }}^{\mathrm{B}},{\mathrm{b}}_{\mathsf{\alpha }}^{\mathrm{B}}$ represents the biases of gyroscope and accelerometer.

We define ${\mathrm{P}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}$,${\mathrm{V}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}$, ${\mathrm{q}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}$ as the translation, velocity, and rotation quaternions in the IMU body coordinate system ${\mathrm{B}}_{\mathrm{i}}$. If we know the measurements value of IMU during $\mathrm{t}\in \left[\mathrm{i},\mathrm{j}\right]$, we can calculate ${\mathrm{P}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}$, ${\mathrm{V}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}$, ${\mathrm{q}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}$ in the coordinate system ${\mathrm{B}}_{\mathrm{j}}$:

$${\mathrm{P}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}={\mathrm{P}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}+{\mathrm{V}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}\mathsf{\Delta }\mathrm{t}+{{\displaystyle \iint }}_{\mathrm{t}\in \left[\mathrm{i},\mathrm{j}\right]}^{}\left[{\mathrm{R}}_{{\mathrm{B}}_{\mathrm{t}}}^{\mathrm{W}}\left({\tilde{\mathsf{\alpha }}}_{\mathrm{t}}-{\mathrm{b}}_{{\mathsf{\alpha }}_{\mathrm{t}}}^{\mathrm{B}}\right)-{\mathrm{g}}^{\mathrm{W}}\right]{\mathsf{\delta }\mathrm{t}}^2$$

(4)

$${\mathrm{V}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}={\mathrm{V}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}+{{\displaystyle \int }}_{\mathrm{t}\in \left[\mathrm{i},\mathrm{j}\right]}^{}\left[{\mathrm{R}}_{{\mathrm{B}}_{\mathrm{t}}}^{\mathrm{W}}\left({\tilde{\mathsf{\alpha }}}_{\mathrm{t}}-{\mathrm{b}}_{{\mathsf{\alpha }}_{\mathrm{t}}}^{\mathrm{B}}\right)-{\mathrm{g}}^{\mathrm{W}}\right]\mathsf{\delta }\mathrm{t}$$

(5)

$${\mathrm{q}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}={\mathrm{q}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}\otimes {{\displaystyle \int }}_{\mathrm{t}\in \left[\mathrm{i},\mathrm{j}\right]}^{}\frac{1}{2}\mathsf{\Omega }\left({\tilde{\mathsf{\omega }}}_{\mathrm{t}}-{\mathrm{b}}_{{\mathsf{\omega }}_{\mathrm{t}}}^{\mathrm{B}}\right){\mathrm{q}}_{{\mathrm{B}}_{\mathrm{t}}}^{\mathrm{W}}\mathsf{\delta }\mathrm{t}$$

(6)

where:

$$\mathsf{\Omega }\left(\mathsf{\omega }\right)=\left[\begin{array}{cc}-{\mathsf{\omega }}_{\times }& \mathsf{\omega }\\ -{\mathsf{\omega }}^T& 0\end{array}\right], {\mathsf{\omega }}_{\times }=\left[\begin{array}{ccc}0& -{\mathsf{\omega }}_z& {\mathsf{\omega }}_y\\ {\mathsf{\omega }}_z& 0& -{\mathsf{\omega }}_x\\ -{\mathsf{\omega }}_y& {\mathsf{\omega }}_x& 0\end{array}\right]$$

From formulas (4)–(6), we can know that the IMU body state ${\mathrm{P}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}$, ${\mathrm{V}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}$, ${\mathrm{q}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}$ is related to the IMU body coordinate system ${\mathrm{B}}_{\mathrm{i}}$. In the back-end tightly coupling optimization, we will continuously iteratively update the IMU state variables in the sliding window. When the IMU body state at time $\mathrm{t}=\mathrm{i}$ is iteratively updated, we need to recalculate the state at time $\mathrm{t}=\mathrm{j}$, which is very time-consuming. We adopt IMU pre-integration technology to avoid unnecessary time consumption. formulas (4)–(6) can be written as:

$${\mathrm{R}}_{\mathrm{W}}^{{\mathrm{B}}_{\mathrm{i}}}{\mathrm{P}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}={\mathrm{R}}_{\mathrm{W}}^{{\mathrm{B}}_{\mathrm{i}}}\left({\mathrm{P}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}+{\mathrm{V}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}\mathsf{\Delta }\mathrm{t}-\frac{1}{2}{\mathrm{g}}^{\mathrm{W}}{\mathsf{\Delta }\mathrm{t}}^2\right)+{\mathsf{\theta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}$$

(7)

$${\mathrm{R}}_{\mathrm{W}}^{{\mathrm{B}}_{\mathrm{i}}}{\mathrm{V}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}={\mathrm{R}}_{\mathrm{W}}^{{\mathrm{B}}_{\mathrm{i}}}\left({\mathrm{V}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}\mathsf{\Delta }\mathrm{t}-{\mathrm{g}}^{\mathrm{W}}{\mathsf{\Delta }\mathrm{t}}^{}\right)+{\mathsf{\beta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}$$

(8)

$${\mathrm{q}}_{\mathrm{W}}^{{\mathrm{B}}_{\mathrm{i}}}\otimes {\mathrm{q}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}={\mathsf{\gamma }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}$$

(9)

where:

$${\mathsf{\theta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}={{\displaystyle \iint }}_{\mathrm{t}\in \left[\mathrm{i},\mathrm{j}\right]}^{}\left[{\mathrm{R}}_{{\mathrm{B}}_{\mathrm{t}}}^{{\mathrm{B}}_{\mathrm{i}}}\left({\tilde{\mathsf{\alpha }}}_{\mathrm{t}}-{\mathrm{b}}_{{\mathsf{\alpha }}_{\mathrm{t}}}^{\mathrm{B}}\right)\right]{\mathsf{\delta }\mathrm{t}}^2$$

(10)

$${\mathsf{\beta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}={{\displaystyle \int }}_{\mathrm{t}\in \left[\mathrm{i},\mathrm{j}\right]}^{}\left[{\mathrm{R}}_{{\mathrm{B}}_{\mathrm{t}}}^{{\mathrm{B}}_{\mathrm{i}}}\left({\tilde{\mathsf{\alpha }}}_{\mathrm{t}}-{\mathrm{b}}_{{\mathsf{\alpha }}_{\mathrm{t}}}^{\mathrm{B}}\right)\right]\mathsf{\delta }\mathrm{t}$$

(11)

$${\mathsf{\gamma }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}={{\displaystyle \int }}_{\mathrm{t}\in \left[\mathrm{i},\mathrm{j}\right]}^{}\frac{1}{2}\mathsf{\Omega }\left({\tilde{\mathsf{\omega }}}_{\mathrm{t}}-{\mathrm{b}}_{{\mathsf{\omega }}_{\mathrm{t}}}^{\mathrm{B}}\right){\mathrm{q}}_{{\mathrm{B}}_{\mathrm{t}}}^{{\mathrm{B}}_{\mathrm{i}}}\mathsf{\delta }\mathrm{t}$$

(12)

From formulas (10)–(12), the pre-integration measurements ${\mathsf{\theta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}$, ${\mathsf{\beta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}$, ${\mathsf{\gamma }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}$, and the IMU body coordinate system ${\mathrm{B}}_{\mathrm{i}}$ are independent of each other. This means that when the states in the ${\mathrm{B}}_{\mathrm{i}}$ coordinate system are iteratively updated, there is no need to recalculate the states in the ${\mathrm{B}}_{\mathrm{j}}$ coordinate system. Since the IMU pre-integrated measurements ${\mathsf{\theta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}$, ${\mathsf{\beta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}$, ${\mathsf{\gamma }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}$ is affected by the bias, when the bias is updated iteratively, we will update the pre-integrated measurement by the first-order approximation method:

$${\mathsf{\theta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}\approx {\tilde{\mathsf{\theta }}}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}+{\mathrm{J}}_{{\mathrm{b}}_{\mathsf{\alpha }}}^{\mathsf{\theta }}{\mathsf{\delta }\mathrm{b}}_{{\mathsf{\alpha }}_{}}^{\mathrm{B}}+{\mathrm{J}}_{{\mathrm{b}}_{\mathsf{\omega }}}^{\mathsf{\theta }}{\mathsf{\delta }\mathrm{b}}_{{\mathsf{\omega }}_{}}^{\mathrm{B}}$$

(13)

$${\mathsf{\beta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}\approx {\tilde{\mathsf{\beta }}}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}+{\mathrm{J}}_{{\mathrm{b}}_{\mathsf{\alpha }}}^{\mathsf{\beta }}{\mathsf{\delta }\mathrm{b}}_{{\mathsf{\alpha }}_{}}^{\mathrm{B}}+{\mathrm{J}}_{{\mathrm{b}}_{\mathsf{\omega }}}^{\mathsf{\beta }}{\mathsf{\delta }\mathrm{b}}_{{\mathsf{\omega }}_{}}^{\mathrm{B}}$$

(14)

$${\mathsf{\gamma }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}\approx {\tilde{\mathsf{\gamma }}}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathrm{J}}_{{\mathrm{b}}_{\mathsf{\omega }}}^{\mathsf{\gamma }}{\mathsf{\delta }\mathrm{b}}_{{\mathsf{\omega }}_{}}^{\mathrm{B}}{}_{}\end{array}\right]$$

(15)

where ${\mathrm{J}}_{{\mathrm{b}}_{\mathsf{\alpha }}}^{\mathsf{\theta }}$, ${\mathrm{J}}_{{\mathrm{b}}_{\mathsf{\omega }}}^{\mathsf{\theta }}$, ${\mathrm{J}}_{{\mathrm{b}}_{\mathsf{\alpha }}}^{\mathsf{\beta }}$, ${\mathrm{J}}_{{\mathrm{b}}_{\mathsf{\omega }}}^{\mathsf{\beta }}$, ${\mathrm{J}}_{{\mathrm{b}}_{\mathsf{\omega }}}^{\mathsf{\gamma }}$ are the Jacobian matrices of pre-integrated measurements with respect to bias.

3.3. Visual-Inertial Alignment

The convergence speed and effect of nonlinear visual-inertial SLAM systems depend heavily on reliable initial values. Therefore, in the initialization procedure of SD-VIS, we align the pre-integrated IMU measurements with the visual image to complete the system initialization.

3.3.1. Gyroscope Bias Correction

We regard the camera coordinate system ${\mathrm{C}}_0$ as the world coordinate system. We detect the feature points on each image and track them with KLT sparse optical flow algorithm, and then the rotation ${\mathrm{R}}_{{\mathrm{C}}_{\mathrm{t}}}^{{\mathrm{C}}_0}$ and ${\mathrm{R}}_{{\mathrm{C}}_{\mathrm{t}+1}}^{{\mathrm{C}}_0}$ of the two adjacent frames ${\mathrm{C}}_t$ and ${\mathrm{C}}_{t+1}$ can be estimated by using visual structure from motion (SfM). Rotation increment ${\tilde{\mathsf{\gamma }}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}$ between the IMU body coordinate system ${\mathrm{B}}_t$ and ${\mathrm{B}}_{t+1}$ can be estimated by IMU pre-integration. We can get the following formula:

$${}_{{\mathsf{\delta }\mathrm{b}}_{{\mathsf{\omega }}_{\mathrm{t}}}^{\mathrm{B}}}{}^{\min }{{\displaystyle \sum }}^{\text{}}\Vert {\mathrm{R}}_{{\mathrm{C}}_{\mathrm{t}+1}}^{{\mathrm{C}}_0}{}^{-1}\otimes {\mathrm{R}}_{{\mathrm{C}}_{\mathrm{t}}}^{{\mathrm{C}}_0}\otimes {\mathsf{\gamma }}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}{\Vert }^2$$

(16)

where:

$${\mathsf{\gamma }}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}\approx {\tilde{\mathsf{\gamma }}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathrm{J}}_{{\mathrm{b}}_{\mathsf{\omega }}}^{\mathsf{\gamma }}{\mathsf{\delta }\mathrm{b}}_{{\mathsf{\omega }}_{\mathrm{t}}}^{\mathrm{B}}{}_{}\end{array}\right]$$

(17)

We solve the above least squares problem to get the initial calibration of the gyroscope bias and use it to update ${\mathsf{\theta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}$, ${\mathsf{\beta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}$, ${\mathsf{\gamma }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}$.

3.3.2. Gravity Vector, Initial Velocity, and Metric Scale Correction

We define the variables that need to be calibrated as:

$${\mathrm{X}}_{\mathrm{I}}=\left[\begin{array}{ccc}{\mathrm{V}}_{{\mathrm{b}}_0}^{{\mathrm{b}}_0}& {\mathrm{V}}_{{\mathrm{b}}_1}^{{\mathrm{b}}_1}& \begin{array}{cc}\dots & \begin{array}{cc}{\mathrm{V}}_{{\mathrm{b}}_{\mathrm{n}}}^{{\mathrm{b}}_{\mathrm{n}}}& {\mathrm{g}}^{{\mathrm{C}}_0}\end{array} s\end{array}\end{array}\right]$$

(18)

where ${\mathrm{V}}_{{\mathrm{b}}_{\mathrm{k}}}^{{\mathrm{b}}_{\mathrm{k}}}$ is the initial velocity in the IMU body coordinate system,${\mathrm{g}}^{{\mathrm{C}}_0}$ is the gravity vector in the camera coordinate system, s represents the metric scale of semi-direct visual-inertial SLAM framework.

Suppose we have obtained pre-calibrated external parameter ${\mathrm{T}}_{\mathrm{BC}}=\left({\mathrm{R}}_{\mathrm{C}}^{\mathrm{B}},{\mathrm{P}}_{\mathrm{C}}^{\mathrm{B}}\right)$, we can transform the pose ${\mathrm{T}}_{{\mathrm{C}}_0{\mathrm{C}}_{\mathrm{t}}}=\left({\mathrm{R}}_{{\mathrm{C}}_{\mathrm{t}}}^{{\mathrm{C}}_0},{\mathrm{P}}_{{\mathrm{C}}_{\mathrm{t}}}^{{\mathrm{C}}_0}\right)$ from the camera coordinate system to the IMU body coordinate system:

$${\mathrm{q}}_{{\mathrm{B}}_{\mathrm{t}}}^{{\mathrm{C}}_0{}_{}}={\mathrm{q}}_{{\mathrm{C}}_{\mathrm{t}}}^{{\mathrm{C}}_0{}_{}}\otimes {\mathrm{q}}_{\mathrm{C}}^{\mathrm{B}}{}^{-1}$$

(19)

$$s{\mathrm{P}}_{{\mathrm{B}}_{\mathrm{t}}}^{{\mathrm{C}}_0{}_{}}={\mathrm{sP}}_{{\mathrm{C}}_{\mathrm{t}}}^{{\mathrm{C}}_0{}_{}}-{\mathrm{R}}_{{\mathrm{B}}_{\mathrm{t}}}^{{\mathrm{C}}_0{}_{}}{\mathrm{P}}_{\mathrm{C}}^{\mathrm{B}}$$

(20)

Considering two adjacent keyframes ${\mathrm{B}}_{\mathrm{t}}$ and ${\mathrm{B}}_{\mathrm{t}+1}$, then formulas (7) and (8) can be rewritten as:

$${\mathsf{\theta }}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}={\mathrm{R}}_{{\mathrm{C}}_0}^{{\mathrm{B}}_{\mathrm{t}}}\left(\mathrm{s}\left({\mathrm{P}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{C}}_0}-{\mathrm{P}}_{{\mathrm{B}}_{\mathrm{t}}}^{{\mathrm{C}}_0}\right)-{\mathrm{R}}_{{\mathrm{B}}_{\mathrm{t}}}^{{\mathrm{C}}_0}{\mathrm{V}}_{{\mathrm{B}}_{\mathrm{t}}}^{{\mathrm{B}}_{\mathrm{t}}}\Delta \mathrm{t}+\frac{1}{2}{\mathrm{g}}^{{\mathrm{C}}_0}{\Delta \mathrm{t}}^2\right)$$

(21)

$${\mathsf{\beta }}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}={\mathrm{R}}_{{\mathrm{C}}_0}^{{\mathrm{B}}_{\mathrm{t}}}\left({\mathrm{R}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{C}}_0}{\mathrm{V}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}+1}}-{\mathrm{R}}_{{\mathrm{B}}_{\mathrm{t}}}^{{\mathrm{C}}_0}{\mathrm{V}}_{{\mathrm{B}}_{\mathrm{t}}}^{{\mathrm{B}}_{\mathrm{t}}}+{\mathrm{g}}^{{\mathrm{C}}_0}{\mathsf{\Delta }\mathrm{t}}^{}\right)$$

(22)

We combine formulas (19)–(22) to get the following formula:

$${\tilde{\mathrm{Z}}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}=\left[\begin{array}{c}{\mathsf{\theta }}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}-{\mathrm{P}}_{\mathrm{C}}^{\mathrm{B}}+{\mathrm{R}}_{{\mathrm{C}}_0}^{{\mathrm{B}}_{\mathrm{t}}}{\mathrm{R}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{\mathrm{C}}{\mathrm{P}}_{\mathrm{C}}^{\mathrm{B}}\\ {\mathsf{\beta }}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}\end{array}\right]={\mathrm{H}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}{\mathrm{X}}_{\mathrm{I}}+{\mathrm{n}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}$$

(23)

where:

$${\mathrm{H}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}=\left[\begin{array}{ccc}-\mathrm{I}\Delta \mathrm{t}& 0& \begin{array}{cc}\frac{1}{2}{\mathrm{R}}_{\mathrm{C}}^{{\mathrm{B}}_{\mathrm{t}}}{\Delta \mathrm{t}}^2& {\mathrm{R}}_{{\mathrm{C}}_0}^{{\mathrm{B}}_{\mathrm{t}}}\left({\mathrm{P}}_{{\mathrm{C}}_{\mathrm{t}+1}}^{{\mathrm{C}}_0}-{\mathrm{P}}_{{\mathrm{C}}_{\mathrm{t}}}^{C_0}\right)\end{array}\\ -\mathrm{I}& {\mathrm{R}}_{{\mathrm{C}}_0}^{{\mathrm{B}}_{\mathrm{t}}}{\mathrm{R}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{C}}_0}& \begin{array}{cc}{\mathrm{R}}_{{\mathrm{C}}_0}^{{\mathrm{B}}_{\mathrm{t}}}\Delta \mathrm{t}& 0\end{array}\end{array}\right]$$

(24)

In the above formula ${\mathrm{R}}_{{\mathrm{B}}_{\mathrm{t}}}^{{\mathrm{C}}_0}$, ${\mathrm{R}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{C}}_0}$,${\mathrm{P}}_{{\mathrm{B}}_{\mathrm{t}}}^{{\mathrm{C}}_0}$, ${\mathrm{P}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{C}}_0}$ can be obtained through visual SfM:

$${}_{{\mathrm{X}}_{\mathrm{I}}}{}^{\min }{{\displaystyle \sum }}^{\text{}}\Vert {\tilde{\mathrm{Z}}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}-{\mathrm{H}}_{{\mathrm{B}}_{\mathrm{t}+1}}^{{\mathrm{B}}_{\mathrm{t}}}{\mathrm{X}}_{\mathrm{I}}\Vert {}^2$$

(25)

Solving the above formula, we can calibrate the initial velocity for each keyframe, gravity vector, and absolute metric scale. After estimating the scale, we will adjust the translation vector of the vision SfM to make the system have an observable scale.

3.3.3. Gravity Vector Refinement

We can know the magnitude of the gravity vector in advance, so we refer to the VINS-Mono [18] method to re-parameterize the gravity vector obtained in Section 3.3.2 with two variables in tangent space, and perform further optimization.

After obtaining the accurate gravity vector, we can rotate the coordinate system ${\mathrm{C}}_0$, which is temporarily the world coordinate system, to the real world coordinate system W. However, since the yaw angle in the visual-inertial SLAM system is unobservable, the yaw angle of the ${\mathrm{C}}_0$ coordinate system remains unchanged during the rotation process. At this time, the initialization procedure of the semi-direct visual-inertial SLAM system is completed.

4. Visual Measurements

4.1. Keyframe Selection

We have three different keyframe selection strategies. Satisfying one of these three strategies makes the current frame a keyframe. The first and third strategies of keyframe selection are based on the feature matching results of the last frame and the penultimate frame in the sliding window, which has been matched before the current frame arrives. The first selection strategy is the tracking number of feature points. No new feature points will be extracted when tracking non-keyframes. The translational motion of the camera will lead to the decrease of tracking feature points. If the number of tracking points in the last frame in the sliding window is less than 70% of the minimum tracking point threshold, the current frame will be treated as a keyframe. The second selection strategy is related to IMU pre-integration. If the translation distance between the last two adjacent frames in the sliding window calculated by the IMU pre-integration exceeds a preset threshold, the current frame is also considered as a new keyframe. The third selection strategy is the average parallax of the feature points tracked on the the last frame and the penultimate frame in the sliding window. The translation or rotation of camera will cause parallax. When the average parallax exceeds the threshold, the current frame will also be regarded as a keyframe.

4.2. Keyframes Tracking

If the current frame is treated as a keyframe, we first use the fast feature detector [28] to add new feature points in the last frame in the sliding window and then use the KLT sparse optical flow algorithm to track them in the current frame (Figure 3). At least 200 feature points will be maintained in each frame. Since there is no need to calculate the feature point descriptor, the optical flow method can save more time. In addition, we also use RANSAC [29] with the fundamental matrix model to eliminate outliers generated during tracking.

4.3. Non-Keyframes Tracking

If the current frame is considered a non-keyframe, we use direct image alignment to estimate the relative pose ${\mathrm{T}}_{t,t+1}$ between the current frame and the last frame in the sliding window. The initial value of the relative pose can be obtained directly by IMU pre-integration. The feature points observed in the last frame are projected into the current frame according to the estimated pose ${\mathrm{T}}_{t,t+1}$. Due to the hypothesis of photometric invariance, if the same feature point is observed by two adjacent frames, the photometric values of the projection points on the two adjacent frames are equal (Figure 4). Therefore, we can optimize the relative pose ${\mathrm{T}}_{t,t+1}$ by minimizing the photometric error between image blocks (4 × 4 pixels):

$$T_{t,t+1}=\arg \underset{T}{\min }{{\displaystyle \iint }}_R^{ }\rho \left[\delta I\left(T,u\right)\right]du$$

(26)

where $\rho [·]=\frac{1}{2}{\Vert ·\Vert }^2$, $u$ is the position of the feature point of the last frame in the sliding window. R is the image region where the depth is known in the last frame, and the back-projected points are visible in the current frame domain.

The photometric error $\delta I$ is:

$$\delta I\left(T_{t,t+1},u\right)=I_{t+1}\left(\mathsf{\pi }{T}_{t,t+1}·{\mathsf{\pi }}^{-1}\left(u,d_p\right)\right)-I_t\left(u\right) \forall u\in R$$

(27)

where $d_p$ is the depth of the feature point in the last frame in the sliding window. $I_k$ represents the intensity image in the k-th frame.

We use the inverse compositional formulation [30] of the photometric error, which can avoid unnecessary Jacobian derivation. The update step $\mathrm{T}\left(\mathsf{\xi }\right)$ for the last frame in the sliding window is:

$$\delta I\left(\mathsf{\xi },u\right)=I_{t+1}\left(\mathsf{\pi }{T}_{t,t+1}·{\mathsf{\pi }}^{-1}\left(u,d_p\right)\right)-I_t\left(\mathsf{\pi }\left(T\left(\mathsf{\xi }\right)·{\mathsf{\pi }}^{-1}\left(u,d_p\right)\right)\right) \forall u\in R$$

(28)

We solve it in an iterative Gauss Newton method and update ${\mathrm{T}}_{t,t+1}$ in the following way:

$$T_{t,t+1}\leftarrow T_{t,t+1}·\mathrm{T}{\left(\mathsf{\xi }\right)}^{-1}$$

(29)

After image alignment, we can get the optimized relative pose ${\mathrm{T}}_{t,t+1}$ between the current frame and the last frame in the sliding window. We define all 3D points observed in all frames in the sliding window as the local map, and project the local map to the current frame to find the visible 3D points of the current frame. Due to the inaccuracy of the visible 3D point position and the camera pose, there will be errors in the projection position of the current frame. To make the projection position more accurate, the current frame needs to be aligned with the local map. The feature matching step optimizes the positions of all the projection points in the current frame by minimizing the photometric errors of the projection blocks (5 × 5 pixels) in the current frame and the reference frame (Figure 5):

$$u_i^{\prime }=\arg \underset{u_i^{\prime }}{\min }\frac{1}{2}\Vert I_{t+1}\left(u_i^{\prime }\right)-A_i·I_i{\left(u_i^{ }\right)\Vert }^2 \forall i$$

(30)

Solving the above formula in an iterative Gauss Newton method, we can get the update of the projection block position. The reference frame is usually far away from the current frame, so we apply an affine warping $A_i$ to the reference patch.

Through image alignment and feature matching, we get the implicit results of direct motion estimation — feature correspondence with sub-pixel accuracy. Note that when tracking non-keyframes with the direct method, no new feature points are extracted. In the back-end optimization, we will combine IMU residual, visual re-projection error, prior information, and re-localization information to optimize the camera pose and 3D point position again.

5. Sliding Window-based Tightly-coupled Optimization Framework

After tracking non-keyframes and keyframes, we proceed with a sliding window-based tightly-coupled optimization framework for high accuracy and robust state estimation. In the optimization framework, we combined IMU residual, visual re-projection error, prior information, and re-localization information to optimize the camera pose and 3D point position again.

5.1. Formulation

The state variables to be estimated by SD-VIS are defined as:

$$\begin{gathered} \mathrm{X}=\left[\begin{array}{ccc}{\mathrm{X}}_0& {\mathrm{X}}_1& \begin{array}{ccc}\dots & {\mathrm{X}}_{\mathrm{n}}& {\mathrm{X}}_{\mathrm{C}}^{\mathrm{B}}\end{array}\end{array}\right] \\ {\mathrm{X}}_{\mathrm{k}}=\left[\begin{array}{ccc}{\mathrm{P}}_{{\mathrm{B}}_{\mathrm{k}}}^{\mathrm{W}}& {\mathrm{V}}_{{\mathrm{B}}_{\mathrm{k}}}^{\mathrm{W}}& \begin{array}{ccc}{\mathrm{q}}_{{\mathrm{B}}_{\mathrm{k}}}^{\mathrm{W}}& {\mathrm{b}}_{\mathsf{\alpha }}^{\mathrm{B}}& {\mathrm{b}}_{\mathsf{\omega }}^{\mathrm{B}}\end{array}\end{array}\right]\hspace{1em}\hspace{1em}\mathrm{k}\in \left[0,\mathrm{n}\right] \\ {\mathrm{X}}_{\mathrm{C}}^{\mathrm{B}}=\left[\begin{array}{cc}{\mathrm{P}}_{\mathrm{C}}^{\mathrm{B}}& {\mathrm{q}}_{\mathrm{C}}^{\mathrm{B}}\end{array}\right] \end{gathered}$$

(31)

where ${\mathrm{X}}_{\mathrm{k}}$ includes the translation, velocity, and rotation quaternions of the ${\mathrm{k}}^{\mathrm{th}}$ IMU body coordinate system concerning the world coordinate system, as well as the bias of gyroscope and accelerometer. $\mathrm{n}$ represents the size of the sliding window. By minimizing the sum of IMU residuals, visual re-projection errors, prior information, and relocation information in the sliding window, we can obtain a robust and accurate semi-direct visual-inertial SLAM system:

$${}_{\mathrm{X}}{}^{\min }\left\{{{\displaystyle \sum }}^{\text{}}\Vert {\mathrm{r}}_{\mathrm{B}}{\left({\tilde{\mathrm{Z}}}_{{\mathrm{B}}_{\mathrm{k}+1}}^{{\mathrm{B}}_{\mathrm{k}}},\mathrm{X}\right)\Vert }_{{\mathrm{P}}_{{\mathrm{B}}_{\mathrm{k}+1}}^{{\mathrm{B}}_{\mathrm{k}}}}^2+{{\displaystyle \sum }}^{\text{}}\Vert {\mathrm{r}}_{\mathrm{C}}{\left({\tilde{\mathrm{Z}}}_{\mathrm{F}}^{{\mathrm{C}}_{\mathrm{j}}},\mathrm{X}\right)\Vert }_{{\mathrm{P}}_{\mathrm{F}}^{{\mathrm{C}}_{\mathrm{j}}}}^2+\Vert {\mathrm{r}}_{\mathrm{p}}-{\mathrm{H}}_{\mathrm{p}}\mathrm{X}{\Vert }^2+{{\displaystyle \sum }}^{\text{}}\Vert {\mathrm{r}}_{\mathrm{C}}{\left({\tilde{\mathrm{Z}}}_{\mathrm{F}}^{{\mathrm{C}}_{\mathrm{L}}},\mathrm{X}\right)\Vert }_{{\mathrm{P}}_{\mathrm{F}}^{{\mathrm{C}}_{\mathrm{L}}}}^2\right\}$$

(32)

where ${\mathrm{r}}_{\mathrm{B}}\left({\tilde{\mathrm{Z}}}_{{\mathrm{B}}_{\mathrm{k}+1}}^{{\mathrm{B}}_{\mathrm{k}}},\mathrm{X}\right)$, ${\mathrm{r}}_{\mathrm{C}}\left({\tilde{\mathrm{Z}}}_{\mathrm{F}}^{{\mathrm{C}}_{\mathrm{j}}},\mathrm{X}\right)$, $\left\{{\mathrm{r}}_{\mathrm{p}},{\mathrm{H}}_{\mathrm{p}}\right\}$ and ${\mathrm{r}}_{\mathrm{C}}\left({\tilde{\mathrm{Z}}}_{\mathrm{F}}^{{\mathrm{C}}_{\mathrm{L}}},\mathrm{X}\right)$ are IMU residuals, visual re-projection errors, prior information and re-localization information respectively.

5.2. IMU Residuals

According to the formulas (4)–(6) in Section 3.2, we can get the IMU measurement residual:

$${\mathrm{r}}_{\mathrm{B}}\left({\tilde{\mathrm{Z}}}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}},\mathrm{X}\right)=\left[\begin{array}{c}{\mathsf{\delta }\mathsf{\theta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}\\ {\mathsf{\delta }\mathsf{\beta }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}\\ {\mathsf{\delta }\mathsf{\gamma }}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}\\ {\mathsf{\delta }\mathrm{b}}_{\mathsf{\alpha }}^{\mathrm{B}}\\ {\mathsf{\delta }\mathrm{b}}_{\mathsf{\omega }}^{\mathrm{B}}\end{array}\right]={\left[\begin{array}{c}{\mathrm{R}}_{\mathrm{W}}^{{\mathrm{B}}_{\mathrm{i}}}\left({\mathrm{P}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}-{\mathrm{P}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}-{\mathrm{V}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}\mathsf{\Delta }\mathrm{t}+\frac{1}{2}{\mathrm{g}}^{\mathrm{W}}{\mathsf{\Delta }\mathrm{t}}^2\right)-{\tilde{\mathsf{\theta }}}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}\\ {\mathrm{R}}_{\mathrm{W}}^{{\mathrm{B}}_{\mathrm{i}}}\left({\mathrm{V}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}-{\mathrm{V}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}+{\mathrm{g}}^{\mathrm{W}}{\mathsf{\Delta }\mathrm{t}}^{}\right)-{\tilde{\mathsf{\beta }}}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}\\ 2{\left[{\mathrm{q}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}{}^{-1}\otimes {\mathrm{q}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}}\otimes {\left({\tilde{\mathsf{\gamma }}}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}\right)}^{-1}\right]}_{\mathrm{xyz}}\\ {\mathrm{b}}_{\mathsf{\alpha }}^{{\mathrm{B}}_{\mathrm{i}}}-{\mathrm{b}}_{\mathsf{\alpha }}^{{\mathrm{B}}_{\mathrm{j}}}\\ {\mathrm{b}}_{\mathsf{\omega }}^{{\mathrm{B}}_{\mathrm{i}}}-{\mathrm{b}}_{\mathsf{\omega }}^{{\mathrm{B}}_{\mathrm{j}}}\end{array}\right]}_{15*1}$$

(33)

where ${[·]}_{\mathrm{xyz}}$ represents the real part of the quaternion. ${\left[{\tilde{\mathsf{\theta }}}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}},{\tilde{\mathsf{\beta }}}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}},{\tilde{\mathsf{\gamma }}}_{{\mathrm{B}}_{\mathrm{j}}}^{{\mathrm{B}}_{\mathrm{i}}}\right]}^{\mathrm{T}}$ are the IMU pre-integration between two adjacent keyframes ${\mathrm{B}}_{\mathrm{i}}$ and ${\mathrm{B}}_{\mathrm{j}}$.

5.3. Visual Re-Projection Errors

When the feature point ${\mathrm{F}}_1$ is first observed in the ${\mathrm{i}}^{\mathrm{th}}$ image, the visual re-projection error in the ${\mathrm{j}}^{\mathrm{th}}$ image can be defined by the pinhole camera model as:

$${\mathrm{r}}_{\mathrm{C}}\left({\tilde{\mathrm{Z}}}_{{\mathrm{F}}_1}^{{\mathrm{C}}_{\mathrm{j}}},\mathrm{X}\right)={\mathsf{\pi }}^{-1}(\left[\begin{array}{c}{\tilde{\mathrm{u}}}_{{\mathrm{F}}_1}^{{\mathrm{C}}_{\mathrm{i}}}\\ {\tilde{\mathrm{v}}}_{{\mathrm{F}}_1}^{{\mathrm{C}}_{\mathrm{i}}}\end{array}\right]-{\mathrm{R}}_{\mathrm{B}}^{\mathrm{C}}({\mathrm{R}}_{\mathrm{W}}^{{\mathrm{B}}_{\mathrm{j}}}({\mathrm{R}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}({\mathrm{R}}_{\mathrm{C}}^{\mathrm{B}}{\mathsf{\pi }}^{-1}\left[\begin{array}{c}{\tilde{\mathrm{u}}}_{{\mathrm{F}}_1}^{{\mathrm{C}}_{\mathrm{j}}}\\ {\tilde{\mathrm{v}}}_{{\mathrm{F}}_1}^{{\mathrm{C}}_{\mathrm{j}}}\end{array}\right]+{\mathrm{P}}_{\mathrm{C}}^{\mathrm{B}})+{\mathrm{P}}_{{\mathrm{B}}_{\mathrm{i}}}^{\mathrm{W}}-{\mathrm{P}}_{{\mathrm{B}}_{\mathrm{j}}}^{\mathrm{W}})-{\mathrm{P}}_{\mathrm{C}}^{\mathrm{B}})$$

(34)

where $\left[{\tilde{\mathrm{u}}}_{{\mathrm{F}}_1}^{{\mathrm{C}}_{\mathrm{i}}},{\tilde{\mathrm{v}}}_{{\mathrm{F}}_1}^{{\mathrm{C}}_{\mathrm{i}}}\right]$ and $\left[{\tilde{\mathrm{u}}}_{{\mathrm{F}}_1}^{{\mathrm{C}}_{\mathrm{j}}},{\tilde{\mathrm{v}}}_{{\mathrm{F}}_1}^{{\mathrm{C}}_{\mathrm{j}}}\right]$ represent the coordinates of the pixels projected from the feature point ${\mathrm{F}}_1$ to the ${\mathrm{i}}^{\mathrm{th}}$ and ${\mathrm{j}}^{\mathrm{th}}$ frame image, respectively. ${\mathsf{\pi }}^{-1}$ is the back-projection function of the pinhole camera model.

5.4. Marginalization Strategy

In order to limit the computational complexity of SD-VIS, the back-end adopts a sliding window-based tightly-coupled optimization framework, so we use the marginalization strategy [31] to make the correct operation of sliding windows. As shown in Figure 6, if the current frame is determined as a keyframe, the frame will remain in the sliding window, and the oldest frame is marginalized out When the oldest frame is marginalized, the feature points that can only be observed by the oldest frame will be discarded directly, and other visual and inertial measurements associated with the frame will be removed from the sliding window by Schur complement. The new prior information constructed by Schur complement will be added to the existing prior information. If the current frame is not a keyframe, the last frame in the sliding window will be marginalized, and all visual measurements related to that frame will be removed directly from the sliding window.

5.5. Re-Localization

Due to the global 3D position and yaw angle are unobservable, there will be inevitable accumulative errors in the vision-inertial SLAM system. To eliminate the accumulated error, we introduce the re-localization module. After the keyframe is traced successfully, it can be judged whether the SLAM system has been here before by loop closure detection. We utilize DBoW2, a state-of-the-art bag-of-word place recognition approach, for loop closure detection. When a loop is detected, the re-localization module can effectively align the current sliding window, thus eliminating the accumulated error. For a detailed description of re-location, readers may refer to [18].

6. Experiment

We evaluate the accuracy, robustness, and real-time performance of SD-VIS on the EuRoC dataset [32]. The SD-VIS method is compared with the state-of-the-art vision SLAM methods, such as VINS-mono [18] and VINS-Fusion [20]. In Section 6.1, the accuracy and robustness of SD-VIS are evaluated, and the experimental results show that the accuracy and robustness of the proposed method reach the same level as the state-of-the-art method. Section 6.2 evaluates real-time performance. The experimental results show that the proposed method achieves a good balance between accuracy and real-time performance. Section 6.3 evaluates the loop closure detection capability and verifies the overall feasibility of the SLAM system.

6.1. Accuracy and Robustness Evaluate

In the experiments on the EuRoC dataset, we adopt the open source tool EVO [33] to evaluate the performance of SD-VIS. By comparing the estimated value with the actual value, we calculate the absolute pose error (APE) as an index of the evaluation algorithm [34].

Table 1. The root mean square error (RMSE) results of the translation (m).

Dataset	VINS-Mono	VINS-Fusion	SD-VIS
MH_01_easy	0.254651	0.364247	0.260793
MH_02_easy	0.263258	0.339122	0.289663
MH_03_medium	0.547901	0.483257	0.577422
MH_04_medium	0.590191	0.614950	0.497288
MH_05_difficult	0.512011	0.524107	0.512458
V1_01_easy	0.217083	0.247467	0.245990
V1_02_medium	0.492645	0.434756	0.502134
V1_03_difficult	0.361521	0.345895	0.388959
V2_01_easy	0.170790	0.177467	0.202474
V2_02_medium	0.424259	0.370081	0.454785
V2_03_difficult	0.475561	0.521573	0.444759

shows the root mean square error (RMSE) of the translation on the EuRoC dataset. For fairness, the following algorithms do not use the loop closure detection module.

As can be seen from Table 1, in terms of accuracy, SD-VIS and VINS-Mono and VINS-Fusion are at the same level. The accuracy of SD-VIS is slightly lower than that of VINS-Mono when moving at low and medium speed in the environment with abundant feature points (such as MH_01_easy and MH_03_medium). This is due to the susceptibility to various illumination changes when tracking non-keyframes using the direct method. We have observed that some datasets exhibit strong exposure changes between images and, therefore, the tracking effect of the direct method is reduced in these cases. In addition, in order to further improve the real-time performance of the algorithm, we only extract new feature points in keyframes, which results in that the number of feature points in non-keyframes will be less than keyframes, which will also bring some negative effects. Although the accuracy performance is not significantly better than the traditional method, it has achieved the same level of accuracy as VINS-Mono while greatly improving real-time performance. Therefore, our algorithm is very suitable for small-sized unmanned platform with limited computing resources. The accuracy of SD-VIS is higher than that of VINS-Mono and VINS-Fusion when moving fast in the low-texture environment (such as V2_03_difficult). This is due to the excellent performance of the direct method in the low-texture environment. In addition, the keyframe selection strategy will tend to generate more keyframes during fast motion, which will also improve the accuracy performance of the algorithm. Figure 7 shows more intuitively the trajectory heat map estimated by SD-VIS, VINS-Mono, and VINS-Fusion in MH_01_easy. Figure 8 shows the change of translation absolute pose error with time in MH_01_easy, MH_04_medium, and V2_03_difficult. Through Figure 7 and Figure 8, we came to the conclusion that the accuracy and robustness of our algorithm have reached the level of the state-of-the-art algorithm. Especially in the initialization procedure and low-texture environment, our algorithm performs better.

6.2. Real-Time Performance Evaluate

In this section, we evaluate the real-time performance of SD-VIS. We compared the average time required to track an image (

Table 2. Average time (ms) spent tracking an image.

Dataset	ORB-SLAM2	VINS-Mono	VINS-Fusion	SD-VIS
MH_01_easy	37.82	17.67	38.91	6.72
MH_02_easy	35.31	13.60	45.86	6.58
MH_03_medium	34.20	12.28	44.15	7.87
MH_04_medium	30.35	12.92	42.54	6.93
MH_05_difficult	30.43	12.77	42.65	6.91
V1_01_easy	37.15	13.38	45.87	7.14
V1_02_medium	28.46	13.89	45.36	10.96
V1_03_difficult	×	13.50	44.40	7.81
V2_01_easy	33.01	15.30	49.10	6.28
V2_02_medium	31.13	13.04	45.56	10.22
V2_03_difficult	×	17.90	45.07	12.38

).

As can be seen from Table 2, the image tracking of ORB-SLAM2 [11] uses the feature-based method to extract and match the ORB features of each frame, which takes a long time. However, VINS-Mono uses the optical flow method to track FAST features, which saves the calculation of feature descriptors, so the time consumption is less than ORB-SLAM2. VINS-Fusion is a stereo visual-inertial fusion SLAM algorithm, and image tracking also takes a long time. In SD-VIS, non-keyframes are used for fast-tracking and localization by direct method, and keyframes are tracked by feature-based method and used for back-end optimization and loop closure detection. This algorithm saves a lot of time and minimizes the average time of SD-VIS tracking images. Due to the keyframe selection strategy, in some low-speed motion scenes (such as MH_01_easy and V1_01_easy), the number of keyframes will be less, and the time to track a frame of the image will be reduced. In some fast-moving scenes (such as V1_02_medium and V2_03_difficult), as the number of keyframes increases, the time required to track an image will be increased.

In summary, the reason why we can obtain good real-time performance is due to the use of KLT sparse optical flow algorithm when tracking keyframes, which eliminates the calculation of descriptors and feature matching. In addition, for non-keyframes, only the direct method is used to track existing feature points, and new feature points are not extracted. Due to the close distance between two adjacent non-keyframes, the direct method of image alignment and feature matching can quickly converge.

Compared with the feature-based method, we use the direct method to track non-keyframes and accelerate the algorithm without reducing the accuracy and robustness. As shown in Figure 9, in MH_02_easy, 26% of the frames are determined to be keyframes, while 74% of the frames are determined to be non-keyframes. The time consumption of tracking keyframes is 65%, while that of non-keyframes are only 35%. Combined with Section 6.1, we can conclude that compared with the state-of-the-art SLAM systems, we can achieve a better balance between quickness and exactness.

6.3. Loop Closure Detection Evaluate

Finally, in order to verify the integrity and feasibility of the proposed algorithm, we evaluate the loop closure detection capability of SD-VIS. As can be seen from Figure 10 and Figure 11, the accuracy of SD-VIS with loop detection is improved obviously. Compared with the direct method, SD-VIS exhibits the function of loop closure detection and solves the problem of drift in long-term operation.

7. Conclusions

We present SD-VIS, a novel fast and accurate semi-direct visual-inertial SLAM framework, which combines the exactness of feature-based method and quickness of direct method. Compared with the state-of-the-art feature-based method, we use the direct method to track non-keyframes and accelerate the algorithm without reducing the accuracy and robustness. Compared with the direct method, SD-VIS exhibits the function of loop closure detection and solves the problem of drift in long-term operation. We get a better balance between accuracy and speed, so the algorithm is more suitable for the platform with limited computing resources. In the future, we will extend the algorithm to support more types of multi-sensor fusion to increase its robustness in complex environments.