State Estimation is undoubtedly the most fundamental module for a wide range of applications, such as robotic navigation, autonomous driving, virtual reality, and augmented reality (AR). Approaches that use only a monocular camera have gained significant interests in the field due to their small size, low-cost, and easy hardware setup  [1]–​[5]. However, monocular vision-only systems are incapable of recovering the metric scale, therefore, limiting their usage in real-world robotic applications. Recently, we have seen a growing trend of assisting the monocular vision system with a low-cost inertial measurement unit (IMU). The primary advantage of this monocular visual-inertial system (VINS) is to observe the metric scale, as well as roll and pitch angles. This enables navigation tasks that require metric state estimations. In addition, the integration of IMU measurements can dramatically improve the motion-tracking performance by bridging the gap between losses of visual tracks due to illumination change, textureless area, or motion blur. The monocular VINS is not only widely available on ground robots and drones, but also practicable on mobile devices. It has great advantages in size, weight and power consumption for self and environmental perception.

However, several issues affect the usage of monocular VINS. The first one is rigorous initialization. Due to the lack of direct distance measurements, it is difficult to directly fuse the monocular visual structure with inertial measurements. Also recognizing the fact that VINSs are highly nonlinear, we see significant challenges in terms of estimator initialization. In most cases, the system should be launched from a known stationary position and moved slowly and carefully at the beginning, which limits its usage in practice. Another issue is that the long-term drift is unavoidable for visual-inertial odometry (VIO). In order to eliminate the drift, loop detection, relocalization, and global optimization has to be developed. Except for these critical issues, the demand for map saving and reuse is growing.

To address all these issues, we propose VINS-Mono, a robust and versatile monocular visual-inertial state estimator, which is the combination and extension of our three previous works [6]–​[8]. VINS-Mono contains following features:

1. robust initialization procedure that is able to bootstrap the system from unknown initial states;

2. tightly coupled, optimization-based monocular VIO with camera–IMU extrinsic calibration and IMU bias correction;

3. online relocalization and four degrees-of-freedom (DOF) global pose graph optimization;

4. pose graph reuse that can save, load, and merge multiple local pose graphs.

Among these features, robust initialization, relocalization, and pose graph reuse are our technical contributions, which come from our previous works [6]–​[8]. Engineering contributions include open-source system integration, real-time demonstration for drone navigation, and mobile applications. The whole system has been successfully applied to small-scale AR scenarios, medium-scale drone navigation, and large-scale state-estimation tasks, as shown in Fig. 1.

The rest of this paper is structured as follows. In Section II, we discuss the relevant literature. We give an overview of the complete system pipeline in Section III. Preprocessing steps for both visual and preintegrated IMU measurements are presented in Section IV. In Section V, we discuss the estimator initialization procedure. A tightly coupled, self-calibrating, nonlinear optimization-based monocular VIO is presented in Section VI. Tightly coupled relocalization is presented in Section VII. Global pose graph optimization and reuse is discussed in Section VIII. Experimental results are shown in Section IX. Finally, this paper is concluded with the discussion and possible future research directions in Section X.

Scholarly works on monocular vision-based state estimation/odometry/SLAM are extensive. Noticeable approaches include PTAM [1], SVO [2], LSD-SLAM [3], DSO [5], and ORB-SLAM [4]. It is obvious that any attempts to give a full relevant review would be incomplete. In this section, however, we skip the discussion on vision-only approaches, and only focus on the most relevant results on the monocular visual-inertial state estimation.

The simplest way to deal with visual and inertial measurements is loosely coupled sensor fusion  [9], [10], where the IMU is treated as an independent module to assist the visual structure. Fusion is usually done by the extended Kalman filter (EKF), where the IMU is used for state propagation and the vision-only pose is used for the update. Further on, tightly coupled visual-inertial algorithms are either based on the EKF [11]–​[13] or graph optimization  [14]–​[19] , where camera and IMU measurements are jointly optimized from the raw measurement level. A popular EKF-based VIO approach is MSCKF [11], [12] . The MSCKF maintains several previous camera poses in the state vector, and uses visual measurements of the same feature across multiple camera views to form multiconstraint update. SR-ISWF [20] , [21] is an extension of MSCKF. It uses the square-root form [14] to achieve single-precision representation and avoid poor numerical properties. This approach employs the inverse filter for iterative relinearization, making it equal to optimization-based algorithms. The batch graph optimization or bundle adjustment techniques maintain and optimize all measurements to obtain the optimal state estimates. To achieve constant processing time, graph-based VIO methods  [15], [17], [18] usually optimize over a bounded-size sliding window of recent states by marginalizing out past states and measurements. Due to high computational demands of iterative solving of nonlinear systems, few graph-based methods can achieve real-time performance on resource-constrained platforms, such as mobile phones.

For visual measurement processing, algorithms can be categorized into either direct or indirect methods according to the definition of residual models. Direct approaches [2], [3], [22] minimize photometric error, while indirect approaches [12], [15] , [17] minimize the geometric displacement. Direct methods require a good initial guess due to their small region of attraction, while indirect approaches consume extra computational resources on extracting and matching features. Indirect approaches are more frequently found in the real-world engineering deployment due to its maturity and robustness. However, direct approaches are easier to be extended for dense mapping as they are operated directly on the pixel level.

The IMUs usually acquire data at a much higher rate than the camera. Different methods have been proposed to handle the high-rate IMU measurements. The most straightforward approach is to use the IMU for state propagation in EKF-based approaches  [9], [11]. In a graph optimization formulation, an efficient technique called IMU preintegration is developed in order to avoid the repeated IMU reintegration. This technique was first introduced in [23], which parameterize the rotation error using Euler angles. Shen et al. [16] derived the covariance propagation using continuous-time error-state dynamics. The preintegration theory was further improved in [19] and [24] by adding posterior IMU bias correction.

Accurate initial values are crucial to bootstrap any monocular VINS. A linear estimator initialization method that leverages relative rotations from the short-term IMU preintegration was proposed in  [17] and [25]. This method fails to model gyroscope bias and image noise in raw projection equations. A closed-form solution to the monocular visual-inertial initialization problem was introduced in [26]. Later, an extension to this closed-form solution by adding a gyroscope bias calibration was proposed in  [27]. These approaches fail to model the uncertainty in inertial integration since they rely on the double integration of IMU measurements over an extended period of time. In  [28], a reinitialization and failure recovery algorithm based on SVO  [2] was proposed. An additional downward-facing distance sensor is required to recover the metric scale. An initialization algorithm built on top of the popular ORB-SLAM  [4] was introduced in [18]. It is reported that the time required for the scale convergence can be longer than 10 s. This can pose problems for robotic navigation tasks that require scale estimates right at the beginning.

Odometry approaches, regardless the underlying mathematical formulation that they rely on, suffer from long-term drifting in global translation and orientation. To this end, loop closure plays an important role in long-term operations. ORB-SLAM [4] is able to close loops and reuse the map, which takes advantage of bag-of-words [29]. A 7-DOF  [30] (position, orientation, and scale) pose graph optimization is followed loop detection.

The structure of the proposed monocular visual-inertial state estimator is shown in Fig. 2. The system starts with measurement preprocessing (see Section IV ), in which features are extracted and tracked, and IMU measurements between two consecutive frames are preintegrated. The initialization procedure (see Section V) provides all necessary values, including, pose, velocity, gravity vector, gyroscope bias, and three-dimensional (3-D) feature location, for bootstrapping the subsequent nonlinear optimization-based VIO. The VIO (see Section VI) with relocalization (see Section VII ) modules tightly fuses preintegrated IMU measurements, feature observations. Finally, the pose graph optimization module (see Section VIII) takes in geometrically verified relocalization results, and perform global optimization to eliminate the drift. It also achieves the pose graph reuse. The VIO and pose graph optimization modules run concurrently in separated threads.

Fig. 1.

Outdoor experimental results of the proposed monocular visual-inertial state estimator. Data are collected by a hand-held monocular camera–IMU setup under normal walking condition. It includes two complete circles inside the field and two semicircles on the nearby driveway. Total trajectory length is 2.5 km. A video of the experiment can be found in the multimedia attachment. (a) Trajectory (blue) and feature locations (red). (b) Trajectory overlaid with Google map for visual comparison.

Show All

Fig. 2.

Block diagram illustrating the full pipeline of the proposed monocular VINS. The system starts with measurement preprocessing (see Section IV). The initialization procedure (see Section V) provides all necessary values for bootstrapping the subsequent nonlinear optimization-based VIO. The VIO with relocalization modules (see Sections VI and VII) tightly fuses preintegrated IMU measurements, feature observations, and redetected features from the loop closure. Finally, the pose graph module (see Section VIII) performs global optimization to eliminate drift and achieve reuse purpose.

Show All

Fig. 3.

Illustration of the visual-inertial alignment process for estimator initialization. The basic idea is to match the up-to-scale visual structure with IMU preintegration.

Show All

In comparison to OKVIS [15], a state-of-the-art VIO algorithm, which is suitable for stereo cameras, our algorithm is specifically designed for the monocular camera. So, we particularly propose an initialization procedure, keyframe selection criteria, and use and handle a large field-of-view (FOV) camera for the better tracking performance. Furthermore, our algorithm presents a complete system with a loop closure and pose graph reuse modules.

We now define notations and frame definitions that we use throughout this paper. We consider (⋅)w as the world frame. The direction of the gravity is aligned with the z-axis of the world frame. (⋅)b is the body frame, which we define to be the same as the IMU frame. (⋅)c is the camera frame. We use both rotation matrices R and Hamilton quaternions q to represent rotation. We primarily use quaternions in state vectors, but rotation matrices are also used for the convenience rotation of 3-D vectors. qwb and pwb are rotation and translation from the body frame to the world frame. bk is the body frame while taking the kth image. ck is the camera frame while taking the kth image. ⊗ represents the multiplication operation between two quaternions. gw=[0,0,g]T is the gravity vector in the world frame. Finally, we denote (⋅)^ as the noisy measurement or estimation of a certain quantity.

This section presents preprocessing steps for both inertial and monocular visual measurements. For visual measurements, we track features between consecutive frames and detect new features in the latest frame. For IMU measurements, we preintegrate them between two consecutive frames.

B. IMU Preintegration

We follow our previous continuous-time quaternion-based derivation of IMU preintegration [16], and include the handling of IMU biases as [19] and [24]. We note that our current IMU preintegration procedure shares almost the same numerical results as [19] and [24], but using different derivations. So, we only give a brief introduction here. Details about the quaternion-based derivation can be found in Appendix A.

1) IMU Noise and Bias

IMU measurements, which are measured in the body frame, combines the force for countering gravity and the platform dynamics, and are affected by acceleration bias ba, gyroscope bias bw , and additive noise. The raw gyroscope and accelerometer measurements, ω^ and a^, are given by

a^tω^t=at+bat+Rtwgw+na=ωt+bwt+nw.(1)

View Source \begin{align} \hat{\mathbf {a}}_t &= {\mathbf {a}}_t + \mathbf {b}_{a_t} + {\mathbf {R}^t_w} \mathbf {g}^w + \mathbf {n}_a \nonumber\\ \hat{\boldsymbol{\omega }}_t &= {\boldsymbol{\omega }}_t + \mathbf {b}_{w_t} + \mathbf {n}_w . \tag{1} \end{align} We assume that the additive noise in acceleration and gyroscope measurements are Gaussian white noise, na∼N(0,σ2a), nw∼N(0,σ2w). Acceleration bias and gyroscope bias are modeled as random walk, whose derivatives are Gaussian white noise, nba∼N(0,σ2ba), nbw∼N(0,σ2bw)

b˙at=nba,b˙wt=nbw.(2)

View Source \begin{equation} \dot{\mathbf {b}}_{a_t} = \mathbf {n}_{b_a},\quad \dot{\mathbf {b}}_{w_t} = \mathbf {n}_{b_w} . \tag{2} \end{equation}

2) Preintegration

For two time consecutive frames bk and bk+1, there exists several inertial measurements in time interval [tk,tk+1]. Given the bias estimation, we integrate them in local frame bk as

αbkbk+1βbkbk+1γbkbk+1=∬t∈[tk,tk+1]Rbkt(a^t−bat)dt2=∫t∈[tk,tk+1]Rbkt(a^t−bat)dt=∫t∈[tk,tk+1]12Ω(ω^t−bwt)γbktdt(3)

View Source \begin{align} \boldsymbol{\alpha }^{b_k}_{b_{k+1}} &= \iint _{t \in [t_k,t_{k+1}]} \mathbf {R}^{b_k}_{t} (\hat{\mathbf {a}}_{t} - \mathbf {b}_{a_t}) dt^2 \nonumber\\ \boldsymbol{\beta }^{b_k}_{b_{k+1}} &= \int _{t \in [t_k,t_{k+1}]} \mathbf {R}^{b_k}_{t} (\hat{\mathbf {a}}_{t} - \mathbf {b}_{a_t}) dt\nonumber\\ \boldsymbol{\gamma }^{b_k}_{b_{k+1}} &=\int _{t \in [t_k,t_{k+1}]} \frac{1}{2} \boldsymbol{\Omega }(\hat{\boldsymbol{\omega }}_t - \mathbf {b}_{w_t}) \boldsymbol{\gamma }^{b_k}_{t} dt \tag{3} \end{align} where

Ω(ω)=[−⌊ω⌋×−ωTω0],⌊ω⌋×=⎡⎣⎢0ωz−ωy−ωz0ωxωy−ωx0⎤⎦⎥.(4)

View Source \begin{equation} \boldsymbol{\Omega }(\boldsymbol{\omega }) = {\left[\begin{array}{cc}-\lfloor {\boldsymbol{\omega }} \rfloor _{\times } & {\boldsymbol{\omega }}\\ -{\boldsymbol{\omega }}^T & 0 \end{array}\right]}, \lfloor \boldsymbol{\omega } \rfloor _{\times } = {\left[\begin{array}{ccc}0& -\omega _z & \omega _y\\ \omega _z& 0 & -\omega _x\\ -\omega _y& \omega _x & 0 \end{array}\right]} . \tag{4} \end{equation} The covariance Pbkbk+1 of α,β,and γ also propagates accordingly. It can be seen that the preintegration terms (3) can be obtained solely with IMU measurements by taking bk as the reference frame given bias.

3) Bias Correction

If the estimation of bias changes minorly, we adjust αbkbk+1,βbkbk+1, and γbkbk+1 by their first-order approximations with respect to the bias as

αbkbk+1βbkbk+1γbkbk+1≈α^bkbk+1+Jαbaδbak+Jαbwδbwk≈β^bkbk+1+Jβbaδbak+Jβbwδbwk≈γ^bkbk+1⊗[112Jγbwδbwk].(5)

View Source \begin{align} \boldsymbol{\alpha }^{b_k}_{b_{k+1}} &\approx \hat{\boldsymbol{\alpha }}^{b_k}_{b_{k+1}} + \mathbf {J}^\alpha _{b_a} \delta \mathbf {b}_{a_k} + \mathbf {J}^\alpha _{b_w} \delta \mathbf {b}_{w_k} \nonumber\\ \boldsymbol{{\beta }}^{b_k}_{b_{k+1}} &\approx \hat{\boldsymbol{\beta }}^{b_k}_{b_{k+1}} + \mathbf {J}^\beta _{b_a} \delta \mathbf {b}_{a_k} + \mathbf {J}^\beta _{b_w} \delta \mathbf {b}_{w_k} \nonumber\\ \boldsymbol{\gamma }^{b_k}_{b_{k+1}} &\approx \hat{\boldsymbol{\gamma }}^{b_k}_{b_{k+1}} \otimes {\left[\begin{array}{c}1\\ \frac{1}{2} \mathbf {J}^\gamma _{b_w} \delta \mathbf {b}_{w_k} \end{array}\right]}. \tag{5} \end{align} Otherwise, when the estimation of bias changes significantly, we do repropagation under the new bias estimation. This strategy saves a significant amount of computational resources for optimization-based algorithms since we do not need to propagate IMU measurements repeatedly.

Monocular tightly coupled VIO is a highly nonlinear system that needs an accurate initial guess at the beginning. We get necessary initial values by loosely align IMU preintegration with the vision-only structure.

A. Vision-Only SfM in Sliding Window

The initialization procedure starts with a vision-only SfM to estimate a graph of up-to-scale camera poses and feature positions.

We maintain several frames in a sliding window for bounded computational complexity. First, we check feature correspondences between the latest frame and all previous frames. If we can find stable feature tracking (more than 30 tracked features) and sufficient parallax (more than 20 pixels) between the latest frame and any other frames in the sliding window. we recover the relative rotation and up-to-scale translation between these two frames using the five-point algorithm [34]. Then, we arbitrarily set the scale and triangulate all features observed in these two frames. Based on these triangulated features, a perspective-n-point (PnP) method [35] is performed to estimate poses of all other frames in the window. Finally, a global full bundle adjustment [36] is applied to minimize the total reprojection error of all feature observations. Since we do not yet have any knowledge about the world frame, we set the first camera frame (⋅)c0 as the reference frame for SfM. All frame poses ( p¯c0ck,qc0ck) and feature positions are represented with respect to (⋅)c0. Given extrinsic parameters (pbc,qbc) between the camera and the IMU, we can translate poses from the camera frame to body (IMU) frame as

qc0bksp¯c0bk=qc0ck⊗(qbc)−1=sp¯c0ck−Rc0bkpbc(6)

View Source \begin{align} \mathbf {q}^{c_0}_{b_k} &= \mathbf {q}^{c_0}_{c_k} \otimes {(\mathbf {q}^b_c)}^{-1} \nonumber\\ s\bar{\mathbf {p}}^{c_0}_{b_k} &= s\bar{\mathbf {p}}^{c_0}_{c_k} - \mathbf {R}^{c_0}_{b_k}\mathbf {p}^b_{c} \tag{6} \end{align} where s is the unknown scaling parameter, which will be solved in the next.

B. Visual-Inertial Alignment

An illustration of the visual-inertial alignment is shown in Fig. 3. The basic idea is to match the up-to-scale visual structure with IMU pre-integration.

1) Gyroscope Bias Calibration

Consider two consecutive frames bk and bk+1 in the window, we get the rotation qc0bk and qc0bk+1 from the visual SfM, as well as the relative constraint γ^bkbk+1 from IMU preintegration. We linearize the IMU preintegration term with respect to gyroscope bias and minimize the following cost function:

minδbw∑k∈B∥∥qc0bk+1−1⊗qc0bk⊗γbkbk+1∥∥2γbkbk+1≈γ^bkbk+1⊗[112Jγbwδbw](7)

View Source \begin{align} \min _{\delta b_w} \sum _{k \in \mathcal {B}} \left\Vert {\mathbf {q}^{c_0}_{b_{k+1}}}^{-1} \otimes \mathbf {q}^{c_0}_{b_{k}} \otimes \boldsymbol{\gamma }^{b_k}_{b_{k+1}} \right\Vert ^2\nonumber\\ \boldsymbol{\gamma }^{b_k}_{b_{k+1}} \approx \hat{\boldsymbol{\gamma }}^{b_k}_{b_{k+1}} \otimes {\left[\begin{array}{c}1\\ \frac{1}{2} \mathbf {J}^\gamma _{b_w} \delta \mathbf {b}_w \end{array}\right]} \tag{7} \end{align} where B indexes all frames in the window. In such way, we get an initial calibration of the gyroscope bias bw. Then, we repropagate all IMU preintegration terms α^bkbk+1,β^bkbk+1, and γ^bkbk+1 using the new gyroscope bias.

2) Velocity, Gravity Vector, and Metric Scale Initialization

After the gyroscope bias is initialized, we move on to initialize other essential states for navigation, namely, velocity, gravity vector, and metric scale

XI=[vb0b0,vb1b1,…vbnbn,gc0,s](8)

View Source \begin{equation} \begin{split} \mathcal {X}_{I} &= \left[ \mathbf {v}^{b_0}_{b_0},\,\mathbf {v}^{b_1}_{b_1},\, \ldots \,\mathbf {v}^{b_n}_{b_n},\,\mathbf {g}^{c_0},\, s \right] \end{split} \tag{8} \end{equation} where vbkbk is velocity in the body frame while taking the kth image, gc0 is the gravity vector in the c0 frame, and s scales the monocular SfM to metric units.

Consider two consecutive frames bk and bk+1 in the window, we have following equations:

αbkbk+1βbkbk+1=Rbkc0(s(p¯c0bk+1−p¯c0bk)+12gc0Δt2k−Rc0bkvbkbkΔtk)=Rbkc0(Rc0bk+1vbk+1bk+1+gc0Δtk−Rc0bkvbkbk).(9)

View Source \begin{align} {\boldsymbol{\alpha }}^{b_k}_{b_{k+1}} &= \mathbf {R}^{b_k}_{c_0} (s(\bar{\mathbf {p}}^{c_0}_{b_{k+1}} - \bar{\mathbf {p}}^{c_0}_{b_k}) + \frac{1}{2} \mathbf {g}^{c_0} {\Delta t}_k^2 - \mathbf {R}^{c_0}_{b_k} \mathbf {v}^{b_k}_{b_k}{\Delta t}_k) \nonumber\\ {\boldsymbol{\beta }}^{b_k}_{b_{k+1}} &= \mathbf {R}^{b_k}_{c_0} (\mathbf {R}^{c_0}_{b_{k+1}}\mathbf {v}^{b_{k+1}}_{b_{k+1}} + \mathbf {g}^{c_0} {\Delta t}_k - \mathbf {R}^{c_0}_{b_k}\mathbf {v}^{b_k}_{b_{k}}). \tag{9} \end{align} We can combine (6) and  (9) into the following linear measurement model:

z^bkbk+1=⎡⎣α^bkbk+1−pbc+Rbkc0Rc0bk+1pbcβ^bkbk+1⎤⎦=Hbkbk+1XI+nbkbk+1(10)

View Source \begin{equation} \hat{\mathbf {z}}^{b_k}_{b_{k+1}} = {\left[\begin{array}{c}\hat{\boldsymbol{\alpha }}^{b_k}_{b_{k+1}} - \mathbf {p}^b_c + \mathbf {R}^{b_k}_{c_{0}}\mathbf {R}^{c_0}_{b_{k+1}}\mathbf {p}^b_c\\ \hat{\boldsymbol{\beta }}^{b_k}_{b_{k+1}} \end{array}\right]} = \mathbf {H}^{b_k}_{b_{k+1}} \mathcal {X}_I + \mathbf {n}^{b_k}_{b_{k+1}} \tag{10} \end{equation} where

Hbkbk+1=[−IΔtk−I0Rbkc0Rc0bk+112Rbkc0Δt2kRbkc0ΔtkRbkc0(p¯c0ck+1−p¯c0ck)0].(11)

View Source \begin{equation} \mathbf {H}^{b_k}_{b_{k+1}}= {\left[\begin{array}{cccc}-\mathbf {I}{\Delta t}_k &\mathbf {0} & \frac{1}{2}\mathbf {R}^{b_k}_{c_0} {\Delta t}_k^2 & \mathbf {R}^{b_k}_{c_0}(\bar{\mathbf {p}}^{c_0}_{c_{k+1}}-\bar{\mathbf {p}}^{c_0}_{c_k})\\ -\mathbf {I} &\mathbf {R}^{b_k}_{c_0}\mathbf {R}^{c_0}_{b_{k+1}} & \mathbf {R}^{b_k}_{c_0} {\Delta t}_k & \mathbf {0} \end{array}\right]} . \tag{11} \end{equation} It can be seen that Rc0bk,Rc0bk+1,p¯c0ck, and p¯c0ck+1 are obtained from the up-to-scale monocular visual SfM. Δtk is the time interval between two consecutive frames. By solving this linear least-square problem

minXI∑k∈B∥∥z^bkbk+1−Hbkbk+1XI∥∥2(12)

View Source \begin{equation} \begin{split} \min _{\mathcal {X}_I}\sum _{k \in \mathcal {B}} \left\Vert \hat{\mathbf {z}}^{b_k}_{b_{k+1}} - \mathbf {H}^{b_k}_{b_{k+1}} \mathcal {X}_I\right\Vert ^2 \end{split} \tag{12} \end{equation} we can get body frame velocities for every frame in the window, the gravity vector in the visual reference frame (⋅)c0, as well as the scale parameter.

3) Gravity Refinement

The gravity vector obtained from the previous linear initialization step can be refined by constraining the magnitude. In most cases, the magnitude of the gravity vector is known. This results in only 2-DOF remaining for the gravity vector. Therefore, we perturb the gravity with two variables on its tangent space, which preserves 2-DOF. Our gravity vector is perturbed by g(g^¯+δg),δg=w1b1+w2b2, where g is the known magnitude of the gravity, g¯^ is a unit vector representing the gravity direction. b1 and b2 are two orthogonal basis spanning the tangent plane, as shown in Fig. 4. w1 and w2 are 2-D perturbation toward b1 and b2 , respectively. We can arbitrarily find any set of b1 and b2 spinning the tangent space. Then, we substitute g into (9) by g(g¯^+δg), and solve for 2-D δg together with other state variables. This process iterates several times until g^ converges.

Fig. 4.

Illustration of 2-DOF perturbation of gravity. Since the magnitude of gravity is known, g lies on a sphere with radius g≈9.81m/s2. The gravity is perturbed around current estimate as g(g¯^+δg),δg=w1b1+w2b2, where b1 and b2 are two orthogonal basis spanning the tangent space.

Show All

4) Completing Initialization

After refining the gravity vector, we can get the rotation qwc0 between the world frame and the camera frame c0 by rotating the gravity to the z-axis. We then rotate all variables from the reference frame (⋅)c0 to the world frame (⋅)w. The body frame velocities will also be rotated to the world frame. Translational components from the visual SfM will be scaled to metric units. At this point, the initialization procedure is completed and all these metric values will be fed to a tightly coupled monocular VIO.

After estimator initialization, we proceed with a sliding window-based tightly coupled monocular VIO for high-accuracy and robust state estimation. An illustration of the sliding window formulation is shown in Fig. 5.

Fig. 5.

Illustration of the sliding window monocular VIO with relocalization. Several camera poses, IMU measurements, and visual measurements exist in the sliding window. It is a tightly coupled formulation with IMU, visual, and loop measurements.

Show All

A. Formulation

The full state vector in the sliding window is defined as

Xxkxbc=[x0,x1,…xn,xbc,λ0,λ1,…λm]=[pwbk,vwbk,qwbk,ba,bg],k∈[0,n]=[pbc,qbc](13)

View Source \begin{align} \mathcal {X} &= \left[ \mathbf {x}_0,\,\mathbf {x}_{1},\, \ldots \,\mathbf {x}_{n},\, \mathbf {x}^b_c,\, \lambda _0,\,\lambda _{1},\, \ldots \,\lambda _{m} \right] \nonumber\\ \mathbf {x}_k &= \left[ \mathbf {p}^w_{b_k},\,\mathbf {v}^w_{b_k},\,\mathbf {q}^w_{b_k}, \,\mathbf {b}_a, \,\mathbf {b}_g \right], k\in [0,n] \nonumber\\ \mathbf {x}^b_c &= \left[ \mathbf {p}^b_c,\,\mathbf {q}^b_{c} \right] \tag{13} \end{align} where xk is the IMU state at the time that the kth image is captured. It contains position, velocity, and orientation of the IMU in the world frame, and acceleration bias and gyroscope bias in the IMU body frame. n is the total number of keyframes, and m is the total number of features in the sliding window. λl is the inverse distance of the lth feature from its first observation.

We use a visual-inertial bundle adjustment formulation. We minimize the sum of prior and the Mahalanobis norm of all measurement residuals to obtain a maximum posteriori estimation as

minX{∥rp−HpX∥2+∑k∈B∥∥rB(z^bkbk+1,X)∥∥2Pbkbk+1+∑(l,j)∈Cρ(∥∥rC(z^cjl,X)∥∥2Pcjl)⎫⎭⎬(14)

View Source \begin{align} \min _{\mathcal {X}} \left\lbrace \left\Vert \mathbf {r}_p - \mathbf {H}_p \mathcal {X} \right\Vert ^2 + \sum _{k \in \mathcal {B}} \left\Vert \mathbf {r}_{\mathcal {B}}(\hat{\mathbf {z}}^{b_k}_{b_{k+1}},\, \mathcal {X}) \right\Vert _{\mathbf {P}^{b_k}_{b_{k+1}}}^2 \right. \nonumber\\ \left. +\sum _{(l,j) \in \mathcal {C}} \rho (\left\Vert \mathbf {r}_{\mathcal {C}}(\hat{\mathbf {z}}^{c_j}_l, \, \mathcal {X}) \right\Vert _{\mathbf {P}^{c_j}_l}^2) \right\rbrace \tag{14} \end{align} where the Huber norm [37] is defined as

ρ(s)={s2s√−1s≤1s>1.(15)

View Source \begin{equation} \rho (s)=\left\{\begin{array}{ll}s & s \leq 1\\ 2\sqrt{s}-1 & s > 1. \end{array}\right. \tag{15} \end{equation} where rB(z^bkbk+1,X) and rC(z^cjl,X) are residuals for IMU and visual measurements, respectively. The detailed definition of the residual terms will be presented in Sections VI-B and VI-C. B is the set of all IMU measurements, C is the set of features that have been observed at least twice in the current sliding window. {rp,Hp} is the prior information from marginalization. The Ceres solver [38] is used for solving this nonlinear problem.