VINS-Fusion

Qin 2019 · Paper

One-line summary — VINS-Fusion generalizes VINS-Mono into an optimization-based framework for local odometry where every sensor is treated as a general factor; factors sharing state variables are summed into one sliding-window problem, demonstrated with stereo-only, mono+IMU, and stereo+IMU suites in a single open-source codebase.

Problem

Robots ship with increasingly diverse sensor suites — stereo cameras on ground vehicles, a monocular camera plus IMU on phones, stereo plus IMU on aerial robots — yet most state estimators are designed for a single sensor or one specific suite and cannot be ported across platforms. A practical system also needs graceful sensor failure handling: an inactive sensor should be removable and an alternative added quickly. VINS-Fusion proposes one general optimization-based framework in which every sensor is just another residual factor in a pose graph.

Method & architecture

States. The sliding window estimates body poses plus optional sensor-specific variables:

X=[p0,R0,…,pn,Rn,xcam,ximu],xcam=[λ0,…,λl],ximu=[v0,ba0,bg0,… ]\mathcal{X} = [\mathbf{p}_0, \mathbf{R}_0, \dots, \mathbf{p}_n, \mathbf{R}_n, \mathbf{x}_{cam}, \mathbf{x}_{imu}], \quad \mathbf{x}_{cam} = [\lambda_0, \dots, \lambda_l], \quad \mathbf{x}_{imu} = [\mathbf{v}_0, \mathbf{b}_{a_0}, \mathbf{b}_{g_0}, \dots]

where λ\lambda is the depth of each feature in its first observing frame; ximu\mathbf{x}_{imu} (velocities and IMU biases) is simply omitted for the stereo-only suite. State estimation is MLE over independent Gaussian measurements, i.e. nonlinear least squares:

X∗=arg⁡min⁡X∑t=0n∑k∈S∥ztk−htk(X)∥Ωtk2\mathcal{X}^{*} = \arg\min_{\mathcal{X}} \sum_{t=0}^{n} \sum_{k\in\mathbf{S}} \left\lVert \mathbf{z}^{k}_{t} - h^{k}_{t}(\mathcal{X}) \right\rVert^{2}_{\mathbf{\Omega}^{k}_{t}}

Camera factor. Shi-Tomasi corners tracked by KLT (and matched left-right for stereo); the factor reprojects feature ll from its first observation in image ii into image tt:

ztl−htl(X)=[utlvtl]−πc((Tcb)−1 Tt−1 Ti Tcb πc−1(λl,[uilvil]))\mathbf{z}^{l}_{t} - h^{l}_{t}(\mathcal{X}) = \begin{bmatrix} u^{l}_{t} \\ v^{l}_{t} \end{bmatrix} - \pi_c\Big( (\mathbf{T}^{b}_{c})^{-1}\, \mathbf{T}_{t}^{-1}\, \mathbf{T}_{i}\, \mathbf{T}^{b}_{c}\, \pi_c^{-1}\big(\lambda_l, \begin{bmatrix} u^{l}_{i} \\ v^{l}_{i} \end{bmatrix}\big) \Big)

with πc\pi_c the camera model’s projection and Tcb\mathbf{T}^b_c the body-to-camera extrinsic. The same factor serves temporal (left-to-left) and spatial (left-to-right) observations — spatial ones constrain metric scale through the calibrated baseline, no IMU excitation needed.

IMU factor. On-manifold preintegration between consecutive frames yields pseudo-measurements αtt−1,βtt−1,γtt−1\boldsymbol{\alpha}^{t-1}_{t}, \boldsymbol{\beta}^{t-1}_{t}, \boldsymbol{\gamma}^{t-1}_{t} (relative position, velocity, rotation) plus propagated covariance; the residual compares them against the state-predicted motion, e.g. for position αtt−1⊖Rt−1−1(pt−pt−1+12g dt2−vt−1dt)\boldsymbol{\alpha}^{t-1}_{t} \ominus \mathbf{R}_{t-1}^{-1}(\mathbf{p}_t - \mathbf{p}_{t-1} + \tfrac{1}{2}\mathbf{g}\,dt^2 - \mathbf{v}_{t-1} dt), with biases modeled as random walks.

Optimization & marginalization. The summed cost is solved by Gauss–Newton/Levenberg–Marquardt in Ceres. The window keeps ten spatial camera frames; when a new keyframe arrives, the oldest frame’s visual and inertial factors are marginalized via the Schur complement,

Hp=Hrr−HrmHmm−1Hmr,bp=br−HrmHmm−1bm\mathbf{H}_p = \mathbf{H}_{rr} - \mathbf{H}_{rm}\mathbf{H}_{mm}^{-1}\mathbf{H}_{mr}, \qquad \mathbf{b}_p = \mathbf{b}_r - \mathbf{H}_{rm}\mathbf{H}_{mm}^{-1}\mathbf{b}_m

turning the problem into MAP with a prior term and no information loss. Because any sensor reducible to a residual factor fits (wheel odometry, LiDAR, radar are named), sensor failure is handled by removing the inactive sensor’s factors and adding another’s.

Results

On EuRoC (RMSE ATE, Horn alignment), the three suites are compared against OKVIS: mono+IMU achieves 0.09 m on MH_02 (OKVIS 0.22), 0.09 m on V1_02 (OKVIS 0.20), 0.06 m on V2_01 (OKVIS 0.13), outperforming OKVIS on most sequences. Stereo-only fails on V1_03 and V2_03 (motion too aggressive for visual tracking) and drifts most elsewhere; every IMU-aided configuration survives all eleven sequences — the IMU bridges illumination change, texture-less areas, and motion blur, and suppresses roll/pitch drift by observing gravity. Stereo+IMU is not always best, being more sensitive to calibration error. In outdoor handheld experiments (mvBlueFOX stereo at 20 Hz + DJI A3 IMU at 200 Hz, GPS as ground truth) over ~224–232 m loops, RMSE drops from 1.85–2.59 m (stereo-only) to 0.43–0.75 m with IMU fusion. GPS fusion is named as future work in this paper; the open-source VINS-Fusion release ships it as a companion global pose-graph module fusing local odometry with global position measurements.

Why it matters for SLAM

VINS-Fusion is one of the most widely deployed open-source odometry stacks in robotics: it took the academically successful VINS-Mono and made it practical for real vehicles by adding stereo scale observability and a sensor-agnostic factor formulation, later extended with GPS drift correction. Its “everything is a factor” design became the template for local-plus-global fusion in autonomous driving and drone autonomy, and it remains a standard baseline for stereo-inertial estimation.