Source-linked AI summary
Direct Sparse Visual-Inertial Odometry using Dynamic Marginalization
Lukas von Stumberg, Vladyslav Usenko, Daniel Cremers
TL;DR
Visual-inertial odometry must handle difficult imagery and initialization when metric scale is not immediately observable. VI-DSO combines direct photometric optimization with IMU constraints, jointly estimates scale and gravity direction, and uses dynamic marginalization. On EuRoC, the method and its initialization procedure outperform the state of the art.
Problem
Monocular visual-inertial odometry can face delayed or impossible scale observability, making initialization difficult for some motions.
Method
VI-DSO jointly optimizes camera poses and sparse geometry from photometric and IMU errors, while including scale and gravity direction and using dynamic marginalization.
Results
VI-DSO outperforms the state of the art on the EuRoC dataset, including its complete system and IMU initialization procedure.
Takeaways & Limitations
The inertial information provides a reliable scale estimate and increases precision and robustness.
Abstract
from arXiv · showhide
We present VI-DSO, a novel approach for visual-inertial odometry, which jointly estimates camera poses and sparse scene geometry by minimizing photometric and IMU measurement errors in a combined energy functional. The visual part of the system performs a bundle-adjustment like optimization on a sparse set of points, but unlike key-point based systems it directly minimizes a photometric error. This makes it possible for the system to track not only corners, but any pixels with large enough intensity gradients. IMU information is accumulated between several frames using measurement preintegration, and is inserted into the optimization as an additional constraint between keyframes. We explicitly include scale and gravity direction into our model and jointly optimize them together with other variables such as poses. As the scale is often not immediately observable using IMU data this allows us to initialize our visual-inertial system with an arbitrary scale instead of having to delay the initialization until everything is observable. We perform partial marginalization of old variables so that updates can be computed in a reasonable time. In order to keep the system consistent we propose a novel strategy which we call "dynamic marginalization". This technique allows us to use partial marginalization even in cases where the initial scale estimate is far from the optimum. We evaluate our method on the challenging EuRoC dataset, showing that VI-DSO outperforms the state of the art.
I. INTRODUCTION
VI-DSO addresses visual-odometry fragility in difficult imagery and delayed inertial observability with a tightly coupled direct visual-inertial system. It jointly optimizes poses, sparse geometry, scale, and gravity direction, using dynamic marginalization for changing variables.
- Visual odometry is vulnerable to low texture and fast maneuvers, motivating combination with inertial sensing.
- VI-DSO jointly optimizes sparse 3D geometry and camera poses using a bundle-adjustment-like photometric error augmented with IMU measurements.
- On EuRoC, the system processes all sequences with RMSE below 0.23 m and is reported to outperform the state of the art.
- Scale may remain unobservable for some motions, making initialization difficult and potentially requiring extended motion before all states are observable.
- The initialization strategy includes scale and gravity direction in the model and jointly optimizes them after initialization.
- Dynamic marginalization adaptively applies partial marginalization when variables, including scale, undergo substantial changes.
III. DIRECT SPARSE VISUAL-INERTIAL ODOMETRY
The system minimizes photometric and inertial errors in a nonlinear optimization over recent frames, combining coarse per-frame tracking with keyframe visual-inertial bundle adjustment. Older states are marginalized to keep computation feasible.
- The optimization minimizes photometric and inertial errors over a window of recent frames while marginalizing older frames.
- The estimated poses and depths are obtained from an energy function combining photometric error Ephoto and inertial error Einertial.
- Coarse tracking runs for every frame, using direct image alignment and an inertial error term to estimate the latest pose.
- For each new keyframe, visual-inertial bundle adjustment jointly estimates geometry and poses of active keyframes, including scale from the beginning.
A. Notation
The paper defines coordinate-frame notation and a photometric error over sparse point observations, then adds inertial constraints based on a nonlinear motion model and IMU preintegration.
- Transformations between coordinate frames use T_i^j ∈ SE(3), while matrices, vectors, and scalars use distinct notation.
- B. Photometric Error: The photometric residual compares a point in a reference image with its projection into another frame, correcting exposure and affine illumination changes.
- B. Photometric Error: The total photometric error aggregates residuals over optimized keyframes, sparse points, and observations of each point.
- The inertial error is built from a nonlinear dynamic model using gyroscope rotational velocities and accelerometer linear measurements.
- IMU preintegration compresses high-frequency measurements between two camera frames into a single factor with an associated covariance.
D. IMU Initialization and the problem of observability
IMU initialization is difficult because metric scale and gravity direction may remain unobservable under some motions. VI-DSO includes both as optimization variables, allowing initialization at arbitrary scale while jointly refining the system state.
- Observability: Certain motions can prevent immediate initialization because metric scale and gravity direction are not yet observable.Constant-velocity motion with zero acceleration is one example; a prior system used 15 seconds of EuRoC motion for initialization.
- Initialization strategy: VI-DSO jointly optimizes scale and gravity direction with poses and geometry instead of waiting until they become observable.This strategy permits initialization with an arbitrary scale.
- Initialization strategy: The visual initializer supplies a rough two-frame pose estimate and approximate point depths normalized to an average depth of 1.These values provide the initial visual state before joint optimization.
- Initialization strategy: The initial gravity direction is estimated by averaging up to 40 accelerometer measurements.The paper reports that this remains sufficiently accurate even under high acceleration.
- Initialization strategy: Velocity and IMU biases start at zero, scale starts at 1.0, and all parameters are jointly optimized in bundle adjustment.The scale initialization is therefore deliberately provisional rather than required to be metric.
- Coordinate-frame model: The DSO frame is modeled as a scaled and rotated version of the metric frame, separating photometric optimization from metric inertial constraints.Photometric error is evaluated in the DSO frame, while inertial error uses the metric frame.
F. Scale-aware Visual-inertial Optimization
The scale-aware optimization estimates active-keyframe states in the scale-independent DSO frame while coupling photometric and inertial constraints through metric-frame representations.
- State and factors: The system jointly optimizes poses, IMU biases, and velocities for a fixed number of active keyframes.Each IMU factor connects consecutive keyframes using preintegrated measurements, with intervals capped at 0.5 seconds.
- State and factors: Optimized poses are represented in the DSO frame, so they do not depend on the environment’s scale.Metric-frame quantities are introduced separately for inertial residuals.
- State and factors: Each keyframe state includes pose, velocity, IMU bias, affine illumination parameters, and inverse depths for hosted points.The state also contains geometric camera parameters and the transformation between DSO and metric frames.
- Nonlinear optimization: Gauss-Newton optimization combines visual and inertial contributions as H = Hphoto + Himu and b = bphoto + bimu.Because the residuals are distinct, the visual and inertial components can be formed independently before combination.
- Nonlinear optimization: Inertial residuals compare relative poses in the metric IMU frame, requiring additional inertial state vectors and a pose-representation conversion Jacobian.The conversion uses Jrel, while transformations are represented in sim(3) with scale fixed except for ξm d.
2) Marginalization using the Schur-Complement:
Partial marginalization removes older keyframe variables with the Schur complement while preserving a consistent optimization through fixed linearization choices and graph partitioning.
- Marginalization: Older keyframes are partially marginalized to keep Gauss-Newton updates computationally feasible.Pose, bias, velocity, and affine illumination variables are eliminated using the Schur complement.
- Marginalization: Visual marginalization first removes points and then the keyframe, dropping residuals that would damage system sparsity.This follows the visual marginalization procedure used in DSO.
- Factor-graph partitioning: The factor graph can be partitioned into Gmetric containing all IMU factors and Gvisual containing factors independent of ξm d.Neither partition contains marginalization factors.
- Marginalization: The Schur-complement factor depends on the connected variables’ linearization point, so further linearization points require an energy approximation.The paper applies the cited approximation to continue optimization around updated points.
- Consistency: First Estimates Jacobians keep Jacobians consistent for variables connected to marginalization factors, preventing elimination of nullspaces.Visual Jacobians use the linearization point, while inertial Jrel evaluation points are fixed for connected variables.
3) Dynamic Marginalization for Delayed Scale Convergence:
Dynamic marginalization maintains multiple priors and replaces them when scale drifts from a prior’s linearization point, preserving consistency while retaining inertial information.
- Marginalization reduces optimization complexity while preserving information about previous states, but scale is not yet reliably estimated.
- Dynamic marginalization maintains several priors and resets the active prior when the scale estimate moves too far from its linearization point.
- Fig. 4 tracks current and ground-truth scale, the allowed interval, and dotted-line events that determine which inertial factors enter Mcurr.
- Mvisual stores marginalized visual information independently of scale, while Mcurr and Mhalf retain inertial or recent scale-consistent information.
- The method exchanges Mcurr and Mhalf when scale boundaries are exceeded, updating the priors and their linearization points to preserve consistent historical information.
- The interval factor di must remain small for consistency but large enough for Mcurr to contain sufficient inertial factors.
G. Coarse Visual-Inertial Tracking
Coarse tracking rapidly estimates each frame’s pose by direct image alignment, providing initialization for subsequent joint visual-inertial optimization.
- Coarse tracking performs direct image alignment between the current frame and latest keyframe while keeping geometry and scale fixed.
- The resulting fast pose estimate initializes the joint optimization, which incorporates preintegrated inertial residuals between subsequent frames.
IV. RESULTS
The method is evaluated on the publicly available EuRoC dataset, including an experiment assessing the effect of adding IMU measurements.
- The evaluation uses the publicly available EuRoC dataset to assess the proposed visual-inertial odometry approach.
- The cumulative error experiment reports that adding IMU measurements provides reliable scale estimation and significantly improves accuracy and robustness.
A. Robust Quantitative Evaluation
The evaluation compares VI-DSO with visual, visual-inertial, stereo, and SLAM methods on EuRoC, emphasizing accuracy and robustness across challenging sequences.
- VI-DSO is run 10 times per EuRoC sequence and compared directly with visual-only DSO and ROVIO using specified trajectory-scaling procedures.
- Table I reports median errors for VI-DSO across 10 runs, whereas most competing methods contribute only one reported result.
- VI-DSO outperforms VI ORB-SLAM in rmse on several sequences and has a lower maximum rmse even among sequences where ORB-SLAM succeeds.
- VI-DSO outperforms monocular alternatives on every sequence and beats stereo and SLAM versions on 9 out of 11 sequences.
- VI-DSO successfully tracks all sequences except ROVIO.
- On the hard V1 0* sequence, VI-DSO outperforms both compared methods without stereo or loop closures.
B. Evaluation of the Initialization
The proposed initialization estimates scale jointly and provides an approximate scale and gravity direction almost instantly, while refining scale over time. On EuRoC, it achieved lower scale errors and greater robustness than visual-inertial ORB-SLAM, and dynamic marginalization supports large initial scale errors.
- Initialization accuracy: 0.7% average scale error versus 1.0% for visual-inertial ORB-SLAM, with a lower maximum error of 1.2% versus 3.4%.The proposed method provided a better scale on most evaluated sequences.
- Initialization robustness: The proposed initialization remained robust on V1 03 difficult, where visual-inertial ORB-SLAM's initialization failed.Both methods estimate scale, gravity direction, bias, and velocity on the challenging EuRoC dataset.
- Initialization procedure: The method provides approximate scale and gravity direction almost instantly, then enhances them over time instead of waiting 15 seconds for initialization.Inertial data is used to improve pose estimation from the beginning.
- Scale convergence: Scale converges to the correct value on MH 04 difficult despite initialization far from the optimum.The figure reports the median result of 10 runs in terms of tracking accuracy.
- Dynamic marginalization: Dynamic marginalization maintains multiple marginalization priors and constrains the maximum scale difference when the initial scale is far from optimal.This technique enables partial marginalization while preserving consistency.