PaperlayerView exact revision on arXiv

Source-linked AI summary

FAST-LIVO: Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry

Chunran Zheng, Qingyan Zhu, Wei Xu, Xiyuan Liu, Qizhi Guo, Fu Zhang

arXiv:2203.00893v1cs.RO

TL;DR

FAST-LIVO targets inaccurate and non-robust pose estimation from single sensors in challenging SLAM environments. It tightly couples direct LiDAR, visual, and inertial measurements using raw points and image patches, and reports strong accuracy and real-time operation across challenging scenes and processors.

  • Problem

    Single-sensor SLAM systems may not provide accurate and robust pose estimation in structure-less or texture-less environments.

  • Method

    FAST-LIVO uses tightly coupled LIO and VIO subsystems, registering raw LiDAR points to a map and aligning image patches through direct photometric errors in an error-state iterated Kalman filter.

  • Results

    0.04 m end-to-end error was achieved over a 79.52 m path in an extremely challenging visual-SLAM scene.

  • Takeaways & Limitations

    The system achieves best overall performance among the evaluated state-of-the-art LIO, VIO, and LIVO algorithms while operating in real time on Intel and ARM processors.

Abstract

from arXiv · show

To achieve accurate and robust pose estimation in Simultaneous Localization and Mapping (SLAM) task, multi-sensor fusion is proven to be an effective solution and thus provides great potential in robotic applications. This paper proposes FAST-LIVO, a fast LiDAR-Inertial-Visual Odometry system, which builds on two tightly-coupled and direct odometry subsystems: a VIO subsystem and a LIO subsystem. The LIO subsystem registers raw points (instead of feature points on e.g., edges or planes) of a new scan to an incrementally-built point cloud map. The map points are additionally attached with image patches, which are then used in the VIO subsystem to align a new image by minimizing the direct photometric errors without extracting any visual features (e.g., ORB or FAST corner features). To further improve the VIO robustness and accuracy, a novel outlier rejection method is proposed to reject unstable map points that lie on edges or are occluded in the image view. Experiments on both open data sequences and our customized device data are conducted. The results show our proposed system outperforms other counterparts and can handle challenging environments at reduced computation cost. The system supports both multi-line spinning LiDARs and emerging solid-state LiDARs with completely different scanning patterns, and can run in real-time on both Intel and ARM processors. We open source our code and dataset of this work on Github to benefit the robotics community.

I. INTRODUCTION

FAST-LIVO addresses the limitations of single-sensor SLAM in challenging environments with a fast, tightly coupled LiDAR-inertial-visual system. It combines direct raw-point LiDAR registration with sparse-direct image alignment while avoiding conventional visual feature extraction.

  • Single-sensor SLAM can lack accurate and robust pose estimation in structure-less or texture-less environments.
  • Existing LIVO systems often process LIO and VIO measurements separately, jointly fusing states without measurement-level coupling and requiring substantial computation.
  • FAST-LIVO builds two direct, tightly coupled LIO and VIO subsystems that jointly fuse LiDAR or visual measurements with IMUs.
  • The VIO subsystem reuses LiDAR map points with attached image patches to minimize direct photometric errors without extracting, triangulating, or optimizing visual features.
  • The implementation supports multi-line spinning and solid-state LiDARs and runs in real time on Intel and ARM processors.

B. LiDAR-Visual-Inertial SLAM

FAST-LIVO extends LiDAR-visual-inertial SLAM beyond feature-based systems by using raw measurements in tightly coupled LiDAR, visual, and inertial estimation. Its design combines frame-to-map visual alignment, LiDAR scan registration, IMU fusion, and outlier rejection.

  • Feature-based LiDAR-visual-inertial systems can be computationally costly because they extract sensor features and perform sliding-window optimization.
  • FAST-LIVO uses raw LiDAR points and image pixels rather than relying on extracted LiDAR or image features.
  • Unlike DVL-SLAM's frame-to-frame visual alignment, FAST-LIVO tightly couples frame-to-map image alignment, LiDAR scan registration, and IMU measurements.
  • Its outlier rejection solution is intended to improve accuracy when LiDAR measurements are projected onto image planes.
  • FAST-LIVO rejects occluded or depth-discontinuous visual map points before sparse-direct visual alignment.
  • The system fuses LiDAR, camera, and IMU measurements through a tightly coupled error-state iterated Kalman filter.

A. The boxplus “⊞” and boxminus “⊟” operator

The state-transition formulation represents estimation errors on a manifold while modeling the IMU-driven evolution of the system state. It assumes calibrated timing and rigid sensor mounting, with IMU biases modeled as random walks.

  • The boxplus and boxminus operations express state errors on the manifold M, with M defined as SO(3) × R^n in this formulation.
  • The system assumes known LiDAR-IMU-camera time offsets, rigid sensor attachment, and pre-calibrated extrinsic parameters.
  • The discrete state-transition model is evaluated at each IMU measurement using the state, input, process noise, and function f.
  • The transition model uses raw angular-velocity and acceleration measurements, gravity, and IMU biases with associated measurement and bias-process noises.

C. Forward Propagation

FAST-LIVO propagates the state and covariance forward using IMU measurements between successive LiDAR or image updates. The propagated estimate becomes the prior for the next measurement update.

  • C. Forward Propagation: The system uses forward propagation to predict the state and covariance at each IMU input.Process noise is set to zero during state propagation.
  • C. Forward Propagation: Covariance propagation uses the process-noise covariance Q and error-state transition models.
  • C. Forward Propagation: Propagation runs from the last LiDAR or image update until the next measurement arrives, incorporating intervening IMU inputs.The initial state and covariance come from the previous fused measurement.
  • C. Forward Propagation: LiDAR scans and images need not arrive simultaneously; either sensor can trigger a state update.

D. Frame-to-map Measurement Model

The LiDAR measurement model registers motion-distortion-compensated scan points against neighboring planes in an incremental global map. Predicted poses support nearest-neighbor plane fitting before evaluating the point-to-plane residual.

  • D. Frame-to-map Measurement Model: Backward propagation first compensates motion distortion so scan points are treated as simultaneous measurements in the LiDAR frame.
  • D. Frame-to-map Measurement Model: Each transformed scan point is associated with a neighboring map plane defined by its normal and center point.
  • D. Frame-to-map Measurement Model: The predicted pose transforms each point globally, then the five nearest map points are queried in an incremental ikd-tree to fit a plane.
  • D. Frame-to-map Measurement Model: The resulting point-to-plane equation provides an implicit state measurement model weighted by LiDAR measurement-noise covariance.

2) Sparse-Direct Visual Alignment Measurement Model:

The visual subsystem performs sparse-direct frame-to-map alignment by minimizing photometric errors between current images and image patches attached to visible map points. Optimization proceeds coarse-to-fine within the tightly coupled state estimation.

  • 2) Sparse-Direct Visual Alignment Measurement Model:: Visible global-map points provide reference patches selected from prior observations with the closest observation angle to the current image.
  • 2) Sparse-Direct Visual Alignment Measurement Model:: The visual measurement model minimizes photometric errors after projecting reference patches into the current image.
  • 2) Sparse-Direct Visual Alignment Measurement Model:: Alignment is optimized at three pyramid levels, progressing from coarser to finer resolution after convergence at each level.
  • 2) Sparse-Direct Visual Alignment Measurement Model:: The propagated state and covariance impose a prior, which is combined with LiDAR and visual measurements for maximum a posteriori state estimation.
  • 2) Sparse-Direct Visual Alignment Measurement Model:: When only one sensor arrives, the update fuses that sensor’s residual with IMU propagation, and the non-convex objective is solved iteratively by Gauss-Newton.

V. MAP MANAGEMENT

FAST-LIVO maintains separate LiDAR and visual global maps, with the visual map reusing LiDAR points augmented by image patches. The LiDAR map uses an incrementally maintained and downsampled ikd-tree.

  • V. MAP MANAGEMENT: The system maintains a LiDAR point cloud map for LIO and a patch-augmented point map for VIO.
  • V. MAP MANAGEMENT: The LiDAR global map contains past 3D points organized in an incremental ikd-tree supporting inquiry, insertion, deletion, downsampling, and dynamic rebalancing.
  • V. MAP MANAGEMENT: The visual global map reuses previously observed LiDAR points and attaches multiple image patches from their observing images.

1) Data Structure:

FAST-LIVO organizes visual map points in hash-indexed voxels and reuses LiDAR-derived points with image patches for direct visual alignment. It rejects occluded, depth-discontinuous, and unstable edge points before updating the visual map.

  • Data Structure: Hash-indexed axis-aligned voxels enable fast retrieval of visual map points near the current LiDAR scan and camera field of view.Each point stores its position, image-patch pyramids, and corresponding reference-camera poses.
  • Outlier Rejection: Occluded and depth-discontinuous map points are rejected by retaining lowest-depth projections and checking recent LiDAR points within 9 × 9 pixel neighborhoods.The procedure uses the predicted pose and a 40 × 40 pixel grid.
  • Patch Update: New image patches are attached to map points with high photometric error after sufficient temporal or spatial change from their previous reference.A new patch is added after more than 20 frames or when the point moves over 40 pixels; each patch is 8 × 8 pixels.
  • Map Update: Projected LiDAR points with the highest image gradient in each 40 × 40 pixel grid are added to the visual map with patches and image poses.Points with high local curvature are skipped to avoid adding edge points.

VI. EXPERIMENT AND RESULTS

The evaluation uses nine NTU-VIRAL sequences and private synchronized-sensor datasets, comparing FAST-LIVO with LiDAR-inertial, visual-inertial, LiDAR-visual, and tightly coupled LIVO systems. The benchmark uses consistent parameters and removes loop closure from DVL-SLAM for fairness.

  • Benchmark Dataset: Nine NTU-VIRAL sequences are evaluated using a left camera, a horizontal 16-channel OS1 gen14 LiDAR, and its internal IMU.The comparison includes R2LIVE, FAST-LIO2, SVO2.0, and DVL-SLAM.
  • Evaluation Protocol: All compared methods use recommended or tuned parameters, while DVL-SLAM’s loop-closure module is removed to match the odometry-only comparison.The modified DVL-SLAM retains sliding-window optimization.
  • Results: FAST-LIVO achieves the best accuracy on every NTU-VIRAL sequence except sbs 01, where fast UAV motion causes severe image blur and slightly higher error than FAST-LIO2.The reported explanation is that low-quality images do not improve odometry accuracy in that sequence.
  • Equipment Setup: The private platform uses an onboard computer, two industrial cameras, and Livox Avia LiDAR with hard synchronization at 10 Hz.Synchronization is generated by STM32 synchronized timers.

2) LiDAR Degenerated Experiment:

FAST-LIVO is evaluated in LiDAR-degenerated, visually challenging, and mapping scenarios. It maintains low drift in degenerated and aggressive-motion scenes and reconstructs detailed RGB-colored point clouds.

  • LiDAR Degenerated Experiment: 0.05m end-to-end drift is achieved in a 30-meter wall scene where FAST-LIO2 lacks along-wall constraints and SVO2.0 experiences repeated-feature and vertical-direction drifts.FAST-LIVO delivers the lowest end-to-end drift among the compared systems.
  • Visual Challenge Experiment: 0.04 m end-to-end error is reported over a 79.52 m path containing aggressive motions, indoor-outdoor transitions, and a texture-less white wall.The algorithm reliably survives the challenging visual scene and returns to the starting point.
  • High Precision Mapping with Colored Point Cloud: FAST-LIVO reconstructs a precise, dense, real-time RGB-colored 3D map of the HKU campus environment.The enlarged colored point cloud contains fine visual details similar to the actual RGB image.

C. Time Analysis

FAST-LIVO reduces processing time relative to R2LIVE while preserving real-time operation across Intel and ARM processors. The conclusion positions the system as a fast, robust, sparse-direct fusion framework validated in challenging environments.

  • Time Analysis: 36.75 ms is the mean complete processing time for FAST-LIVO per LiDAR and image frame, compared with 45.16 ms frontend plus 59.27 ms backend for R2LIVE.The comparison is performed on private datasets using an 8-core Intel Core i7-10700U desktop PC.
  • Time Analysis: FAST-LIVO runs in real time on both Intel and ARM processors with a significant computation margin.The time breakdown includes both the desktop PC and an embedded Qualcomm Kryo585 platform.
  • Conclusion: The system fuses LiDAR, inertial, and camera measurements in an error-state iterated Kalman filter using patch-based photometric error.The reported evaluations cover aggressive motion, indoor-outdoor transitions, texture-less walls, and LiDAR-degenerated environments.
Loading 2203.00893v1…