PaperlayerView exact revision on arXiv

Source-linked AI summary

On-Manifold Preintegration for Real-Time Visual-Inertial Odometry

Christian Forster, Luca Carlone, Frank Dellaert, Davide Scaramuzza

arXiv:1512.02363v3cs.RO

TL;DR

Real-time visual-inertial odometry becomes difficult as trajectories and high-rate inertial data make full optimization increasingly costly. This paper introduces manifold-aware IMU preintegration within a factor-graph pipeline with structureless vision, achieving 100 Hz full smoothing and higher accuracy than competing approaches.

  • Problem

    Full-smoothing VIO becomes computationally infeasible in real time as trajectory and map sizes grow and high-rate IMU measurements add variables rapidly.

  • Method

    The paper develops SO(3)-aware IMU preintegration and combines it with iSAM2 incremental smoothing and structureless visual factors that eliminate 3D landmarks.

  • Results

    100 Hz full smoothing is achieved with superior accuracy to competitive state-of-the-art filtering and optimization approaches.

  • Takeaways & Limitations

    The approach provides constant-time incremental smoothing while retaining accuracy comparable to batch estimation in the reported odometry experiment.

Abstract

from arXiv · show

Current approaches for visual-inertial odometry (VIO) are able to attain highly accurate state estimation via nonlinear optimization. However, real-time optimization quickly becomes infeasible as the trajectory grows over time, this problem is further emphasized by the fact that inertial measurements come at high rate, hence leading to fast growth of the number of variables in the optimization. In this paper, we address this issue by preintegrating inertial measurements between selected keyframes into single relative motion constraints. Our first contribution is a \emph{preintegration theory} that properly addresses the manifold structure of the rotation group. We formally discuss the generative measurement model as well as the nature of the rotation noise and derive the expression for the \emph{maximum a posteriori} state estimator. Our theoretical development enables the computation of all necessary Jacobians for the optimization and a-posteriori bias correction in analytic form. The second contribution is to show that the preintegrated IMU model can be seamlessly integrated into a visual-inertial pipeline under the unifying framework of factor graphs. This enables the application of incremental-smoothing algorithms and the use of a \emph{structureless} model for visual measurements, which avoids optimizing over the 3D points, further accelerating the computation. We perform an extensive evaluation of our monocular \VIO pipeline on real and simulated datasets. The results confirm that our modelling effort leads to accurate state estimation in real-time, outperforming state-of-the-art approaches.

I. INTRODUCTION … A. Notions of Riemannian geometry

The paper develops a real-time visual-inertial odometry system that combines manifold-aware IMU preintegration, factor-graph incremental smoothing, and structureless visual measurements. It reviews the computational trade-offs among filtering and smoothing methods, then introduces the SO(3) and SE(3) geometry underlying its MAP formulation.

  • I. INTRODUCTION: VIO combines camera geometry, which lacks metric scale, with IMU measurements that provide metric scale and robust inter-frame motion estimates.These complementary sensors support applications including GPS-denied navigation, 3D reconstruction, and augmented reality.
  • I. INTRODUCTION: The paper addresses the accuracy-efficiency trade-off by enabling fast incremental smoothing while computing the optimal maximum a posteriori estimate in real time.Filtering is fast but accumulates linearization errors, whereas full nonlinear smoothing is accurate but computationally demanding.
  • I. INTRODUCTION: Its first contribution is manifold-aware preintegration, combining inertial measurements between keyframes into one relative motion constraint while properly modeling rotations on SO(3).The theory provides analytic Jacobians and supports a-posteriori bias correction.
  • I. INTRODUCTION: The second contribution embeds preintegrated IMU measurements in factor graphs, enabling incremental smoothing and structureless vision that eliminates all 3D-point variables.This framework is intended to avoid accumulated linearization errors and further accelerate computation.
  • II. RELATED WORK: Related VIO methods differ by how many camera poses they estimate, how uncertainty is represented, and how often measurement models are linearized.These dimensions distinguish full smoothing, fixed-lag smoothing, filtering, covariance or information representations, and single- versus repeated-linearization methods.
  • A. Filtering: Filtering restricts inference to the latest state, while structureless filtering marginalizes landmarks but delays landmark processing and can lock in linearization or outlier errors.The EKF’s complexity grows quadratically with the number of estimated landmarks, so typically about 20 landmarks are tracked for real-time operation.
  • B. Fixed-lag Smoothing: Fixed-lag smoothers optimize states within a recent window and marginalize older states, generally improving accuracy and outlier resilience by relinearizing past measurements.They provide a compromise between full smoothing and filtering by limiting the optimized state history.
  • C. Full Smoothing: Full smoothing offers the highest accuracy but becomes infeasible as trajectories and maps grow, while high-rate IMU data makes adding a state per measurement impractical.The paper reports full-smoothing operation at 100 Hz with superior accuracy over competitive filtering and optimization approaches.

B. Uncertainty Description in SO(3)

The paper models rotational uncertainty by placing a Gaussian perturbation in the tangent space and mapping it to SO(3) through the exponential map. The resulting distribution accounts for the logarithmic coordinate change and yields a geodesic-distance-based likelihood under small-covariance assumptions.

  • Uncertainty representation: Rotational uncertainty is represented by a zero-mean Gaussian perturbation with covariance Σ in the tangent space around a noise-free mean rotation R.The perturbation is mapped from the tangent space to SO(3) using the exponential map.
  • Distribution derivation: Applying ϵ = Log(R^-1˜R) converts the Euclidean Gaussian integral into an explicit distribution over rotations.The logarithmic reparameterization is valid for perturbations with norm below π.
  • Likelihood: The negative log-likelihood is interpreted geometrically as the squared geodesic angle between ˜R and R weighted by Σ^-1.This connects the rotational likelihood to a Mahalanobis-weighted distance on SO(3).
  • Distribution derivation: The change of variables introduces a normalization factor involving the determinant of the right Jacobian Jr(Log(R^-1˜R)).The Jacobian term arises as a by-product of the coordinate transformation.
  • Small-covariance approximation: For small covariances, the normalization factor is approximated as constant because the right Jacobian is close to the identity near R.The model clips probability tails outside the open ball of radius π, so it assumes relatively small covariance Σ.

C. Gauss-Newton Method on Manifold

The paper extends Gauss-Newton optimization to manifold-valued variables by lifting updates into a suitable tangent space, solving there, and retracting the result back onto the manifold. This avoids over-parameterization and infeasible updates while supporting convenient retractions for SO(3) and SE(3).

  • Motivation: Directly optimizing manifold variables can over-parameterize the problem and produce solutions outside the manifold.For example, rotations may be represented with 9 matrix elements although 3D rotations require only a vector in R3, potentially making normal equations under-determined.
  • Lift-Solve-Retract: A retraction maps a tangent-space increment δx to a neighborhood of the current manifold estimate, enabling local Euclidean optimization.This lifting uses a tangent space of suitable dimension, such as δx ∈ R3 for SO(3).
  • Lift-Solve-Retract: The Gauss-Newton update lifts the problem, solves a quadratic approximation for δx⋆ in the tangent space, and retracts the increment onto the manifold.The same lift-solve-retract scheme also generalizes to trust-region methods and provides a unifying view of error-state models.
  • Retractions: The exponential map is a possible retraction, but it may be computationally inconvenient.The work therefore specifies alternative retractions for SO(3) and SE(3).
  • Retractions: For SE(3), the retraction at T = (R, p) is RT(δφ, δp) = (R Exp(δφ), p + R δp), with [δφ δp] ∈ R6.This choice eliminates the need to compute the exponential map for SE(3).

IV. MAXIMUM A POSTERIORI VISUAL-INERTIAL STATE ESTIMATION … V. IMU MODEL AND MOTION INTEGRATION

The paper formulates VIO as MAP estimation over keyframe states using IMU and monocular-camera measurements, factorized in a factor graph with preintegrated inertial and structureless visual constraints. It models IMU-driven motion through continuous-time kinematics and a discrete integration scheme whose constant-orientation approximation is mitigated by high-rate sensing.

  • IV. MAXIMUM A POSTERIORI VISUAL-INERTIAL STATE ESTIMATION: The VIO system tracks an IMU body frame with known camera–IMU calibration while a front-end supplies image measurements of unknown 3D landmarks.The sensing platform may be a mobile robot, UAV, or hand-held device.
  • A. The State: Each keyframe state contains IMU orientation, position, velocity, and gyroscope and accelerometer biases, with pose in SE(3) and velocity in R3.The implementation estimates keyframe states while excluding 3D landmarks under a structureless formulation.
  • B. The Measurements: Measurements comprise multiple landmark observations at each camera keyframe and IMU sets collected between consecutive keyframes, ranging from a few to hundreds of samples.The number of IMU measurements depends on sensor rate and keyframe-selection frequency.
  • C. Factor Graphs and MAP Estimation: The posterior is maximized, equivalently minimizing a negative log-posterior that becomes a sum of squared visual, inertial, and prior residuals under zero-mean Gaussian noise.Residuals quantify measurement–prediction mismatch and use corresponding covariance matrices.
  • C. Factor Graphs and MAP Estimation: Independence and Markov assumptions yield a factor graph in which multiple IMU measurements are summarized by one preintegrated factor and structureless vision connects keyframes sharing landmarks.Known measurements are treated as parameters rather than variables in the graphical model.
  • V. IMU MODEL AND MOTION INTEGRATION: The IMU model uses accelerometer and gyroscope measurements with additive white noise and slowly varying bias, representing angular velocity and acceleration in specified sensor and world frames.Earth rotation is neglected by treating the world frame as inertial.
  • V. IMU MODEL AND MOTION INTEGRATION: Motion is inferred with a kinematic model describing pose and velocity evolution, then integrated over intervals while assuming acceleration and angular velocity remain constant.The integration expresses these quantities through IMU measurements.
  • V. IMU MODEL AND MOTION INTEGRATION: The discrete scheme assumes constant orientation during each integration interval, which is not exact under non-zero rotation rates; a high-rate IMU mitigates this approximation.The paper adopts this scheme for practical integration between measurements.

VI. IMU PREINTEGRATION ON MANIFOLD · A. Preintegrated IMU Measurements

The paper compresses high-rate IMU measurements between consecutive keyframes into a single manifold-aware relative-motion constraint, avoiding state insertion at every IMU sample. It then reformulates the constraint as a noise-explicit measurement model that supports straightforward likelihood construction.

  • VI. IMU PREINTEGRATION ON MANIFOLD: High-rate IMU factors would otherwise require adding new states at every measurement, causing rapid growth of the estimation problem.
  • VI. IMU PREINTEGRATION ON MANIFOLD: All IMU measurements between keyframes i and j are summarized as one preintegrated measurement constraining motion between consecutive keyframes.The theory extends earlier Euler-angle preintegration to the manifold SO(3).
  • VI. IMU PREINTEGRATION ON MANIFOLD: Direct integration must be repeated whenever the linearization point at time ti changes, because future rotations and accumulated sums depend on it.
  • VI. IMU PREINTEGRATION ON MANIFOLD: Relative motion increments are defined independently of the pose and velocity at ti, enabling their computation directly from measurements between keyframes.The velocity and position increments are not the true physical changes because they are constructed to remove initial-state and gravity dependence.
  • VI. IMU PREINTEGRATION ON MANIFOLD: The remaining dependence of the preintegrated increments on the bias estimate is handled by assuming bi known in Section VI-A and addressing changing bias estimates later.The paper assumes the bias remains constant between two keyframes.
  • A. Preintegrated IMU Measurements: The keyframe relation is rewritten as a preintegrated measurement model whose compound measurements equal estimated state terms plus rotation, velocity, and position noise.The reformulation uses first-order approximations and drops higher-order noise terms.
  • A. Preintegrated IMU Measurements: The model explicitly defines preintegrated rotation, velocity, and position measurements together with their associated noises.
  • A. Preintegrated IMU Measurements: With suitable noise distributions, the reformulated model makes log-likelihood construction straightforward; additive Gaussian terms produce quadratic negative log-likelihoods.

B. Noise Propagation

The section derives the covariance of preintegrated IMU noise, emphasizing its importance for weighting the MAP estimator. Under first-order approximations, rotation, velocity, and position noises are modeled as zero-mean Gaussian variables whose covariance can be propagated online in iterative form.

  • Covariance derivation: The covariance Σij strongly influences the MAP estimator because its inverse weights the optimization terms.The section derives Σij for the preintegrated measurements from the IMU noise covariance.
  • Rotation noise: Up to first order, preintegrated rotation noise δφij is zero-mean Gaussian because it is a linear combination of zero-mean gyroscope noise terms.This places the rotation measurement model in the desired Gaussian form.
  • Velocity and position noise: Velocity and position noises δvij and δpij are also zero-mean Gaussian because they combine acceleration noise with preintegrated rotation noise.The stated relations are valid up to first order.
  • Online covariance propagation: Σij is obtained by linear propagation from the IMU noise covariance and can be updated iteratively as new measurements arrive.The iterative update avoids recomputing the covariance from scratch and is more amenable to online inference.

C. Incorporating Bias Updates … VII. STRUCTURELESS VISION FACTORS

The paper efficiently updates preintegrated measurements when bias estimates change, models bias evolution in the factor graph, and eliminates visual landmarks linearly while preserving the optimal MAP estimate.

  • C. Incorporating Bias Updates: A first-order expansion updates preintegrated delta measurements after a small bias change, avoiding computationally expensive recomputation.The correction operates directly on SO(3).
  • C. Incorporating Bias Updates: Bias-update Jacobians describe measurement sensitivity, remain constant, and can be precomputed during preintegration.They are evaluated at the bias estimate used during integration.
  • D. Preintegrated IMU Factors: The preintegrated IMU model yields residual errors with bias updates and analytic Jacobians suitable for Gauss-Newton linearization.The residual model assumes zero-mean Gaussian measurement noise up to first order.
  • E. Bias Model: Slowly time-varying biases are modeled as Brownian motion, producing discrete bias noises with covariances proportional to the keyframe interval.The covariances are Σbgd .= ∆tijCov(ηbg) and Σbad .= ∆tijCov(ηba).
  • E. Bias Model: The discrete bias model is incorporated into the factor graph as an additional additive term for consecutive keyframes.This extends the graph model across each keyframe interval.
  • VII. STRUCTURELESS VISION FACTORS: The structureless vision model linearly eliminates landmarks at every Gauss-Newton iteration while retaining the optimal MAP estimate.This avoids optimizing over 3D landmark positions.
  • VII. STRUCTURELESS VISION FACTORS: Linearizing reprojection residuals and minimizing over each landmark perturbation produces pose-only factors through landmark elimination.The resulting cost uses an orthogonal projector and can be further manipulated for efficient implementation.
  • VII. STRUCTURELESS VISION FACTORS: The approach reduces pose-and-landmark factors to L pose-only factors and supports multiple relinearizations with incremental inclusion of new measurements.Updated landmark positions are obtained through fast linear triangulation.

VIII. EXPERIMENTAL ANALYSIS · A. Simulation Experiments · 1) Pose Estimation Accuracy and Timing:

The simulation experiments evaluate pose-estimation accuracy and timing using a controlled camera trajectory and compare batch optimization with incremental iSAM2 inference. The analysis frames iSAM2 as preserving real-time capability while batch computation grows with trajectory length.

  • VIII. EXPERIMENTAL ANALYSIS: Simulation results are presented as accurate, fast, and consistent, with separate real-data experiments later comparing against state-of-the-art methods.
  • A. Simulation Experiments: The simulated camera follows a 120-meter circular trajectory with three-meter radius and sinusoidal vertical motion, observing at most 50 landmark measurements per frame.
  • A. Simulation Experiments: Landmark measurements are corrupted with isotropic Gaussian noise having standard deviation σpx = 1 pixel.
  • 1) Pose Estimation Accuracy and Timing:: The experiments cover pose-estimation accuracy and timing, consistency, bias-estimation accuracy, and first-order bias correction.
  • 1) Pose Estimation Accuracy and Timing:: Batch nonlinear optimization gives the optimal MAP estimate, but its computational cost quickly increases as trajectory length grows.
  • 1) Pose Estimation Accuracy and Timing:: iSAM2 performs close-to-optimal inference while preserving real-time capability, and Fig. 6 compares its CPU time with batch estimation.

2) Consistency: … B. Real Experiments

The experiments evaluate estimator consistency, bias tracking and correction, manifold-based preintegration advantages over Euler angles, and integration into a monocular VIO pipeline. Results show bounded NEES, accurate bias tracking, exact rotation integration, and improved real-time accuracy relative to state-of-the-art methods.

  • 2) Consistency:: The proposed estimator’s average NEES remains below the upper bound of 7.0, indicating that it is not overconfident.The average NEES approaches the lower bound, while the acceptance interval is [5.0, 7.0].
  • 3) Bias Estimation Accuracy:: The estimated gyroscope and accelerometer biases correctly track the ground-truth biases in simulation.Because smoothing can revise the entire bias history, the estimates are visualized with multiple curves.
  • 4) First-Order Bias Correction:: A first-order correction updates preintegrated measurements when older bias estimates change, avoiding repeated integration.The correction is evaluated through an additional Monte Carlo analysis.
  • 5) Advantages over the Euler-angle-based formulation:: Unlike Euler-angle integration, the proposed exponential-map integration is exact regardless of the sampling time and angular rate.Euler-angle integration is only first-order accurate, and its error accumulates when Δt or angular rate is large.
  • 5) Advantages over the Euler-angle-based formulation:: The SO(3) formulation avoids the Euler-angle likelihood’s dependence on the chosen world frame under rigid-body transformations.The Euler-angle negative log-likelihood is not invariant, so different world-frame choices may produce different estimates.
  • 5) Advantages over the Euler-angle-based formulation:: Euler-angle parameterization suffers gimbal-lock singularities at pitch values θ = π/2 + nπ, whereas the proposed rotation-manifold formulation avoids this issue.The paper evaluates the singularity’s effect on preintegrated measurement-noise computation using Monte Carlo trajectories with increasing maximum pitch.
  • B. Real Experiments: The proposed inertial factors are integrated into a monocular VIO pipeline and benchmarked against state-of-the-art filtering and fixed-lag smoothing algorithms.Experiments include indoor motion-capture ground truth and longer outdoor trajectories, with results supporting greater accuracy and fast real-time inference.

1) Implementation: … IX. CONCLUSION

The implementation combines high-rate SVO tracking with iSAM2 optimization, while experiments demonstrate accurate, consistent, and real-time VIO indoors and outdoors. The conclusion emphasizes on-manifold preintegration, structureless vision factors, constant-time smoothing, and efficient inference.

  • 1) Implementation:: The implementation tracks salient image features at camera rate while iSAM2 concurrently optimizes selected keyframe states.SVO provides the tracking front-end and iSAM2 the optimization back-end.
  • 1) Implementation:: SVO exploits previously triangulated feature depths to jointly track features through sparse image alignment, photometric error minimization, and epipolar constraints.This differs from tracking features individually with standard Lucas-Kanade methods.
  • 1) Implementation:: The approach does not marginalize past states, allowing future extension to incorporate loop closures if desired.The stated design prioritizes fast visual-inertial odometry without removing past states.
  • 2) Indoor Experiments:: 0.3m drift on average over 360m traveled distance was achieved indoors, compared with 0.7m average error for both OKVIS and MSCKF.The comparison used relative drift over trajectory segments of {10, 40, 90, 160, 250, 360}m.
  • 2) Indoor Experiments:: The indoor estimation errors remained within the 3-sigma uncertainty bounds in a single real-world run.The authors note that evaluating average NEES would require multiple runs with different sensor-noise realizations.
  • 3) Outdoor Experiments:: The outdoor office-building experiment reported 1.5m end-to-end error for the proposed approach versus 2.2m for Google Tango.The trajectory started and ended at the same location, enabling end-to-end error measurement.
  • 3) Outdoor Experiments:: A three-floor trajectory returning to its starting point produced 0.5m end-to-end error for the proposed approach versus 1.4m for Tango.Because the systems used different sensors, the authors characterize this comparison as qualitative.
  • IX. CONCLUSION: The conclusion presents on-manifold preintegration, structureless visual measurements, and iSAM2 constant-time incremental smoothing as the paper’s main contributions.The preintegration theory models many IMU measurements as one motion constraint without fixing a linearization point during integration.

APPENDIX · A. Iterative Noise Propagation · B. Bias Correction via First-Order Updates

The appendix derives an iterative formulation for propagating preintegrated IMU noise covariance and a first-order method for updating preintegrated rotation, velocity, and position when the bias estimate changes.

  • A. Iterative Noise Propagation: The preintegrated noise covariance can be computed iteratively, yielding simpler expressions that are more amenable to online inference.
  • A. Iterative Noise Propagation: Preintegrated rotation, velocity, and position noise are rewritten recursively by extracting the final measurement term and rearranging the sums.
  • A. Iterative Noise Propagation: The recursive noise relations are assembled into a compact matrix model using the IMU measurement noise.
  • A. Iterative Noise Propagation: Given the raw IMU measurement-noise covariance Ση ∈ R6×6, the preintegrated measurement covariance can then be propagated iteratively.
  • B. Bias Correction via First-Order Updates: The bias-correction derivation starts from preintegrated variables evaluated at an old bias estimate and seeks updates when the estimate changes by a small correction.
  • B. Bias Correction via First-Order Updates: A first-order expansion expresses the preintegrated rotation at the new bias as the old preintegrated rotation plus a correction, avoiding repeated integration.
  • B. Bias Correction via First-Order Updates: The same derivation extends bias correction to preintegrated velocity and position, producing the Jacobians used for the a posteriori bias update.
  • B. Bias Correction via First-Order Updates: The Jacobians for bias correction can be computed incrementally as new measurements arrive.

C. Jacobians of Residual Errors

This section derives analytic Jacobians for the preintegration residual errors by lifting the cost onto a vector space through manifold-aware retractions. It identifies residual-specific variable dependencies, including zero Jacobians for states absent from each residual.

  • Purpose: Analytic Jacobians of the residual errors support iterative optimization methods such as Gauss–Newton for minimizing the cost.The derivations target the residual errors in Eq. (45) and the cost in Eq. (26).
  • Retractions: Lifting applies retractions that update rotations multiplicatively with Exp(δφ) and Euclidean states additively, making residuals functions on a vector space.The retractions are specified for Ri, Rj, pi, pj, vi, vj, and bias perturbations.
  • Jacobian structure: For r∆pij, Jacobians with respect to δφj and δvj are zero because Rj and vj do not appear in the residual.The residual is also linear in δbg i and δba i, with remaining Jacobians derived separately.
  • Jacobian structure: For r∆vij, Jacobians with respect to δφj, δpi, and δpj are zero because Rj, pi, and pj do not appear in the residual.The remaining Jacobians account for the residual’s dependence on the other perturbation variables and bias terms.
  • Jacobian structure: For r∆Rij, Jacobians with respect to pi, pj, vi, vj, and δba i are zero because these quantities do not appear in the rotation residual.The remaining rotation-residual Jacobians require a more involved derivation.

D. Structureless Vision Factors: Null Space Projection · E. Rotation Rate Integration Using Euler Angles

The paper presents an efficient null-space implementation of structureless vision factors and recalls Euler-angle integration of rotation-rate measurements with first-order covariance propagation.

  • D. Structureless Vision Factors: Null Space Projection: D. Structureless Vision Factors: Null Space Projection uses an orthogonal projector to map vectors into the null space of the landmark matrix E_l.The projector operates in R^(2n_l), where n_l is the number of cameras observing landmark l.
  • D. Structureless Vision Factors: Null Space Projection: D. Structureless Vision Factors: Null Space Projection states that E_l has rank 3, giving its null space dimension 2n_l − 3.The null-space basis E_l^⊥ has dimensions R^(2n_l × (2n_l−3)).
  • D. Structureless Vision Factors: Null Space Projection: A unitary null-space basis can be computed from E_l using singular value decomposition.The basis satisfies the unitary relation described in the passage.
  • D. Structureless Vision Factors: Null Space Projection: The null-space representation is computationally preferable because it avoids matrix inversion and requires fewer matrix multiplications.It provides an alternative representation of the structureless vision-factor cost function.
  • E. Rotation Rate Integration Using Euler Angles: E. Rotation Rate Integration Using Euler Angles integrates the measured rotation rate ˜ω_k and noise η_g into the next Euler-angle vector θ_k+1.The update uses the conjugate Euler angle rate matrix E′(θ_k).
  • E. Rotation Rate Integration Using Euler Angles: The covariance of θ_k+1 is approximated through first-order propagation.The propagation uses the Jacobian-related quantities ∂θ_k, B_k, and the measurement-noise covariance Σ_η.
Loading 1512.02363v3…