DM-VIO

von Stumberg 2022 · Paper

One-line summary — A direct (DSO-based) monocular VIO built on two new techniques — delayed marginalization and pose graph bundle adjustment (PGBA) for IMU initialization — that outperforms even stereo-inertial systems with a single camera and IMU.

Problem

Marginalization keeps sliding-window VIO real-time, but “it cannot easily be reversed, and linearization points of connected variables have to be fixed” (First-Estimates Jacobians). This is especially painful in monocular VIO, where the scale is connected to the marginalization prior as soon as the first keyframe is marginalized — yet may still change drastically, since scale is unobservable under constant-velocity motion (common in cars). Prior systems either ran visual-only + separate IMU init (losing photometric uncertainty) or, like VI-DSO, initialized immediately with arbitrary scale and used lossy dynamic marginalization. DM-VIO answers three questions with one mechanism: how to capture full visual uncertainty in the initializer, how to transfer it into the main system, and how to keep the prior consistent when scale changes.

Method & architecture

The core is a visual-inertial bundle adjustment over all active keyframes, minimized with Levenberg-Marquardt (photometric part SIMD-accelerated from DSO, other factors in GTSAM):

$$E(\mathbf{s}) = W(e_{\text{photo}}) \cdot E_{\text{photo}} + E_{\text{imu}} + E_{\text{prior}}$$

• Photometric energy (DSO-style). Each active keyframe $i \in \mathcal{F}$ hosts points $\mathbf{p}$ projected into observing frames $j$: $$E_{\mathbf{p}j} = \sum_{\mathbf{p} \in \mathcal{N}_{\mathbf{p}}} \omega_{\mathbf{p}} \left\lVert (I_j[\mathbf{p}'] - b_j) - \frac{t_j e^{a_j}}{t_i e^{a_i}} (I_i[\mathbf{p}] - b_i) \right\rVert_{\gamma},$$ with affine brightness parameters $a, b$, exposure times $t$, and Huber norm $\lVert\cdot\rVert_\gamma$.
• Dynamic photometric weight. With $e_{\text{photo}} = \sqrt{E_{\text{photo}}/n_{\text{residuals}}}$, the weight $W(e_{\text{photo}}) = \lambda \cdot (\theta/e_{\text{photo}})^2$ if $e_{\text{photo}} \geq \theta$ (else $\lambda$), with $\theta = 8$. Unlike the per-point Huber norm, this downweights the whole image when quality is bad, shifting trust to the IMU.
• IMU factors. On-manifold preintegration predicts state $\widehat{\mathbf{s}}_j^I$ with covariance $\widehat{\Sigma}_j$: $$E_{\text{imu}}(\mathbf{s}_i^I, \mathbf{s}_j^I) = \left(\widehat{\mathbf{s}}_j^I \boxminus \mathbf{s}_j^I\right)^T \widehat{\Sigma}_j^{-1} \left(\widehat{\mathbf{s}}_j^I \boxminus \mathbf{s}_j^I\right)$$
• Explicit scale and gravity. Visual factors live in a scale-arbitrary frame $V$, IMU factors in the metric frame $I$; the state includes scale $s$ and rotation $\mathbf{R}_{V\_I}$ explicitly, so scale keeps being optimized in the main system after initialization.
• Marginalization. Old variables $\beta$ are removed via Schur complement, $\widehat{\mathbf{H}}_{\alpha\alpha} = \mathbf{H}_{\alpha\alpha} - \mathbf{H}_{\alpha\beta}\mathbf{H}_{\beta\beta}^{-1}\mathbf{H}_{\beta\alpha}$, keeping at most $N_f = 8$ keyframes with DSO’s non-fixed-lag strategy.
• Delayed marginalization. A second factor graph replays the same marginalization order with delay $d = 100$ frames (points are still marginalized immediately, so each linearized photometric factor connects exactly $N_f$ keyframes; the Markov blanket — and hence runtime, measured at 0.44 ms — stays the same as the main graph). This graph can be (1) populated with IMU factors, (2) re-advanced to rebuild the prior, (3) used to relinearize FEJ values.
• PGBA for IMU initialization. IMU and bias factors are inserted into the delayed graph (at least $d - N_f + 2$ poses can receive them, i.e. ≥ 93 IMU factors) and all variables are optimized. Unlike pose graph optimization it uses “octonary” factors capturing the full BA probability distribution; unlike full BA it never relinearizes photometric terms — accurate and fast. Re-advancing the optimized graph then hands the main system a marginalization prior containing all visual and inertial information.
• Multi-stage initializer. Coarse IMU init (poses fixed, single bias; gravity from averaged accelerometer, scale = 1) → threshold on marginal scale covariance → PGBA init (with optional re-init under a tighter threshold $\theta_{\text{reinit}}$) → marginalization replacement whenever $\max(s, s_{\text{fej}})/\min(s, s_{\text{fej}}) > \theta_s$, rebuilding the prior with current linearization points.

Results

Evaluated on EuRoC, TUM-VI, and 4Seasons (drone, handheld, automotive); 10 runs per EuRoC sequence, 5 elsewhere, in real-time mode on a 2013 MacBook Pro (i7 2.3 GHz, no GPU).

• EuRoC: average RMSE ATE 0.069 m (mono, no loop closure) — best reported VIO at the time, beating stereo-inertial Basalt (0.072), VI-DSO (0.089, mono), VINS-Mono (0.184), OKVIS (0.231). Also the lowest average scale error reported on the dataset (0.6%).
• TUM-VI: mean drift 0.472% vs. Basalt 0.939% (stereo) and VINS-Mono 1.700%; best result on 16 of 28 sequences (e.g. outdoors8: 2.11 m vs. Basalt 13.53 m). More robust overall than ORB-SLAM3 in cumulative plots, though ORB-SLAM3’s loop closures win on some sequences.
• 4Seasons: with long constant-velocity stretches where mono scale is unobservable, DM-VIO still outperforms stereo-inertial ORB-SLAM3 and Basalt.
• Runtime: tracking 10.34 ms, keyframe processing 53.67 ms; delayed marginalization adds only 0.44 ms (0.8%) to the keyframe thread.

Why it matters for SLAM

DM-VIO closed the gap between direct and feature-based VIO: a monocular photometric system with a well-designed marginalization and initialization strategy can beat feature-based stereo-inertial pipelines. It is the culmination of the TUM direct-method line (DSO → VI-DSO → DM-VIO) and the system to study for how marginalization consistency issues (FEJ, unobservable scale) manifest — and are mitigated — in practice. Delayed marginalization is a general tool: the paper itself suggests keyframe reactivation for map reuse and PGBA for long-term loop closures.

Related

• DSO — the direct sparse odometry core.
• VI-DSO — the earlier direct visual-inertial predecessor from the same group.
• Basalt — the alternative fix for marginalization linearization error (nonlinear factor recovery).
• IMU Preintegration on Manifold — the IMU factor formulation used.
• Marginalization — background on the mechanism being “delayed.”
• Schur complement / Sparsity — the linear-algebra tool behind Eq. (10).
• Observability — why monocular scale is initially unobservable and must be handled explicitly.