3.1. IMU preintegration

The raw measurements of an IMU primarily consist of the three-axis acceleration $\hat {\mathbf{a}}_t$ and angular velocity $\hat {\boldsymbol{\omega }}_t$ , both of which are in the IMU body frame. These measurements are influenced by $\mathbf{b}_a$ , $\mathbf{b}_{\omega _t}$ , and additive Gaussian white noise. Short-term IMU measurements can be used for pose prediction, and the IMU continuous-time kinematics integration model is given as follows:

(2) \begin{align} \mathbf{p}_{b_{k+1}}^w &= \mathbf{p}_{b_{k}}^w + \mathbf{v}_{b_k}^w \delta t + \iint _{t_k}^{t_{k+1}} [\mathbf{R}_t^{w}(\hat {\mathbf{a}}_t-\mathbf{b}_{a_t}) - \mathbf{g}^w]dt^2, \nonumber \\ \mathbf{v}_{b_{k+1}}^w &= \mathbf{v}_{b_{k}}^w + \int _{t_k}^{t_{k+1}} [\mathbf{R }_t^{w}(\hat {\mathbf{a}}_t-\mathbf{b}_{a_t}) - \mathbf{g}^w]dt, \nonumber \\ \mathbf{q}_{b_{k+1}}^w &= \mathbf{q}_{b_{k}}^w \otimes \int _{t_k}^{t_{k+1}} \frac {1}{2} \Omega (\hat {\boldsymbol{\omega }}_t - \mathbf{b}_{\omega _t})\mathbf{q}_t^{b_k}dt, \end{align}

where

(3) \begin{equation} \boldsymbol{\Omega }(\boldsymbol{\omega }) = \begin{bmatrix} -[\boldsymbol{\omega }]_\times & \boldsymbol{\omega } \\ -\boldsymbol{\omega }^T & 0 \end{bmatrix}, \quad [\boldsymbol{\omega }]_\times = \begin{bmatrix} 0 & -\omega _z & \omega _y \\ \omega _z & 0 & -\omega _x \\ -\omega _y & \omega _x & 0 \end{bmatrix}. \end{equation}

In the back-end optimization, the estimation of state vector $\mathcal{X}$ is adjusted to minimize the IMU and visual measurement residuals. It is evident from (2) that changes in the state variables necessitate the repropagation of the integral terms. To avoid this, we apply preintegration [Reference Shen, Michael and Kumar21] to all IMU measurements between consecutive image frames to estimate their incremental motion. Specifically, by applying $R_w^{b_k}$ to both sides of (2), we decouple the integral term on the right side from the state variables, with the preintegration terms detailed as follows:

(4) \begin{align} \mathbf{\alpha } _{b_{k+1}}^{b_k} &= \iint _{t_k}^{t_{k+1}} \mathbf{R}_t^{b_k}(\hat {\mathbf{a}}_t-\mathbf{b}_{a_t})dt^2, \nonumber \\ \mathbf{\beta } _{b_{k+1}}^{b_k} &= \int _{t_k}^{t_{k+1}} \mathbf{R}_t^{b_k}(\hat {\mathbf{a}}_t-\mathbf{b}_{a_t})dt, \nonumber \\ \mathbf{\gamma } _{b_{k+1}}^{b_k} &= \int _{t_k}^{t_{k+1}} \frac {1}{2} \mathbf{\Omega }(\hat {\boldsymbol{\omega }}_t-\mathbf{b}_{\omega _t}) \mathbf{\gamma } _{t}^{b_k} dt^2. \end{align}

3.2. Lite-RAFT feature tracking

Feature association in dynamic illumination environments is challenging. To mitigate sensitivity to these variations, we design the Lite-RAFT, a lightweight deep network for optical flow estimation, inspired by the RAFT [Reference Teed and Deng17] and ThermalRAFT [Reference Jiang, Chen, Dai, Gao and Zhang22]. Due to the lightweight design, the proposed network can achieve real-time operations within the SLAM system. It comprises three primary components, as shown in Figure 3, including the feature encoder, the correlation volumes, and the feature decoder.

Figure 3.

Structure of the proposed Lite-RAFT.

We employ a weight-sharing feature encoder to extract high-dimensional features from the input image pairs, producing feature maps $F_1, F_2\in \mathbb{R} ^{H\times W\times D}$ , where $H$ , $W$ , and $D$ denote height, width, and feature dimension, respectively. These extracted features are then added with positional encoding [Reference Carion, Massa, Synnaeve, Usunier, Kirillov and Zagoruyko23], allowing for the consideration of both feature similarity and their spatial distance in subsequent similarity calculations. Additionally, a separate context network with the identical structure as the feature encoder processes the first image to produce a context map, which is used as the input for the feature decoder.

For the two-layer correlation volumes, the first layer with dimension $H\times W\times H\times W$ is constructed by taking the dot product between every pair of feature vectors, quantifying the visual similarity of two feature maps. The second layer is obtained by downsampling the first layer. To be specific, the last two dimensions of the first layer are pooled using a kernel with the size of 4 and an equivalent stride, then the second layer with dimension $H\times W\times H/4\times W/4$ is obtained. This two-layer correlation volumes we constructed can maintain sensitivity to both large and small displacements. At the same time, it reduces the amount of computation and memory consumption compared to the original RAFT. Note that the lookup radius for the search of correlation information in the correlation volumes is always 4.

The feature decoder estimates the residual flow iteratively, starting with an initial flow set to zero. The context map, previously estimated flow, and indexed correlation volumes are concatenated as the input of the feature decoder. Then, the input passes through four convolutional layers and the residual flow $\Delta {\mathbf{f}}$ is obtained as the update term. This term is added to the current flow, which will be fed into the next iteration. Following multiple iterations, the estimated flow undergoes upsampling to match the resolution of the original image using a mask network.

Introducing the aforementioned three components results in a slight reduction in accuracy for Lite-RAFT compared to the original RAFT model. However, it yields a significant improvement in inference speed. By choosing proper number of iterations for the feature decoder, the balance between the accuracy and computational cost can be improved.

The designed Lite-RAFT is trained using an RTX 4070 GPU. To be specific, we pretrain the network on the FlyingChairs dataset [Reference Dosovitskiy, Fischer, Ilg, Hausser, Hazirbas, Golkov, Van Der Smagt, Cremers and Brox13] for 120k iterations with a batch size of 8, and train for 120k iterations on the FlyingThings3D dataset [Reference Mayer, Ilg, Hausser, Fischer, Cremers, Dosovitskiy and Brox24] with a batch size of 5. We then finetune on the Sintel dataset [Reference Butler, Wulff, Stanley and Black25] for another 120k iterations. The pretraining process improves the convergence of the training process of the Lite-RAFT. During the training, we supervise our network using the loss introduced in RAFT [Reference Teed and Deng17], which is given as

(5) \begin{equation} L = \sum _{i=1}^{N}\gamma ^{N-i}\left \| \mathbf{f}_{\text{true}} - \mathbf{f}_i \right \| _1, \end{equation}

where $\mathbf{f}_{\text{true}}$ denotes the ground truth flow and $\mathbf{f}_i$ represents the estimated flow at the $i$ th iteration. The weight factor is set to $\gamma = 0.8$ , implying that the optical flows obtained in the later iterations have higher weights in the overall loss function.

Since the proposed Lite-RAFT can provide efficient and accurate dense optical flow estimation of every integer pixel under dynamic illumination conditions, we employ bilinear interpolation to derive displacement information for tracking existing non-integer features. Furthermore, if the count of features within the field of view falls below a preset threshold during the tracking process, we detect the Shi-Tomas corners [Reference Shi and Tomasi26] and set a minimum separation of corners to ensure a uniform distribution.

3.3. Moving consistency check

Although the feature tracking performance is significantly improved via using the Lite-RAFT, the tracking errors still exist in certain cases, such as image noise or optical flow prediction inaccuracies in dynamic illumination scenarios. To tackle these issues, we design a moving consistency check strategy within the RAFT-VINS, leveraging the optimized poses within the sliding window to calculate multi-frame reprojection errors. The process to obtain the reprojection errors is illustrated in Figure 4. Considering the $k$ th feature, which is initially observed in the $i$ th frame and contained in the subsequent $n-1$ frames in the sliding windows, we reproject the first observation onto other frames, and the reprojection error is defined as

(6) \begin{align} r_{k} =\frac {1}{n-1} \sum _{j\ne i} \left \| \hat {\bar {P}}^{c_j}_k -\frac {P_k^{c_j}}{[P_k^{c_j}]_z} \right \| _2, \end{align}

where

\begin{align*} P_k^{c_j} = T_{b}^{c}T_{w}^{b_j}T_{b_i}^{w}T_{c}^{b}\frac {\hat {\bar {P}}^{c_i}_k}{\lambda }. \end{align*}

$\lambda$ is the inverse depth of the $k$ th feature. $\hat {\bar {P}}^{c_i}_k$ and $\hat {\bar {P}}^{c_j}_k$ denote the observed normalized coordinates of the $k$ th feature in the $i$ th and $j$ th camera frame, respectively. $T_{c}^{b}$ is the extrinsic parameter that represents the transformation from the camera frame to body frame. $T_{b_i}^{w}$ and $T_{w}^{b_j}$ represent the inverse transformation matrices between the body frame and world frame at $i$ th and $j$ th frame, respectively. When the residual $r_k$ is over a preset threshold, the $k$ th feature is considered as an outlier point and removed from the VIO system.

Figure 4.

Illustration of the moving consistency check strategy. The red pentagon represents a feature, which is first observed in the $i$ th frame. The black arrow represents the projection process, the yellow points are the projected results, and the black points are the actual observations of the landmark in corresponding image frame.

3.4. Tightly-coupled VIO

A tightly-coupled optimization-based framework is used to estimate the state vector $\mathcal{X}$ by fusing IMU and visual measurements and minimizing total residuals. The overall objective function for the joint optimization is given as follows:

(7) \begin{equation} \underset {\mathcal{X}}{\textrm{min}} \left \{ \sum || e_{\mathcal{B}} (\mathcal{X})||^2_{\Sigma _b} + \sum \rho || e_{\mathcal{C}}(\mathcal{X})||^2_{\Sigma _c} + \left \|I_p(\mathcal{X}) \right \|^2 \right \}, \end{equation}

where $e_{\mathcal{B}}$ is the IMU residual. With the IMU preintegration introduced in Section 3.1, we can define the IMU residual between two body frames $b_k$ and $b_{k+1}$ as follows:

(8) \begin{align} e_{\mathcal{B}} = \begin{bmatrix} \mathbf{R}_w^{b_k}(\mathbf{p}_{b_{k+1}}^w-\mathbf{p}_{b_{k}}^w - \mathbf{v}_{b_{k}}^w t - \mathbf{g}^w t^2 ) - \mathbf{\alpha } _{b_{k+1}}^{b_k} \\[8pt] \mathbf{R}_w^{b_k}(\mathbf{v}_{b_{k+1}}^w-\mathbf{v}_{b_{k}}^w - \mathbf{g}^w \delta t ) - \mathbf{\beta } _{b_{k+1}}^{b_k} \\[8pt] 2\left [ \mathbf{q}_w^{b_k} \otimes \mathbf{q}_{b_{k+1}}^w \otimes (\mathbf{\gamma } _{b_{k+1}}^{b_k})^{-1} \right ]_{xyz} \\[8pt] b_{a_{b_{k+1}}} - b_{a_{b_{k}}} \\[8pt] b_{g_{b_{k+1}}} - b_{g_{b_{k+1}}} \end{bmatrix}, \end{align}

where $t$ is the time interval between two consecutive image frames. $[\stackrel{{}_{\circ}}{{\textrm{u}}}]_{xyz}$ represents the vector part of the quaternion. $e_{\mathcal{C}}$ is the visual measurement residuals, its calculation process is the same as the projection process in Section 3.3. $\Sigma _b$ and $\Sigma _c$ are the corresponding covariance matrices. We invert covariance matrices to obtain the corresponding information matrices, which are then used to characterize uncertainty and are multiplied by the residuals. $I_p$ is the prior information generated during the marginalization process. For more details about $e_{\mathcal{B}}$ and $I_p$ , please refer to ref. [Reference Qin, Li and Shen5]. $\rho$ denotes the Huber loss function [Reference Huber27] and is defined as

(9) \begin{equation} \rho (s) = \begin{cases} s, & s \leq 1; \\ 2\sqrt {s} - 1, & s \gt 1. \end{cases} \end{equation}

This loss function is utilized to effectively suppress large residuals caused by incorrect feature associations, without overly suppressing them, while maintaining a balanced influence on the reprojection error. Equation (7) fundamentally represents a nonlinear least-squares problem, and the Ceres Solver [Reference Agarwal and Mierle28] is employed for its iterative solution.