Toward Consistent Drift-free Visual Inertial Localization on Keyframe Based Map

When we receive IMU data at time step $k$, we could employ the IMU kinematics equations to predict the body state at time step $k+1$:

$$\mathbf{x}_{k+1|k}=\mathbf{f}(\mathbf{x}_{k},\mathbf{a}_{m_{k}}-\mathbf{n}_{a_{k}},\mathbf{\omega}_{m_{k}}-\mathbf{n}_{\omega_{k}}),$$

(4)

where $\mathbf{f}$ represents the IMU kinematics equations, $\mathbf{a}_{m_{k}}$ and $\mathbf{\omega}_{m_{k}}$ are the observations (linear acceleration and angular velocity) from the IMU. $\mathbf{n}_{a_{k}}$ and $\mathbf{n}_{\omega_{k}}$ are the corresponding zero-mean Gaussian white noise of the IMU observations. With this function, we could propagate the state covariance by

$$\mathbf{P}_{k+1|k}=\mathbf{\Phi}_{k}\mathbf{P}_{k|k}\mathbf{\Phi}_{k}^{\top}+\mathbf{G}_{k}\mathbf{Q}_{k|k}\mathbf{G}_{k}^{\top},$$

(5)

where $\mathbf{P}_{k|k}$ and $\mathbf{Q}_{k|k}$ are the system state covariance and the noise covariance respectively. $\mathbf{\Phi}_{k}$ and $\mathbf{G}_{k}$ are the Jacobians of (5) with respect to the system state and the noise respectively. For detailed derivation, please refer to [3, 4].

In the framework of MSCKF, before fusing feature observations by the observation function, we could get a list of features $\left\{f\right\}$ and related cloned poses $\left\{\mathbf{T}_{C}\right\}$. For each feature $f_{i}$ observed by $\mathbf{T}_{C_{j}}$, we have the following observation function:

$$\mathbf{z}_{f_{i}}=\mathbf{h}\left(\mathbf{T}_{C_{j}},\mathbf{f}_{i}\right)+\mathbf{n}_{f_{i}},$$

(6)

where $\mathbf{z}_{f_{i}}$ is the 2D observation of $f_{i}$ in the image. $\mathbf{f}_{i}$ is the 3D coordinate of $f_{i}$. By applying first-order Taylor expansion to the above observation function at the current estimated state, we could get the following functions:

$$\mathbf{z}_{f_{i}}\approx\mathbf{h}\left(\hat{\mathbf{x}}_{k},\hat{\mathbf{f}}_{i}\right)+\mathbf{H}_{\hat{\mathbf{x}}_{k}}\tilde{\mathbf{x}}_{k}+\mathbf{H}_{\hat{\mathbf{f}}_{i}}\tilde{\mathbf{f}}_{i}+\mathbf{n}_{f_{i}},$$

(7)

$$\mathbf{r}_{f_{i}}:=\tilde{\mathbf{z}}_{f_{i}}=\mathbf{H}_{\hat{\mathbf{x}}_{k}}\tilde{\mathbf{x}}_{k}+\mathbf{H}_{\hat{\mathbf{f}}_{i}}\tilde{\mathbf{f}}_{i}+\mathbf{n}_{f_{i}},$$

(8)

where $\hat{\cdot}$ represents the estimated values and $\tilde{\cdot}$ represents the errors between true values and estimated values. $\mathbf{H}_{\hat{\mathbf{x}}_{k}}$ and $\mathbf{H}_{\hat{\mathbf{f}}_{i}}$ are the Jacobians of $\mathbf{h}$ with respect to the estimated state and the observed feature respectively. As we do not maintain features in the state vector, $\tilde{\mathbf{f}}_{i}$ is marginalized by projecting $\mathbf{r}_{f_{i}}$ into the left null space of $\mathbf{H}_{\hat{\mathbf{f}}_{i}}$, $\mathbf{N}$:

$$\mathbf{N}^{\top}\mathbf{r}_{f_{i}}=\mathbf{N}^{\top}\mathbf{H}_{\hat{\mathbf{x}}_{k}}\tilde{\mathbf{x}}_{k}+\mathbf{N}^{\top}\mathbf{H}_{\hat{\mathbf{f}}_{i}}\tilde{\mathbf{f}}_{i}+\mathbf{N}^{\top}\mathbf{n}_{f_{i}},$$

(9)

$$\mathbf{r}^{\prime}_{f_{i}}=\mathbf{H}^{\prime}_{\hat{\mathbf{x}}_{k}}\tilde{\mathbf{x}}_{k}+\mathbf{n}^{\prime}_{f_{i}},$$

(10)

where $\mathbf{r}^{\prime}_{f_{i}}=\mathbf{N}^{\top}\mathbf{r}_{f_{i}}$, $\mathbf{H}^{\prime}_{\hat{\mathbf{x}}_{k}}=\mathbf{N}^{\top}\mathbf{H}_{\hat{\mathbf{x}}_{k}}$, $\mathbf{N}^{\top}\mathbf{n}_{f_{i}}=\mathbf{N}^{\top}\mathbf{n}_{f_{i}}$. We would employ $\mathbf{H}^{\prime}_{\hat{\mathbf{x}}_{k}}$ and $\mathbf{r}^{\prime}_{f_{i}}$ to do the state update.

When there is a match between the current image ($\mathbf{C}_{k}$ in Fig. 2) and a keyframe in the pre-built map ($\mathbf{kf_{1}}$ in Fig. 2), we could get many corresponding 3D-2D feature matches (the red dot and the black dot in Fig. 2). We represent the 3D features in the coordinate system of the matched keyframe (anchored keyframe), and through EPnP[23] we could get the relative pose ${}^{\mathbf{kf_{1}}}\mathbf{T}_{\mathbf{C}_{k}}$ between the keyframe and the current frame. This value, combined with the matched keyframe pose ${}^{G}\mathbf{T}_{\mathbf{kf_{1}}}$ and the current frame pose ${}^{L}\mathbf{T}_{\mathbf{C}_{k}}$, would be used to initialize ${}^{G}\mathbf{T}_{L}$. After initialization, ${}^{G}\mathbf{T}_{L}\triangleq\mathbf{x}_{t}$ would be added into the state vector:

$$\mathbf{x}_{k}=\left[\begin{array}[]{ccc}\mathbf{x}_{I}^{\top}&\mathbf{x}_{C}^{\top}&\mathbf{x}_{t}^{\top}\\ \end{array}\right]^{\top}.$$

(11)

Besides, the covariance of the state would also be augmented. As the initial estimation of $\mathbf{x}_{t}$ may be inaccurate, its covariance could be assigned with a big value. It should be noted that every time we have matches from the map, the related keyframes’ poses should be added into the state vector:

$$\mathbf{x}_{k}=\left[\begin{array}[]{cccc}\mathbf{x}_{I}^{\top}&\mathbf{x}_{C}^{\top}&\mathbf{x}_{t}^{\top}&\mathbf{x}_{KF}^{\top}\\ \end{array}\right]^{\top},$$

(12)

$$\mathbf{x}_{KF}=\left[\begin{matrix}{}^{G}\mathbf{q}_{kf_{1}}^{\top}&{}^{G}\mathbf{p}_{kf_{1}}^{\top}&\dots&{}^{G}\mathbf{q}_{kf_{M}}^{\top}&{}^{G}\mathbf{p}_{kf_{M}}^{\top}\\ \end{matrix}\right]^{\top}.$$

(13)

Also, the covariance of keyframes needs to be considered. For the sake of simplicity, we divide the state into two parts: the active part $\mathbf{x}_{A}=\left[\begin{matrix}\mathbf{x}_{I}^{\top}&\mathbf{x}_{C}^{\top}&\mathbf{x}_{t}^{\top}\\ \end{matrix}\right]^{\top}$, and the nuisance part $\mathbf{x}_{N}=\mathbf{x}_{KF}$, so that

$$\mathbf{x}_{k}=\left[\begin{array}[]{cc}\mathbf{x}^{\top}_{A_{k}}&\mathbf{x}_{N_{k}}^{\top}\\ \end{array}\right]^{\top}.$$

(14)

This kind of partition is very useful for Schmidt-EKF to be introduced in Sec. III-A.

As is shown in Fig. 2, supposing the current image matches with multiple frames (for example, $\mathbf{kf_{1}}$ and $\mathbf{kf_{2}}$), for each 3D landmark (for example, ${}^{\mathbf{kf}_{1}}\mathbf{p}_{f_{1}}$), we could project it from the anchored keyframe ($\mathbf{kf}_{1}$) coordinate system into the 2D current image coordinate system by a nonlinear observation function ${}^{1}\mathbf{g}$:

$${}^{1}\mathbf{z}_{f}={}^{1}\mathbf{g}(^{G}\mathbf{T}_{\mathbf{kf}_{1}},^{G}\mathbf{T}_{L},^{L}\mathbf{T}_{\mathbf{C}_{k}},^{\mathbf{kf}_{1}}\mathbf{p}_{f_{1}})+{}^{1}\mathbf{n}_{f},$$

(15)

where ${}^{1}\mathbf{z}_{f}$ is the 2D pixel coordinate in the current image. Linearizing above function at the estimated values, just like (7), we could get

$${}^{1}\mathbf{r}_{f}={}^{1}\mathbf{H}_{\hat{\mathbf{x}}_{{A}_{k}}}\tilde{\mathbf{x}}_{{A}_{k}}+{}^{1}\mathbf{H}_{\hat{\mathbf{x}}_{{N}_{k}}}\tilde{\mathbf{x}}_{{N}_{k}}+{}^{1}\mathbf{H}_{{}^{\mathbf{kf}_{1}}\hat{\mathbf{p}}_{f_{1}}}{}^{\mathbf{kf}_{1}}\tilde{\mathbf{p}}_{f_{1}}+{}^{1}\mathbf{n}_{f}.$$

(16)

The subscript ${A}$ and ${N}$ represent the active part and the nuisance part of the state vector respectively (c.f. (14)).

Besides, the 3D landmark ${}^{\mathbf{kf}_{1}}\mathbf{p}_{f_{1}}$ could also be directly reprojected into the anchored keyframe $\mathbf{kf}_{1}$ by a projection function ${}^{2}\mathbf{g}$:

$${}^{2}\mathbf{z}_{f}={}^{2}\mathbf{g}({}^{\mathbf{kf}_{1}}\mathbf{p}_{f_{1}})+{}^{2}\mathbf{n}_{f},$$

(17)

$${}^{2}\mathbf{r}_{f}={}^{2}\mathbf{H}_{{}^{\mathbf{kf}_{1}}\hat{\mathbf{p}}_{f_{1}}}{}^{\mathbf{kf}_{1}}\tilde{\mathbf{p}}_{f_{1}}+{}^{2}\mathbf{n}_{f}.$$

(18)

Furthermore, if the 3D landmark ${}^{\mathbf{kf}_{1}}\mathbf{p}_{f_{1}}$ can be observed by the other matched keyframe(s), like $\mathbf{kf}_{2}$ in Fig. 2, there is another reprojection function ${}^{3}\mathbf{g}$ which reprojects ${}^{\mathbf{kf}_{1}}\mathbf{p}_{f_{1}}$ into the $\mathbf{kf}_{2}$ image plane:

$${}^{3}\mathbf{z}_{f}={}^{3}\mathbf{g}({}^{G}\mathbf{T}_{\mathbf{kf}_{1}},{}^{G}\mathbf{T}_{\mathbf{kf}_{2}},{}^{\mathbf{kf}_{1}}\mathbf{p}_{f_{1}})+{}^{3}\mathbf{n}_{f},$$

(19)

$${}^{3}\mathbf{r}_{f}={}^{3}\mathbf{H}_{\hat{\mathbf{x}}_{{N}_{k}}}\tilde{\mathbf{x}}_{{N}_{k}}+{}^{3}\mathbf{H}_{{}^{\mathbf{kf}_{1}}\hat{\mathbf{p}}_{f_{1}}}{}^{\mathbf{kf}_{1}}\tilde{\mathbf{p}}_{f_{1}}+{}^{3}\mathbf{n}_{f}.$$

(20)

By stacking (16), (18) and (20) we could get the 6-dimension observation for the 3D landmark ${\mathbf{p}}_{f_{1}}$.

In summary, whenever the current image matches with a single keyframe or multiple keyframes, for each 3D landmark ${}^{\mathbf{kf}_{j}}\mathbf{p}_{f_{i}}$ anchored in the keyframe $\mathbf{kf}_{j}$, we could get the observation function with the following form:

$$\mathbf{r}_{f_{i}}=\mathbf{H}_{\hat{\mathbf{x}}_{{A}_{k}}}\tilde{\mathbf{x}}_{{A}_{k}}+\mathbf{H}_{\hat{\mathbf{x}}_{{N}_{k}}}\tilde{\mathbf{x}}_{{N}_{k}}+\mathbf{H}_{{}^{\mathbf{kf}_{j}}\hat{\mathbf{p}}_{f_{i}}}{}^{\mathbf{kf}_{j}}\tilde{\mathbf{p}}_{f_{i}}+\mathbf{n}_{f_{i}}.$$

(21)

It is important to note that since we regard landmarks of the map as variables with uncertainties, the Jacobian matrix $\mathbf{H}_{{}^{\mathbf{kf}_{j}}\hat{\mathbf{p}}_{f_{i}}}$ needs to be computed. Otherwise, the landmarks would be treated as constants, like [11], which would make the system inconsistent. However, different from [10], we do not maintain map landmarks in the state vector, so we need to marginalize ${}^{\mathbf{kf}_{j}}\mathbf{p}_{f_{i}}$. For each landmark, it has at least two measurements (in the current image, and the matched one or more keyframes), so the row number of $\mathbf{H}_{{}^{\mathbf{kf}_{j}}\hat{\mathbf{p}}_{f_{i}}}$ is more than the dimension of the landmark. This fact guarantees that we could project $\mathbf{r}_{f_{i}}$ into the left null space of $\mathbf{H}_{{}^{\mathbf{kf}_{j}}\hat{\mathbf{p}}_{f_{i}}}$, like (9). In this way, the items related to the landmarks are eliminated while taking the uncertainties of the landmarks into consideration :

$$\mathbf{r}_{f_{i}}^{*}=\mathbf{H}^{*}_{\hat{\mathbf{x}}_{{A}_{k}}}\tilde{\mathbf{x}}_{{A}_{k}}+\mathbf{H}^{*}_{\hat{\mathbf{x}}_{{N}_{k}}}\tilde{\mathbf{x}}_{{N}_{k}}+\mathbf{n}_{f}^{*}\triangleq\mathbf{H}^{*}_{\hat{\mathbf{x}}_{k}}\tilde{\mathbf{x}}_{k}+\mathbf{n}_{f}^{*}.$$

(22)

Note that (22) only considers one landmark. We need to stack (22) for all matched landmarks to get the final observation function:

$$\mathbf{r}^{*}_{k}=\mathbf{H}_{k}^{*}\tilde{\mathbf{x}}_{k}+\mathbf{n}^{*},$$

(23)

where $\mathbf{r}^{*}_{k}$, $\mathbf{H}_{k}^{*}$ and $\mathbf{n}^{*}$ are the stacking of many $\mathbf{r}_{f_{i}}^{*}$, $\mathbf{H}^{*}_{\hat{\mathbf{x}}_{k}}$ and $\mathbf{n}_{f}^{*}$. (23) will be used to do the state update following the Schmidt-EKF as stated in the next section.

The state and the covariance update equations for EKF are as follows:

$$\hat{\mathbf{x}}_{k}=\hat{\mathbf{x}}_{k|k-1}+\mathbf{K}\mathbf{r}^{*}_{k},$$

(25)

$$\mathbf{P}_{k}=\mathbf{P}_{k|k-1}-\mathbf{K}\mathbf{H}^{*}_{k}\mathbf{P}_{k|k-1},$$

(26)

where $\hat{\mathbf{x}}_{k|k-1}$ and $\mathbf{P}_{k|k-1}$ are the output of the propagation step (c.f. (4), (5)), and

$$\mathbf{S}=\mathbf{H}^{*}_{k}\mathbf{P}_{k|k-1}\mathbf{H}^{*\top}_{k}+\mathbf{R},$$

(27)

$$\mathbf{K}=\mathbf{H}^{*}_{k}\mathbf{P}_{k|k-1}\mathbf{H}^{*\top}_{k}\mathbf{S}^{-1}.$$

(28)

$\mathbf{R}$ is the covariance of $\mathbf{n}^{*}$. Expanding (28),

	$\displaystyle\mathbf{K}=\left[\begin{matrix}\mathbf{K}_{A}\\ \mathbf{K}_{N}\end{matrix}\right]$	$\displaystyle=\left[\begin{matrix}\mathbf{P}_{AA_{k|k-1}}\mathbf{H}_{A_{k}}^{*\top}+\mathbf{P}_{AN_{k|k-1}}\mathbf{H}_{N_{k}}^{*\top}\\ \mathbf{P}_{NA_{k|k-1}}\mathbf{H}_{A_{k}}^{*\top}+\mathbf{P}_{NN_{k|k-1}}\mathbf{H}_{N_{k}}^{*\top}\end{matrix}\right]\mathbf{S}_{k}^{-1}$		(29)
		$\displaystyle=\left[\begin{matrix}\bar{\mathbf{K}}_{A}\\ \bar{\mathbf{K}}_{N}\end{matrix}\right]\mathbf{S}_{k}^{-1},$

and substituting (29) into (26), we could get:

	$\displaystyle\mathbf{P}_{k}$	$\displaystyle=\mathbf{P}_{k|k-1}-$		(30)
		$\displaystyle\left[\begin{matrix}\mathbf{K}_{A}\mathbf{S}\mathbf{K}_{A}^{\top}&\mathbf{K}_{A}\mathbf{H}_{k}^{*\top}\left[\begin{matrix}\mathbf{P}_{AN_{k|k-1}}\\ \mathbf{P}_{NN_{k|k-1}}\end{matrix}\right]\\ \left[\begin{matrix}\mathbf{P}_{AN_{k|k-1}}\\ \mathbf{P}_{NN_{k|k-1}}\\ \end{matrix}\right]^{\top}\mathbf{H}_{k}^{*\top}\mathbf{K}_{A}^{\top}&\mathbf{K}_{N}\mathbf{S}\mathbf{K}_{N}^{\top}\end{matrix}\right].$

From (30), we could see that in the standard EKF update step, when we need to update the covariance, the computation is dominated by $\mathbf{K}_{N}\mathbf{S}\mathbf{K}_{N}^{\top}$, which is $\mathcal{O}(n^{2})$ (suppose the dimension of $\mathbf{x}_{N}$ is $n$). As map information is added continuously while the robot is traveling, the size of the nuisance part $\mathbf{x}_{N}$ would grow over time. Especially for a large scene, the dimension of nuisance part can be thousands easily. In this situation, the filter-based system would suffer from the huge amount of computation and even could not run in real-time. Thanks to Schmidt-EKF, this problem could be solved elegantly.

As analysed before, computing $\mathbf{K}_{N}\mathbf{S}\mathbf{K}_{N}^{\top}$ is time-consuming. Therefore, in Schmidt-EKF, we just drop this part directly when we update the covariance. More specifically, we set $\mathbf{K}_{N}=\mathbf{0}$, so that (30) can be rewritten as:

	$\displaystyle\mathbf{P}_{k}$	$\displaystyle=\mathbf{P}_{k|k-1}-$		(31)
		$\displaystyle\left[\begin{matrix}\mathbf{K}_{A}\mathbf{S}\mathbf{K}_{A}^{\top}&\mathbf{K}_{A}\mathbf{H}_{k}^{*\top}\left[\begin{matrix}\mathbf{P}_{AN_{k|k-1}}\\ \mathbf{P}_{NN_{k|k-1}}\end{matrix}\right]\\ \left[\begin{matrix}\mathbf{P}_{AN_{k|k-1}}\\ \mathbf{P}_{NN_{k|k-1}}\\ \end{matrix}\right]^{\top}\mathbf{H}_{k}^{*\top}\mathbf{K}_{A}^{\top}&\mathbf{0}\end{matrix}\right],$

and (25) would be divided into two parts:

	$\displaystyle\hat{\mathbf{x}}_{A_{k}}$	$\displaystyle=\hat{\mathbf{x}}_{A_{k|k-1}}+\mathbf{K}_{A}\mathbf{r}_{k}^{*},$		(32)
	$\displaystyle\hat{\mathbf{x}}_{N_{k}}$	$\displaystyle=\hat{\mathbf{x}}_{N_{k|k-1}}.$

It can be seen that the active part update is identical to the standard EKF while the nuisance part would not be updated. Comparing (30) and (31), it is easy to see that:

$$\mathbf{P}_{SKF}=\mathbf{P}_{EKF}+\left[\begin{matrix}\mathbf{0}\\ \bar{\mathbf{K}}_{N}\end{matrix}\right]\mathbf{S}^{-1}\left[\begin{matrix}\mathbf{0}&\bar{\mathbf{K}}_{N}^{\top}\end{matrix}\right],$$

(33)

where $\mathbf{P}_{SKF}$ and $\mathbf{P}_{EKF}$ are updated covariance of Schmidt-EKF and standard EKF respectively. As $\mathbf{S}^{-1}$ is postive definite, $\left[\begin{matrix}\mathbf{0}\\ \bar{\mathbf{K}}_{N}\end{matrix}\right]\mathbf{S}^{-1}\left[\begin{matrix}\mathbf{0}&\bar{\mathbf{K}}_{N}^{\top}\end{matrix}\right]$ must be positive semi-definite. Therefore, $\mathbf{P}_{SKF}-\mathbf{P}_{EKF}>=\mathbf{0}$, and it is not overconfident about the estimated state, i.e. maintains the system consistency. Besides, different from [9], the cross-covariance of active state and map information is considered (c.f. (31)) whose computation complexity is $\mathcal{O}(n)$. Therefore, for the state update with global information, we just need to linearize the global observation function to the form of (24), and substitute (24) into (31)-(32).

We make some adaptations to the simulator of Open-VINS [4] to make it suitable for localization. The simulator needs to be fed a ground truth trajectory so that the system could generate image features and IMU information. Here, we feed the ground truth trajectory of our own data set (denoted as YQ3, see Fig. 4). It has a length of 1.282km. The map keyframes’ poses come from another trajectory of our own data set YQ1, which has the similar running path as YQ3. For the details of YQ data set, please refer to Sec. IV-B3.

To test the consistency of our proposed system, we artificially add the noise to the ground truth of YQ1 to simulate that the map is not perfect. To be specific, the position of the ground truth is perturbed with the Gaussian white noise $\mathbf{n}_{p}\sim\mathcal{N}(\mathbf{0},0.01\mathbf{I}_{3\times 3})$, and the orientation is perturbed with $\mathbf{n}_{o}\sim\mathcal{N}(\mathbf{0},0.00025\mathbf{I}_{3\times 3})$. After the perturbation, the root-mean-squared error (RMSE) of the trajectory of YQ1 is 0.179m.

For the map matching information, we first randomly generate 3D landmarks, then we utilize the global observation function (15) to reproject 3D landmarks into the map keyframes and the current frame so that the 2D-2D matching features are obtained, after which we add Gaussian white noise to the 2D features. Finally, for each 3D landmark, we utilize the noisy 2D features and map keyframes’ perturbed poses to triangulate its 3D position (estimated 3D landmark).

In our simulation, there are four settings: the original Open-VINS[4]; our method (CS-MSCKF) with single matching frame (SM); our method (CS-MSCKF) with multiple matching frames (MM); our method that treats map poses and landmarks as constants (mapconst).

Fig. 4 demonstrates the trajectories derived from different situations. The blue one is the trajectory from the Open-VINS odometry. We could see that there is an apparent drift in the latter part of the trajectory, whereas our localization result (red one for multiple matching frames) fit nicely with the ground truth (black one). The green one is the result from the algorithm treating the map as perfect. The reason why the green one fit badly with the ground truth is that our map information is not absolutely accurate, and if we do not consider the uncertainty of the map, the estimator would highly rely on the inaccurate map information and therefore leading to a bad result.

Table I lists the RMSE of the four settings. Noting that the initial pose of odometry is set as the initial pose of the ground truth, the trajectory of the odometry is already in the global coordinate system, i.e. the initial poses of the odometry and the ground truth are naturally aligned, so that the RMSEs between the estimated trajectories and the ground truth can be computed directly without the need of alignment.

From Table I, we could find that CS-MSCKF+MM is better than CS-MSCKF+SM. It is in accordance with our intuition, as multiple frames could provide more information. Besides, as is shown in Table I, when the map is treated as perfect (CS-MSCKF+mapconst), the performance of the algorithm is very bad, which demonstrates the necessity of taking the map’s covariance into consideration.

To demonstrate the consistency of our proposed method, we draw the error of estimation with 3-$\sigma$ bounds (c.f. Fig. 5). The figure is corresponding to the results of CS-MSCKF+MM ((a),(b)) and CS-MSCKF+mapconst ((c),(d)). From the figure, we could conclude that our proposed algorithm has good consistency while the algorithm that treats the map information as perfect has poor consistency.

In this part, the effectiveness of our proposed algorithm is validated on a popular data set EuRoC [25]. To be specific, we use the sequences of machine hall (MH) to do experiments. The sequence MH01 is used to build the map, and the other sequences of MH (MH02-MH05) are used to do localization. The map contains keyframes and 3D landmarks, where 3D landmarks are triangulated and refined across multiple adjacent map images and the keyframes’ poses are given by the ground truth (we assume the uncertainties of the ground truth are $1cm$ and $1^{\circ}$. We make the comparison between the two settings (CS-MSCKF + SM/MM) of our proposed methods with the benchmark from Open-VINS[4] and VINS-Fusion[5, 6, 17, 18].

Open-VINS is a pure odometry, and the comparison with it could demonstrate that global localization could constrain the odometry’s drift error effectively. For VINS-Fusion, its global localization mode is used. To be specific, we firstly use its SLAM mode to generate a pose-graph (map information) based on MH01, and we modified the poses of the pose-graph node as the ground truth to make the comparison fair. As the localization needs to be performed in real-time, i.e. the estimated pose in time step $k$ has to be computed in time step $k$, not the result of loop closure or global optimization thereafter, we record the global localization results from the combination of the odometry ${}^{L}\mathbf{T}_{k}$ (without loop closure or global optimization) and the estimated ${}^{G}\mathbf{T}_{L}$. The comparison of experimental results is plotted by box-plot in Fig. 6, where the RMSE of Open-VINS is computed based on the alignment of the initial pose and the rest are computed without alignments. In Fig. 6, values along $Y$-axis are projected into log space, and their specific values are also given. For the localization methods, their processing times are also given in Fig. 7. All the results are the average of three runs.

From Fig. 6 and Fig. 7, we could find that our proposed algorithms perform better than the VINS-Fusion in terms of both accuracy and efficiency. This is because our framework take the uncertainty of the map information into consideration. Besides, the effective and robust matching algorithm provides us better matching information.

To show the advantage of our proposed re-linearization mechanism, we do experiments on Kaist data set [26]. This data set was recorded in urban environments where the majority of the scene can be taken up by other moving vehicles, so this is a challenging data set. In the data set, there are two sequences Urban38 and Urban39 with high similarity. We employ Urban38 to build the map and localization algorithms are tested on Urban39 (the length of the trajectory is 10.67km). The covariance of each map keyframe’s pose is given as $0.1\mathbf{I}_{6\times 6}$ and the re-linearization threshold is set as 20 pixels. The comparison of different algorithms are given in Table II, and the trajectories derived from CS-MSCKF+SM and CS-MSCKF+SM+R are plotted in Fig. 8.

In this challenging scene, the VINS-Fusion failed to perform global localization due to the significant drifts of the odometry. From the results, we could find that for the case of single matching frame, there is a tremendous promotion in accuracy when we employ the re-linearization mechanism. Fig. 8 could illustrate this phenomenon to some extent.

In Fig. 8, the pair of stars represents the two contiguous matches (for example, the $i^{th}$ match in red and the ${i+1}^{th}$ match in blue), and so does the pair of diamonds. The motion direction of the car from the red mark to the blue mark is shown by black arrows in the picture. We could find that the distance between the two stars or the two diamonds is far, which means the accuracy of the localization highly relies on the performance of the odometry. Unfortunately, the odometry inevitably introduces drifts and the estimated values of the state variables are not accurate enough. These two sources of error would make the first-order Taylor approximation of the observation function not good, as analyzed in Sec. III-B. Therefore, we introduce a re-linearization mechanism to make the first-order Taylor approximation more accurate. The effectiveness of this mechanism is shown in Fig. 8. We could find that for the CS-MSCKF+SM, the trajectory around the blue star (c.f. the area of the green dotted circle) hardly changes when there is a match from the map, i.e. since the approximated observation function is not accurate, the single matching frame can hardly correct the trajectory. On the contrary, the trajectory from CS-MSCKF+SM+R shows an obvious sign of being corrected by the single matching frame information. Besides, as shown in the bottom part of Fig. 8, the estimated $z$ value has been corrected distinctly with the help of re-linearization mechanism while the estimated value from CS-MSCKF+SM changes little (refer to the area indicated by the red and the blue dotted arrow).

From Table II, algorithms with multiple matching frames also have good performance, which is according with our intuition. This could be explained by more map information provides more constrains to the current frame so that the large drift could be corrected and bounded.

In this part we conduct some experiments on our own data set (YQ). This data set contains four sequences, YQ1-YQ4, where YQ1-YQ3 were recorded on three separate days with different weather in summer and YQ4 was collected in winter after snowing (c.f. Fig. 3). For this data set, we use YQ1 to build the map, and use YQ2-YQ4 to test the performance of the algorithms. The covariance of each map keyframe’s pose is given as $0.1\mathbf{I}_{6\times 6}$ and the re-linearization threshold is set as 20 pixels. The comparison of different algorithms is given in Table III. VINS-Fusion is failed to perform continuous and consistent global localization because its odometry has large drift and its re-localization mechanism is triggered multiple times. So, the experimental results of VINS-Fusion are not listed.

From the results, we could find that for the case of single matching frame, introducing re-linearization mechanism could significantly improve the positioning accuracy. Combining multiple frames matching information and re-linearization mechanism, we could get the best result.