PaperlayerView exact revision on arXiv

Source-linked AI summary

LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping

Tixiao Shan, Brendan Englot, Carlo Ratti, Daniela Rus

arXiv:2104.10831v2cs.RO

TL;DR

LVI-SAM targets robust real-time lidar-visual-inertial odometry and mapping despite complementary sensor weaknesses. It tightly couples visual-inertial and lidar-inertial subsystems through a factor graph, exchanging initialization, depth, scan-matching, and loop-closure information. In evaluation, lidar depth further reduced LVIO translation error by 29%, from 45.42 m to 32.18 m, while the system remained effective across varied datasets.

  • Problem

    Lidar and vision odometry each have distinct weaknesses, motivating tightly coupled lidar-visual-inertial estimation for robust state estimation and mapping.

  • Method

    LVI-SAM uses a factor graph coupling VIS and LIS, with exchanged initialization, lidar-derived visual depth, scan-matching guesses, and loop-closure refinement.

  • Results

    29% translation-error reduction, from 45.42 m to 32.18 m, occurred when lidar-derived feature depth was enabled in LVIO without loop closure.

  • Takeaways & Limitations

    LVI-SAM provides a tightly coupled real-time odometry and mapping framework that can continue operating when one subsystem fails.

Abstract

from arXiv · show

We propose a framework for tightly-coupled lidar-visual-inertial odometry via smoothing and mapping, LVI-SAM, that achieves real-time state estimation and map-building with high accuracy and robustness. LVI-SAM is built atop a factor graph and is composed of two sub-systems: a visual-inertial system (VIS) and a lidar-inertial system (LIS). The two sub-systems are designed in a tightly-coupled manner, in which the VIS leverages LIS estimation to facilitate initialization. The accuracy of the VIS is improved by extracting depth information for visual features using lidar measurements. In turn, the LIS utilizes VIS estimation for initial guesses to support scan-matching. Loop closures are first identified by the VIS and further refined by the LIS. LVI-SAM can also function when one of the two sub-systems fails, which increases its robustness in both texture-less and feature-less environments. LVI-SAM is extensively evaluated on datasets gathered from several platforms over a variety of scales and environments. Our implementation is available at https://git.io/lvi-sam

I. INTRODUCTION

LVI-SAM addresses complementary weaknesses of lidar-, vision-, and inertial-based odometry through a tightly coupled factor-graph framework. Its VIS and LIS exchange estimates, operate independently when needed, and are validated across varied platforms and environments.

  • Lidar methods capture long-range detail but can fail in structure-less environments, while vision methods are sensitive to illumination, rapid motion, and initialization.IMU coupling improves the respective robustness and accuracy of lidar- and vision-based systems.
  • LVI-SAM combines a visual-inertial system and lidar-inertial system in a factor graph for tightly coupled multi-sensor fusion and global optimization.The framework uses place recognition to aid global optimization.
  • The VIS and LIS can function independently after failure detection, allowing the framework to bypass failed subsystems and remain robust to sensor degradation.The subsystems also exchange information when both provide sufficient features.
  • LVI-SAM is extensively validated using data from varied scales, platforms, and environments as a baseline for lidar-visual-inertial odometry.The authors report comparable or improved accuracy relative to existing publicly available methods.

A. System Overview

The system comprises a visual-inertial subsystem and a lidar-inertial subsystem that jointly optimize sensor constraints while maintaining real-time operation through sliding-window estimation.

  • LVI-SAM receives 3D lidar, monocular-camera, and IMU inputs and divides processing between VIS and LIS.VIS uses visual and IMU measurements, with lidar measurements optional; LIS performs lidar-feature matching against a feature map.
  • The VIS obtains visual odometry by minimizing joint visual and IMU residuals in a sliding-window bundle-adjustment framework.The visual-inertial state contains rotation, position, velocity, and IMU bias variables.
  • The LIS extracts lidar features and matches them with a sliding-window feature map to perform lidar odometry with bounded computation.The overall state-estimation problem is formulated for joint optimization.

1) Initialization:

LVI-SAM improves visual-inertial initialization and feature-depth estimation by using lidar-observable depth and lidar-derived state estimates.

  • 1) Initialization:: Optimization-based VIO can diverge at initialization because metric scale is poorly observable under insufficient acceleration excitation.Initialization also depends on accurate IMU bias and noise parameters.
  • 1) Initialization:: LVI-SAM initializes the LIS first and uses its estimated state and IMU bias as interpolated initial guesses for VIS initialization.The LIS benefits from directly observable lidar depth during its initialization.
  • 2) Feature depth association:: Lidar frames are registered to the camera frame and stacked to form a denser depth map for visual-feature depth estimation.Feature and lidar points are projected onto a unit sphere, and nearby depth points are used to estimate feature depth geometrically.
  • 2) Feature depth association:: Registered depth maps distinguish depth changes by color, while green and red visual features indicate successful and failed depth association, respectively.
  • 2) Feature depth association:: Feature-depth estimates are rejected when the maximum distance among the three nearest depth points exceeds 2m.This check addresses ambiguity caused by stacking frames from different timestamps and objects.

3) Failure detection:

The framework detects visual failures and uses a factor-graph lidar-inertial subsystem with multiple constraints and loop-closure support to maintain robust estimation.

  • 3) Failure detection:: VIS failure is associated with aggressive motion, illumination change, and texture-less environments that reduce tracked features and can cause optimization divergence.
  • 3) Failure detection:: The LIS factor graph jointly optimizes IMU preintegration, visual odometry, lidar odometry, and loop-closure constraints.Lidar odometry constraints come from scan-matching against a global feature map.
  • 3) Failure detection:: The VIS reports failure when tracked features or estimated IMU bias cross thresholds, then re-initializes and informs the LIS.Active detection prevents VIS failure from corrupting LIS operation.
  • 3) Failure detection:: VIS identifies loop-closure candidates using DBoW2 and sends their image timestamps to the LIS for further validation.

C. Lidar-Inertial System

The lidar-inertial system maintains a factor graph that jointly optimizes IMU, visual odometry, lidar odometry, and loop-closure constraints. It uses IMU integration before initialization and relies on initial motion estimates for scan-matching.

  • Factor-graph optimization: LIS maintains a factor graph with IMU preintegration, visual odometry, lidar odometry, and loop-closure constraints optimized jointly.Lidar odometry comes from matching the current lidar keyframe against a global feature map.
  • Initial guess: Initial guesses are critical for scan-matching, particularly during aggressive motion, and their source changes before and after LIS initialization.
  • Initial guess: Before LIS initialization, the system assumes a static start with zero velocity and integrates raw IMU measurements with zero bias and noise assumptions.
  • Failure conditions: Degenerate scenes such as ground-facing lidar or flat, open, structure-less environments make scan-matching ill-constrained.Figure 6 depicts these degraded mapping scenarios; color variation represents elevation change.

4) Loop closure detection:

The system uses LIS initialization and failure detection to support robust scan-matching. It identifies degraded lidar conditions by examining the scan-matching optimization and avoids adding unreliable lidar constraints.

  • Initialization: Two lidar keyframes provide the scan-matching initial guess, enabling initialization when linear velocity is below 10 m/s and angular velocity below 180 °/s.
  • Initialization: After LIS initialization, visual-inertial estimates provide initial guesses when available, while corrected-bias IMU integration is used if VIS fails.This switching procedure improves initial-guess accuracy and robustness in texture-rich and texture-less environments.
  • Failure detection: Scan-matching becomes ill-constrained in degraded lidar scenes, motivating explicit LIS failure detection.The paper illustrates ground-facing and flat, structure-less environments as examples.
  • Failure detection: LIS reports failure when the smallest eigenvalue of A^T A falls below a threshold during the first optimization iteration.Lidar odometry constraints are omitted from the factor graph after failure is detected.
  • Ablation trajectories: Figure 7 presents Urban-dataset trajectories for the ablation study, enabling visual comparison of system configurations.
  • Error evaluation: Table I reports end-to-end translation and rotation errors for the evaluated configurations.

III. EXPERIMENTS

The experiments evaluate LVI-SAM on three self-gathered datasets using a multimodal sensor suite and compare it with open-sourced state-estimation and mapping solutions.

  • Datasets and sensors: Experiments use Urban, Jackal, and Handheld datasets collected with lidar, camera, IMU, and GPS for ground truth.The sensor suite includes a Velodyne VLP-16, FLIR camera, MicroStrain IMU, and Reach RS+ GPS.
  • Baselines: The evaluation compares LVI-SAM with open-sourced solutions including VINS-Mono, LOAM, and LIO.

A. Ablation Study

The Urban ablation study isolates lidar scan-matching, visual-inertial estimation, lidar-derived feature depth, and loop closure. Combining these components improves robustness and reduces pose error in degraded settings.

  • Dataset: The Urban dataset includes buildings, vehicles, pedestrians, cyclists, and vegetation, with deliberately challenging sensor placements in a GPS-denied region.The route starts and ends at the same position for end-to-end translation evaluation.
  • A1 - Feature depth: With LIS scan-matching disabled, VIS-only trajectories are compared with and without lidar depth registration.The trajectory direction is clockwise, and Table I reports the resulting end-to-end pose errors.
  • A1 - Feature depth: Depth registration for visual features greatly reduces end-to-end pose errors in the VIS-only ablation.
  • A2 - VIS assistance: With VIS disabled, the LIS-only trajectory diverges multiple times when degraded scenarios are encountered.
  • A3 - Feature depth: Using VIS and LIS together, lidar-derived feature depth reduces translation error by 29%, from 45.42 m to 32.18 m.Loop closure detection is disabled in this test to evaluate pure odometry.

4) A4 - Effect of including visual loop closure detection:

With visual loop closure enabled, LVI-SAM eliminates system drift and achieves the lowest average RMSE and rotational error against GPS on the Jackal dataset.

  • 4) A4 - Effect of including visual loop closure detection:: LVI-SAM eliminates drift when visual loop closure detection is enabled, producing the A4 trajectory in Figure 7.The final trajectory with every module enabled is labeled A4.
  • 4) A4 - Effect of including visual loop closure detection:: LVI-SAM achieves the lowest average RMSE against GPS measurements treated as ground truth on the Jackal dataset.The dataset uses a feature-rich environment and begins and ends at the same position.
  • 4) A4 - Effect of including visual loop closure detection:: LVI-SAM achieves the lowest end-to-end rotational error, while LINS achieves the lowest end-to-end translation error.LINS is adapted from LeGO-LOAM and designed specifically for UGV operations.
  • 4) A4 - Effect of including visual loop closure detection:: Figure 9 compares method trajectories with GPS positioning measurements shown as red-dot ground truth; several methods are omitted after failing to generate meaningful results.The compared trajectories are shown in green.

C. Handheld Dataset

On the challenging Handheld dataset, LVI-SAM completes the open-field test and achieves the lowest errors across all three benchmarking criteria, with or without loop closures.

  • C. Handheld Dataset: LVI-SAM achieves the lowest errors across all three benchmarking criteria on the Handheld dataset.The dataset challenges the system with an open baseball field where camera observations mainly contain grass and lidar observations mainly contain a ground plane.
  • C. Handheld Dataset: LVI-SAM successfully finishes the Handheld test both with and without loop closures enabled.The test uses sensor data collected while an operator walks through several open fields.
  • C. Handheld Dataset: LVI-SAM is designed as a tightly coupled VIS-LIS framework for real-time state estimation and mapping in complex environments.Evaluations span datasets covering varied scales, platforms, and environments.
Loading 2104.10831v2…