PaperlayerView exact revision on arXiv

Source-linked AI summary

Visual-Inertial Mapping with Non-Linear Factor Recovery

Vladyslav Usenko, Nikolaus Demmel, David Schubert, Jörg Stückler, Daniel Cremers

arXiv:1904.06504v3cs.CVcs.RO

TL;DR

Globally consistent visual-inertial mapping must reconcile large visual baselines with inertial information that degrades across long keyframe intervals. The paper recovers non-linear factors from VIO and integrates them into bundle adjustment, achieving better trajectory estimates than alternative preintegrated-IMU approaches on a public benchmark.

  • Problem

    Globally consistent mapping is difficult because visual methods require large-baseline keyframes while inertial information quickly degrades over long intervals.

  • Method

    The method hierarchically recovers non-linear factors summarizing accumulated high-frame-rate visual-inertial information between keyframes and uses them in global bundle adjustment.

  • Results

    The system shows better trajectory estimates than alternative approaches using preintegrated IMU measurements between keyframes on a public benchmark.

  • Takeaways & Limitations

    The recovered factors support globally consistent, gravity-aligned mapping while reducing optimization state dimensionality by avoiding velocity and bias estimation.

Abstract

from arXiv · show

Cameras and inertial measurement units are complementary sensors for ego-motion estimation and environment mapping. Their combination makes visual-inertial odometry (VIO) systems more accurate and robust. For globally consistent mapping, however, combining visual and inertial information is not straightforward. To estimate the motion and geometry with a set of images large baselines are required. Because of that, most systems operate on keyframes that have large time intervals between each other. Inertial data on the other hand quickly degrades with the duration of the intervals and after several seconds of integration, it typically contains only little useful information. In this paper, we propose to extract relevant information for visual-inertial mapping from visual-inertial odometry using non-linear factor recovery. We reconstruct a set of non-linear factors that make an optimal approximation of the information on the trajectory accumulated by VIO. To obtain a globally consistent map we combine these factors with loop-closing constraints using bundle adjustment. The VIO factors make the roll and pitch angles of the global map observable, and improve the robustness and the accuracy of the mapping. In experiments on a public benchmark, we demonstrate superior performance of our method over the state-of-the-art approaches.

I. INTRODUCTION

The paper addresses drift and weak long-interval inertial constraints in globally consistent visual-inertial mapping. It proposes recovering non-linear factors from local VIO and combining them with keypoint-based bundle adjustment.

  • VIO combines visual and IMU measurements for accurate, robust camera-motion tracking but local fixed-lag optimization remains prone to drift.
  • Globally consistent mapping is computationally difficult because dense frame optimization is costly, while keyframe subsampling weakens long-interval inertial constraints.High-frame-rate measurements create too many optimization parameters, and preintegrated inertial information provides little constraint after accumulated sensor noise.
  • The method uses a hierarchical two-layer formulation that recovers non-linear factors from marginalized local VIO information between keyframes for global bundle adjustment.The global layer combines these factors with keyframes and loop-closing-compatible visual measurements.
  • High-frame-rate visual-inertial tracking is combined with low-frame-rate keypoint matching and loop closing to produce globally consistent, gravity-aligned maps.The recovered factors also bridge segments lacking sufficient visual information.
  • The proposed VIO layer outperforms state-of-the-art methods in trajectory accuracy on the majority of evaluated sequences.
  • The recovered factors reduce the mapping state by avoiding velocity and bias estimation, while improving pose estimates in the gravity-aligned map.

III. PRELIMINARIES

This section defines the paper’s manifold-aware increments and Jacobians, then describes nonlinear least-squares optimization and fixed-lag visual-inertial tracking with marginalization.

  • Rotations use Exp and Log on SO(3), while poses are represented in SO(3) × R3 or SE(3), with ⊕ and ⊖ defining rotation increments and differences.
  • The Jacobian differentiates a residual function with respect to stacked state increments, applying the appropriate increment and difference operations component-wise.
  • The nonlinear least-squares objective minimizes a weighted squared norm of residuals, which is linearized near the current state for Gauss–Newton updates.
  • The camera–IMU system is tracked with fixed-lag smoothing using patch-based optical flow, reprojection errors, IMU propagation terms, and marginalization of old states.

A. KLT Tracking

The system tracks image patches across consecutive frames with robust KLT-based optical flow, while combining tracked features with preintegrated IMU measurements to estimate camera motion.

  • KLT feature tracking: FAST detects sparse keypoints, which are tracked across consecutive frames using KLT-based sparse optical flow.The tracker combines inverse compositional optimization with an intensity-scaling-invariant patch dissimilarity norm.
  • Patch motion estimation: Patch tracking estimates an SE(2) transform between corresponding patches by minimizing their intensity differences.The formulation uses a sum of squared residuals over patch pixels.
  • Robustness: KLT tracks remain usable for estimating the patch warp in SE(2) despite changes in exposure time.The figure illustrates example tracks estimated by the system under exposure variation.
  • Robustness: Pyramidal tracking handles large image displacements, while forward-backward consistency removes points classified as outliers.A point is discarded when tracking it back does not return it to its initial location.
  • Visual-inertial estimation: Camera motion estimation combines KLT feature-location errors with IMU errors based on preintegrated measurements.The estimated motion uses the world, IMU, and calibrated camera coordinate frames, with camera-to-world transforms computed from the optimized IMU pose and fixed extrinsics.
  • State representation: The state includes older keyframe IMU poses, recent-frame poses with velocities and biases, and landmarks stored relative to their first observing keyframe.Only keyframes host landmarks, which are represented by a unit-length camera-frame direction vector and inverse landmark distance.

1) Representation of Unit Vectors in 3D:

The system represents landmarks with a minimal stereographic parameterization and uses reprojection and preintegrated IMU residuals for visual-inertial motion estimation.

  • Representation of Unit Vectors in 3D:: Stereographic projection maps 2D coordinates (u, v) to a unit-length 3D bearing vector using a minimal two-dimensional representation.The parameterization avoids additional constraints and reduces optimization variables.
  • Representation of Unit Vectors in 3D:: All finite (u, v) values are valid, while only the negative Z-direction lacks a finite representation.This limitation is considered practical because cameras generally cannot observe points behind them.
  • Reprojection Error:: Reprojection residuals compare observed image coordinates with projections of landmarks hosted in one frame and observed in another.Landmarks use bearing coordinates and inverse distance, while camera poses transform them between frames.
  • IMU Error:: IMU factors preintegrate consecutive high-frequency measurements into pseudo-measurements of relative rotation, velocity, and position.Bias-corrected accelerations are integrated recursively, with bias changes handled by linear approximation.
  • IMU Error:: The IMU residuals incorporate gravity and pose rotation and translation, then use a covariance matrix reflecting accelerometer and gyroscope noise.The covariance can be calculated recursively from hardware-specific noise parameters.
  • Visual-Inertial Odometry:: The VIO subsystem uses local visual-inertial bundle adjustment and later approximates local keyframe information with nonlinear factors for global mapping.Keyframe poses and landmarks are distinguished from current states and the estimated trajectory in the subsystem visualization.

4) Optimization and Partial Marginalization:

The VIO layer minimizes reprojection, IMU, and marginalization-prior terms while partially marginalizing old states to keep the optimization bounded.

  • Optimization: Each new frame is optimized using reprojection terms over observed points, IMU terms between connected frames, and a marginalization prior.The energy combines visual and inertial constraints with information retained from marginalized variables.
  • Optimization: Gauss-Newton optimization fixes the number of keyframe poses and newest consecutive states optimized at each iteration.Partial marginalization follows optimization to prevent the problem size from growing.
  • Partial Marginalization: Marginalization removes either the oldest non-keyframe or velocity, biases, and an old keyframe with its landmarks when a new keyframe is added.Reprojection factors targeting marginalized frames are dropped to preserve sparsity.
  • Partial Marginalization: The system marginalizes variables on their linearized Markov blanket, whose H and b describe the estimated-state distribution in increment space.The Markov blanket contains incident states connected to the variables being removed.
  • Partial Marginalization: The Schur complement converts the joint linearized distribution into an energy term depending only on variables retained in the system.The resulting term is added to the total energy at the next iteration.
  • Linearization: First-estimate Jacobians keep marginalization-prior linearization points fixed while residuals are evaluated at the current state estimate.This preserves the nullspace properties of the linearized prior.

V. VISUAL-INERTIAL MAPPING

The mapping layer combines loop-closing bundle adjustment with nonlinear factors recovered from the lower-level VIO system to address drift and maintain global consistency.

  • V. VISUAL-INERTIAL MAPPING: The two-layered system runs visual-inertial odometry below bundle adjustment, with recovered nonlinear factors summarizing keyframe pose information above it.The mapping layer optimizes keyframe poses and keypoint positions while incorporating loop-closing constraints.
  • V. VISUAL-INERTIAL MAPPING: Keypoint matching implicitly detects loop closures and supports globally consistent mapping.The approach addresses drift accumulated by fixed-lag smoothing outside the active optimization window.
  • Factor Graphs: After marginalization, factor graphs retain older keyframes and recent frames while removing selected velocities, biases, poses, landmarks, and reprojection factors.The exact removals depend on whether the marginalized state belongs to a keyframe.
  • Nonlinear Factor Recovery: Nonlinear factor recovery replaces a dense marginalization factor with extracted nonlinear factors that approximate its stored distribution.The dense factor is saved before removing a keyframe pose and then approximated for reuse in mapping.

A. Global Map Optimization

Global map optimization uses statistically independent keyframe matches together with recovered nonlinear factors, optimizing keyframe poses and newly introduced landmark positions.

  • A. Global Map Optimization: ORB features distinct from VIO points are matched between global-map keyframes to provide statistically independent reprojection observations.These observations use the reprojection error function in the global optimization.
  • A. Global Map Optimization: The global objective combines ORB reprojection errors with error terms from recovered nonlinear factors.The recovered-factor term collects constraints supplied by the VIO layer.
  • A. Global Map Optimization: The global state contains keyframe poses and positions of newly added landmarks.The mapping layer interfaces with VIO at keyframe poses.
  • A. Global Map Optimization: When a VIO keyframe is marginalized, the system saves its Markov-blanket linearization and marginalizes all variables except keyframe poses.Nonlinear factors are then recovered from this prior to approximate its pose distribution.

B. Non-Linear Factor Recovery

Non-linear factor recovery approximates the dense visual-inertial information accumulated by VIO with a sparser set of non-linear factors. The approximation is constructed by matching Gaussian distributions through Kullback-Leibler divergence minimization.

  • Non-linear factor recovery transfers VIO information into globally consistent mapping while preserving a sparse factor-graph topology.It approximates a dense distribution stored in the original graph’s linearized Markov blanket with different non-linear factors.
  • Linearizing the original residual function yields a Gaussian distribution centered at the state estimate, which the recovered distribution approximates.
  • The recovered distribution is obtained by minimizing the Kullback-Leibler divergence between it and the original distribution.
  • Pseudo-measurements are chosen so residuals vanish at the original estimate, preserving the recovered distribution’s mean.
  • Information matrices are estimated from the Jacobians and block-diagonal factor information matrix, with a closed-form solution available for full-rank invertible Jacobians.

C. Non-Linear Factors for Distribution Approximation

When VIO marginalizes a keyframe, the method recovers sparse factors that retain its accumulated information relative to the remaining keyframes. It keeps relative-pose and roll-pitch information for mapping while discarding unobservable yaw and absolute-position information.

  • Marginalizing a keyframe produces a dense factor over the current keyframe poses, from which non-linear factors are recovered.
  • The recovered measurements include relative pose, roll-pitch, absolute position, and yaw quantities defined from the linearization state.
  • The method retains pairwise relative-pose factors and roll-pitch factors for the keyframe removed from the VIO window.
  • Yaw and absolute position factors are dropped because those four states are unobservable in VIO and their information comes only from the initial prior.
  • The mapping energy contains separate sets of relative-pose factors and roll-pitch factors.

VI. EVALUATION

On EuRoC, the proposed system performs strongly in both local VIO and global mapping evaluations, while recovered factors improve mapping accuracy and robustness. Its mapping stage also benefits from a smaller state and low measured processing time.

  • Accuracy: The proposed VIO achieves the best trajectory performance on eight of ten EuRoC sequences, compared with five for VI DSO.
  • Accuracy: The mapping system outperforms ORB-SLAM most clearly on machine hall sequences, where large environments create long intervals between keyframes.
  • Qualitative results: The reconstructed maps are globally consistent and gravity-aligned, including keyframe poses for segments without estimated ORB-feature matches.
  • Factor weighting: Properly weighted recovered factors provide better accuracy and robustness than using no factors or identity weights.
  • Timing: 2.5 smaller state size improves mapping efficiency by removing velocity and bias estimates from global optimization.
  • Timing: 19.2 seconds for VIO and 9.7 seconds for mapping process the 114-second MH 05 sequence, around 4x faster than real-time playback.

VII. CONCLUSIONS

The paper combines accurate visual-inertial odometry with globally consistent keyframe bundle adjustment by recovering non-linear factors between keyframes. This formulation improves trajectory estimates over preintegrated-IMU alternatives and can reduce optimization state dimensionality.

  • The hierarchical framework recovers factors from VIO to summarize accumulated inertial and visual information between keyframes.
  • The approach combines highly accurate VIO with globally consistent keyframe-based bundle adjustment.
  • The system shows better trajectory estimates than alternatives using preintegrated IMU measurements between keyframes on a public benchmark.
  • The formulation has potential to reduce optimization cost by reducing the dimensionality of the state space.
Loading 1904.06504v3…