PaperlayerView exact revision on arXiv

Source-linked AI summary

Robocentric Visual-Inertial Odometry

Zheng Huai, Guoquan Huang

arXiv:1805.04031v1cs.RO

TL;DR

The paper addresses initialization and consistency problems in world-centric visual-inertial navigation by reformulating VINS around a moving local frame. Its sliding-window R-VIO analytically avoids observability mismatch, compensates degenerate motions without additional sensors, and is validated in simulations and real-world experiments.

  • Problem

    World-centric VINS can require difficult initial-pose alignment and suffer observability mismatch associated with estimation inconsistency.

  • Method

    R-VIO uses a moving local IMU frame, relative poses in a sliding-window filter, and tightly fused monocular-camera and IMU measurements.

  • Results

    The proposed R-VIO analytically avoids observability mismatch under generic motion and compensates planar-motion degradation without additional sensor information, with simulations and real-world experiments validating its performance.

  • Takeaways & Limitations

    The robocentric formulation provides a consistent, accurate, and robust motion-tracking alternative to world-centric VINS across different platforms and environments.

Abstract

from arXiv · show

In this paper, we propose a novel robocentric formulation of the visual-inertial navigation system (VINS) within a sliding-window filtering framework and design an efficient, lightweight, robocentric visual-inertial odometry (R-VIO) algorithm for consistent motion tracking even in challenging environments using only a monocular camera and a 6-axis IMU. The key idea is to deliberately reformulate the VINS with respect to a moving local frame, rather than a fixed global frame of reference as in the standard world-centric VINS, in order to obtain relative motion estimates of higher accuracy for updating global poses. As an immediate advantage of this robocentric formulation, the proposed R-VIO can start from an arbitrary pose, without the need to align the initial orientation with the global gravitational direction. More importantly, we analytically show that the linearized robocentric VINS does not undergo the observability mismatch issue as in the standard world-centric counterpart which was identified in the literature as the main cause of estimation inconsistency. Additionally, we investigate in-depth the special motions that degrade the performance in the world-centric formulation and show that such degenerate cases can be easily compensated in the proposed robocentric formulation, without resorting to additional sensors as in the world-centric formulation, thus leading to better robustness. The proposed R-VIO algorithm has been extensively tested through both Monte Carlo simulations and real-world experiments with different sensor platforms navigating in different environments, and shown to achieve better (or competitive at least) performance than the state-of-the-art VINS, in terms of consistency, accuracy and efficiency.

I. INTRODUCTION

The paper reformulates VINS around a moving local IMU frame to address initialization, consistency, and computational issues in world-centric systems. The resulting sliding-window R-VIO uses relative poses and analytically avoids observability mismatch while achieving strong simulation and real-world performance.

  • World-centric VINS estimates motion in a fixed global frame but can require inaccurate initialization under quick starts, latency, or poor vision.
  • R-VIO reformulates VINS around a local IMU frame, treating the global frame as a feature and shifting the local frame after image updates.
  • The sliding-window filter keeps a constant-size set of relative poses instead of observed features or global poses, reducing computational cost while tightly fusing camera and IMU measurements.
  • Under generic motion, R-VIO has a constant unobservable subspace and avoids the observability mismatch associated with inconsistency in standard world-centric VINS.
  • Monte Carlo simulations and real-world tests across sensor platforms and environments validate the proposed R-VIO's performance.

B. Propagation

The propagation stage advances the robocentric IMU state and covariance between image times using locally parameterized dynamics, IMU preintegration, and discrete-time error-state propagation.

  • The propagation model advances the robocentric state from one image interval to the next while treating the global state as static in the current local frame.
  • IMU measurements provide angular velocity and linear acceleration, with white Gaussian sensor noise and gravity represented in the IMU frame.
  • The continuous-time propagation uses bias-corrected angular velocity and acceleration to update velocity, while gyro and accelerometer biases follow constant-bias dynamics.
  • Position and velocity estimates are computed with IMU preintegration, while quaternion integration propagates orientation between image times.
  • Covariance propagation uses a forward-Euler discrete error-state transition and recursively propagates the augmented state covariance through the sliding window.

C. Update

The update stage converts monocular feature observations into inverse-depth residuals, removes landmark variables through nullspace projection, rejects outliers, compresses measurements, and performs an EKF update.

  • R-VIO models each landmark with inverse depth, using its bearing and depth together with relative camera poses across the observing frames.
  • The measurement Jacobians connect each feature observation to a sequence of relative poses, creating a dense state connection without increasing computational complexity.
  • Nullspace projection removes the inverse-depth error from the residual, yielding a measurement involving the filter state and projected image noise.
  • Landmarks pass a Mahalanobis-distance χ2 test before EKF updating, and QR decomposition compresses large stacked measurement models before the update.
  • After the update, the corrected relative pose is copied into the sliding window through stochastic cloning.

D. Composition

After each update, R-VIO shifts the estimation frame to the newest IMU frame and composes the state and covariance accordingly.

  • The newest IMU frame becomes the next robocentric reference frame after each EKF update.
  • Frame shifting resets the relative pose to the origin while leaving current-frame velocity and biases unchanged.
  • The covariance is composed with the state transformation, and the robocentric frame itself is assigned zero uncertainty.

E. Initialization

R-VIO initializes relative to the first local frame, using simple zero-pose initialization and stationary IMU measurements for gravity and bias estimates. Its processing then alternates visual tracking, IMU propagation, EKF updating, state augmentation, and frame composition.

  • Initialization: R-VIO initializes the state and covariance with respect to the first local frame when the first IMU measurements arrive.The first local frame is denoted {R0}, also serving as {G}.
  • Processing cycle: Visual tracking extracts features, applies KLT tracking and outlier rejection, and records inlier histories within the sliding window.
  • Processing cycle: IMU propagation preintegrates measurements from the last image time to predict the state and covariance.
  • Processing cycle: Completed feature tracks undergo inverse-depth modeling, Mahalanobis screening, EKF updating, and state augmentation using updated relative poses.
  • Processing cycle: Composition shifts the reference frame to the current IMU frame, updates global state and covariance, and resets the relative pose.

IV. OBSERVABILITY ANALYSIS

The observability analysis models a landmark observed by a moving sensor and examines propagation, measurement updates, and frame composition. It proves that the linearized R-VIO has a constant unobservable subspace and avoids observability mismatch.

  • Analysis setup: The analysis evaluates R-VIO observability for a single landmark observed by a mobile sensor under arbitrary motions, with conclusions generalizable to multiple landmarks.
  • Analysis setup: The state includes the landmark position in the current local frame, while bearing or inverse-depth measurements provide the observation model.
  • Observability formulation: The observability matrix stacks measurement Jacobians propagated by the state-transition matrix, and its nullspace represents state directions left uninformative by measurements.
  • Analysis formulation: The analysis covers a complete cycle of propagation and update followed by composition, because composition changes the local reference frame.
  • Main result: The authors analytically prove a constant unobservable subspace and no observability mismatch, which is associated with improved estimation performance.

A. Observability of propagation and update

For propagation and update under generic motion, the observability analysis establishes a nine-dimensional unobservable subspace. These directions correspond to global-frame transformations and a shared sensor–landmark translation.

  • Generic motion: For generic motion with nonzero angular velocity and acceleration, the relevant matrix blocks are linearly independent.Time-varying terms and differing time intervals provide the required independence.
  • Nullspace result: The observability matrix is rank deficient by 9, so its unobservable nullspace has rank 9.
  • Nullspace interpretation: The first six unobservable degrees of freedom represent global-frame orientation and position, while the final three represent a common translation of the sensor and landmarks.
  • Nullspace interpretation: These unobservable directions reflect that relative IMU and camera measurements provide no global state information.

B. Observability with composition

Including the composition step preserves the same unobservable subspace under generic motion. Thus, changing the local frame does not alter the system’s unobservable directions, supporting consistency.

  • Composition analysis: After propagation, update, and composition, the relevant transition terms remain linearly independent under generic motion.
  • Composition result: The composed observability matrix has the same nullspace as the propagation-and-update analysis.
  • Composition result: Changing the local reference frame through composition does not alter the unobservable subspace.
  • Implication: Because the unobservable subspace is constant and has the desired dimensions and directions, the model is expected to improve estimation consistency.

C. Observability under special motions

The robocentric formulation’s observability changes under special motions, with scale becoming unobservable in specific cases such as stationary motion and constant local acceleration. Monte Carlo simulations evaluate the resulting estimator against world-centric alternatives using RMSE and NEES.

  • Special motions: Planar motion introduces additional unobservable directions in world-centric VINS, while the robocentric analysis focuses on scale observability because global orientation is already unobservable.The additional planar-motion directions identified for world-centric VINS are global orientation and scale.
  • Scale unobservability: Scale ambiguity cannot be distinguished from camera and IMU measurements, so scale is an unobservable direction under the analyzed conditions.The analysis relates metric scaling by s to an error-state change along the scale direction by a factor of s − 1.
  • Special motions: Two special motions can cause scale unobservability: no rotations and constant local acceleration.The lemma identifies these motion conditions as the principal special cases for the proposed robocentric system.
  • Practical mitigation: Stationarity and deceleration-related cases can be mitigated in practice by increasing the inter-frame interval or relying on the inverse-depth measurement model.The text also notes that stationarity leads to low parallax.
  • Simulation evaluation: The simulation compares R-VIO with standard MSCKF and STOC-MSCKF using orientation and position RMSE and NEES over 50 Monte Carlo trials.The simulated camera/IMU follows a 5 m-radius circular path at 1 m/s with 1.5-pixel image noise and MEMS-quality IMU noise.
  • Simulation evaluation: R-VIO significantly outperforms both world-centric baselines in RMSE accuracy and NEES consistency, although its orientation NEES initially jumps before recovering.The initial jump is attributed to the small initialization covariance and resolves after a short period.

VI. EXPERIMENTAL RESULTS

Experiments across benchmark, hand-held, and urban-driving data validate R-VIO’s real-time operation, accuracy, and robustness across varied environments and motion conditions.

  • EuRoC dataset: R-VIO was evaluated on all 11 EuRoC sequences using a 20-pose sliding window and compared with OKVIS after 6DOF pose alignment.Only left-camera images were used, with 200 uniformly extracted features per image.
  • EuRoC dataset: R-VIO performs comparably to OKVIS and better in most EuRoC sequences.
  • Urban Driving dataset: The urban-driving evaluation compared high, low, and adaptive update rates because deceleration can make scale unobservable.The adaptive mode lowers the update rate after recognizing deceleration from speed changes.
  • Overall evaluation: The paper concludes that R-VIO provides long-term, high-precision 3D motion tracking in real time across different sensor platforms, environments, and motions.The conclusion attributes this validation to extensive Monte Carlo simulations and real-world experiments.

APPENDIX A: BUNDLE ADJUSTMENT USING

The appendix estimates landmark inverse depth from multi-frame image measurements using an iterative weighted bundle-adjustment procedure within the robocentric sliding window.

  • Landmark parameterization: A landmark observed across consecutive robocentric frames is parameterized by inverse depth λ = [φ, ψ, ρ]⊤.
  • Bundle adjustment: Given image measurements and known relative poses, the method formulates a Λ-weighted bundle-adjustment problem for estimating λ.
  • Optimization: Gauss–Newton iterations compute the optimal inverse-depth correction δλ∗ and update the estimate until convergence.The final estimate is λ∗ = ˆλ once the correction falls below a threshold.
  • Initialization: The initial angular parameters are obtained directly from a landmark measurement, while the inverse-depth parameter ρ is initialized at zero to place the landmark at infinity.The estimate is then refined iteratively.
  • Scale analysis: The scale analysis preserves rotation while scaling translation and landmark position, with gravity retaining known magnitude and therefore remaining unaffected by scaling.

APPENDIX C: PROOF OF LEMMA 2

The proof establishes when the scale direction is unobservable by examining the observability matrix under no-rotation and constant-local-acceleration motions.

  • Observability condition: The scale direction u is unobservable exactly when the relevant observability-matrix block lies in the right nullspace of H′p.
  • Special motions: The proof evaluates two special cases: zero angular velocity throughout the interval and constant local acceleration throughout the interval.
  • Conclusion: Both derived conditions must equal zero for the required relation to hold, completing Lemma 2.
Loading 1805.04031v1…