PaperlayerView exact revision on arXiv

Source-linked AI summary

FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry

Chunran Zheng, Wei Xu, Zuhao Zou, Tong Hua, Chongjian Yuan, Dongjiao He, Bingyang Zhou, Zheng Liu, Jiarong Lin, Fangcheng Zhu, Yunfan Ren, Rong Wang, Fanle Meng, Fu Zhang

arXiv:2408.14035v2cs.ROcs.CV

TL;DR

FAST-LIVO2 addresses the challenge of accurate, robust, and efficient LIVO under high-rate heterogeneous sensing and degraded visual or geometric conditions. It tightly fuses raw LiDAR, image, and IMU measurements through a sequential ESIKF and unified voxel map, with plane-aware and patch-based direct alignment. Across benchmark sequences and applications, the system achieves high localization accuracy and robustness while supporting real-time onboard operation and dense colored mapping.

  • Problem

    Efficient and accurate LIVO remains difficult because high-rate LiDAR and high-resolution images strain onboard computation, while feature extraction can be limited in structureless or textureless environments.

  • Method

    FAST-LIVO2 sequentially fuses raw LiDAR, image, and IMU measurements in an ESIKF using direct updates and a shared voxel map with plane priors and image patches.

  • Results

    0.044 m average RMSE across all benchmark sequences, compared with 0.137 m for FAST-LIVO, while the system also demonstrates real-time UAV navigation and airborne mapping.

  • Takeaways & Limitations

    FAST-LIVO2 provides fast, accurate, and robust state estimation with dense colored map reconstruction for onboard navigation, mapping, and subsequent 3D rendering applications.

Abstract

from arXiv · show

This paper proposes FAST-LIVO2: a fast, direct LiDAR-inertial-visual odometry framework to achieve accurate and robust state estimation in SLAM tasks and provide great potential in real-time, onboard robotic applications. FAST-LIVO2 fuses the IMU, LiDAR and image measurements efficiently through an ESIKF. To address the dimension mismatch between the heterogeneous LiDAR and image measurements, we use a sequential update strategy in the Kalman filter. To enhance the efficiency, we use direct methods for both the visual and LiDAR fusion, where the LiDAR module registers raw points without extracting edge or plane features and the visual module minimizes direct photometric errors without extracting ORB or FAST corner features. The fusion of both visual and LiDAR measurements is based on a single unified voxel map where the LiDAR module constructs the geometric structure for registering new LiDAR scans and the visual module attaches image patches to the LiDAR points. To enhance the accuracy of image alignment, we use plane priors from the LiDAR points in the voxel map (and even refine the plane prior) and update the reference patch dynamically after new images are aligned. Furthermore, to enhance the robustness of image alignment, FAST-LIVO2 employs an on-demanding raycast operation and estimates the image exposure time in real time. Lastly, we detail three applications of FAST-LIVO2: UAV onboard navigation demonstrating the system's computation efficiency for real-time onboard navigation, airborne mapping showcasing the system's mapping accuracy, and 3D model rendering (mesh-based and NeRF-based) underscoring the suitability of our reconstructed dense map for subsequent rendering tasks. We open source our code, dataset and application on GitHub to benefit the robotics community.

I. INTRODUCTION

FAST-LIVO2 addresses the accuracy, robustness, and computational challenges of LIVO by tightly fusing LiDAR, images, and IMU data through direct measurement updates. Its unified voxel map and practical optimizations support diverse sensors, real-time operation, and applications including navigation and mapping.

  • Motivation: Single-sensor SLAM systems suffer from complementary limitations: visual methods lack direct depth, while LiDAR methods lack color and can degrade under weak geometric constraints.These limitations motivate fusing LiDAR, cameras, and IMU measurements.
  • Motivation: LIVO remains challenging because systems must process high-rate LiDAR and high-resolution images efficiently, while feature extraction can fail in structureless or textureless environments.The challenge is especially significant under limited onboard computing resources.
  • Approach: FAST-LIVO2 tightly integrates LiDAR, image, and IMU measurements through a sequentially updated ESIKF using direct methods and a unified voxel map.LiDAR updates register raw points, while image updates use map-associated patches and direct photometric errors.
  • Approach: The framework improves alignment through refined LiDAR plane priors, dynamic reference-patch selection, online exposure estimation, and on-demand voxel raycasting.These mechanisms target alignment accuracy and robustness under lighting variation or missing LiDAR measurements.
  • Evaluation and applications: FAST-LIVO2 supports multi-line and solid-state LiDARs, pinhole and fisheye cameras, and real-time operation on Intel and ARM processors.The authors evaluate the system on 25 public-dataset sequences and additional private datasets against representative SLAM systems.
  • Evaluation and applications: The system is applied to onboard autonomous UAV navigation, airborne mapping, and high-quality 3D model rendering.The applications demonstrate real-time navigation, mapping precision, and use of dense colored maps for rendering.
  • Relation to direct methods: Direct LiDAR and visual methods optimize raw measurements rather than relying on extracted features and descriptors.The visual and LiDAR modules minimize photometric and point-to-plane errors, respectively.
  • Relation to prior systems: Compared with pixel-level or separate-map systems, FAST-LIVO2 uses sparse patch alignment and tightly couples frame-to-map visual, LiDAR, and inertial updates.This design reduces reliance on frame-to-frame optical flow and avoids the computational cost of dense pixel-level alignment.

III. SYSTEM OVERVIEW

The system overview combines synchronized LiDAR, image, and IMU processing in a sequential ESIKF and a shared voxel map. LiDAR builds geometric structure, while visual measurements use map-associated patches for direct image alignment.

  • Architecture: FAST-LIVO2 contains ESIKF, local mapping, LiDAR measurement, and visual measurement components.The architecture is organized around a sequentially updated ESIKF framework.
  • Sequential processing: Asynchronously sampled LiDAR points are recombined into scans at camera sampling times before sequential state updates.This aligns LiDAR and image data for processing at the same update frequency.
  • Measurement updates: LiDAR and image measurements are incorporated sequentially using direct methods based on a single unified voxel map.The LiDAR update uses point-to-plane residuals, while the visual update uses frame-to-map photometric errors.
  • Local mapping: Visual images append patches to selected map points and dynamically update reference patches, whose normal vectors are refined in a separate thread.This lets the visual map evolve alongside LiDAR geometric updates.
  • Coordinate and calibration assumptions: The system assumes known inter-sensor time offsets, rigid sensor attachment, and pre-calibrated extrinsic parameters.The IMU frame is used as the body frame and the first body frame as the global frame.
  • Sequential processing: The system propagates the state and covariance forward with IMU inputs, then backward-propagates LiDAR points to compensate for motion distortion.The propagated state and covariance provide the prior for subsequent measurement updates.

B. Scan Recombination

Scan recombination converts sequentially sampled LiDAR points into scans aligned with camera sampling moments. This synchronizes LiDAR and camera data for same-time state updates.

  • Scan recombination: Scan recombination segments high-frequency sequential LiDAR points into distinct scans at camera sampling moments.The process synchronizes both sensors at the same frequency, such as 10 Hz, enabling simultaneous state updates.

C. Propagation

FAST-LIVO2 propagates the state and covariance with IMU inputs, then sequentially updates the ESIKF using LiDAR followed by camera measurements.

  • Propagation: IMU forward propagation predicts the state and covariance between successive LiDAR scans and image frames, forming the prior for subsequent updates.Backward propagation compensates for motion distortion by aligning LiDAR measurements to the scan-end time.
  • Propagation: The IMU-propagated state and covariance impose the prior distribution for the state at the current time.
  • Sequential Update: Sequential updating addresses the dimension mismatch between LiDAR and image measurements while remaining theoretically equivalent to a joint update under conditional independence.
  • Sequential Update: The first update fuses LiDAR with the IMU prior, and the second fuses camera measurements with the LiDAR-updated distribution.
  • ESIKF Update: Each measurement update linearizes the model around the current iterated state and uses Jacobians, residuals, and measurement noise in the ESIKF step.
  • ESIKF Update: After convergence, the updated state and covariance define the posterior distribution, which then supplies the prior for the visual update after LiDAR fusion.

V. LOCAL MAPPING

FAST-LIVO2 maintains a bounded adaptive voxel map that jointly stores LiDAR geometry and selected image patches for LiDAR and visual updates.

  • Map Structure: The adaptive voxel map uses a hash table with octrees, variable-size leaf voxels, plane features, raw LiDAR points, and attached three-level image patches.Variable leaf sizes represent local planes at different scales.
  • Local Map Slide: The local map remains bounded by sliding a fixed-size region when the LiDAR detection area reaches its boundary and recycling memory for incoming areas.A ring-buffer hash map resets memory moved outside the local region.
  • Geometry Update: Registered LiDAR points initialize or extend hash-map voxels, whose contained points are tested with singular value decomposition to estimate planes and their uncertainties.
  • Geometry Update: Existing voxel planes are updated when new points remain coplanar, subdivided otherwise, and fixed after convergence as mature planes.
  • Visual Map Points: Visual map points are selected from visible, high-gradient LiDAR candidates, retaining the smallest-depth point per local plane and attaching image patches.
  • Reference Patch Update: Reference patches are scored using photometric similarity and viewing angle, favoring representative appearance and near-orthogonal views for preserving texture details.

E. Normal Refine

FAST-LIVO2 refines LiDAR-derived plane normals through photometric alignment, reparameterizing the constrained optimization into an unconstrained two-dimensional problem.

  • Normal Prior: Each visual map point lies on a local plane, whose LiDAR-derived normal supports affine warping more accurately than the constant-depth assumption.
  • Affine Warping: Affine warping maps source-patch pixels to target patches using the local plane normal, point position, relative pose, and camera projection models.The implementation supports pinhole and fisheye cameras without image rectification.
  • Normal Optimization: The plane normal is refined by minimizing photometric errors between a reference patch and target image patches while accounting for inverse exposure times.
  • Normal Optimization: The constrained normal optimization is reparameterized through M and then m ∈R2, allowing unconstrained optimization in a separate thread.The optimized m* is used to recover the optimal plane normal.
  • Normal Optimization: After convergence, the refined normal and selected reference patch are fixed, while the remaining patches are deleted.

VI. LIDAR MEASUREMENT MODEL

The LiDAR measurement model registers undistorted scan points in the global frame and constrains them using planes stored in the voxel map.

  • Measurement Model: The LiDAR update uses the measurement model y_l = h_l(x, v_l) within the ESIKF.
  • Point Registration: Undistorted scan points are projected into the global frame using the iterated state estimate.
  • Point-to-Plane Constraint: Points without a matching voxel or plane are discarded; otherwise, the voxel plane supplies the measurement equation for the point.
  • Point-to-Plane Constraint: The model assumes the ground-truth point lies on the voxel plane defined by its normal and center, with estimated plane parameters carrying covariance.
  • Measurement Noise: LiDAR measurement noise includes uncertainty in the point, plane normal, and plane center.

B. LiDAR Measurement Noise with Beam Divergence

FAST-LIVO2 models LiDAR uncertainty from time-of-flight, bearing, and laser-beam divergence, then uses that model to improve point selection and pose estimation. Its visual map-point pipeline combines voxel queries, on-demand raycasting, and outlier rejection to build reliable photometric measurements.

  • LiDAR uncertainty: Laser-beam divergence increases ranging uncertainty as the beam intersects surfaces at larger angles, while bearing-direction uncertainty remains unaffected.Ground points can receive higher uncertainty because the beam spreads over a larger area.
  • LiDAR uncertainty: Accounting for time-of-flight and divergence-related ranging uncertainty yields more precise pose estimation when selecting points from ground or walls.
  • Visual map-point selection: The visual submap is formed by querying voxels hit by current LiDAR points or previously visible map points, leveraging likely overlap between sensor fields of view.
  • Visual map-point selection: On-demand raycasting fills uncovered 30 × 30-pixel image cells by sampling backward rays from dmin to dmax until map-containing voxels are found.Sample-point positions are pre-computed in the camera body frame to reduce computation.
  • Visual map-point selection: After voxel querying and raycasting, the system rejects occluded, depth-discontinuous, or poorly viewed map points before constructing photometric errors.It keeps the lowest-depth point per grid cell and compares points against a LiDAR-derived depth map and 9 × 9 neighborhoods.

B. Sparse-Direct Visual Measurement Model

The visual update performs sparse direct image alignment using LiDAR-derived visual map points and photometric errors. It supports camera models and exposure variation while using inverse compositional optimization and multilevel updates for efficient alignment.

  • Photometric measurement: Visual map points are transformed into the current image so the update minimizes photometric error between reference and current patches.The visual update uses a three-level coarse-to-fine process and then regenerates map points and reference patches.
  • Photometric measurement: The projection model supports pinhole, MEI, ATAN, Scaramuzza, and Equidistant cameras, with affine warping based on relative reference-to-current poses.
  • Photometric measurement: Image pixel values from reference and current frames are modeled with measurement noise from sources including shot noise and CMOS ADC noise.
  • Efficient optimization: Inverse compositional alignment moves the pose increment into the reference-frame formulation, allowing Jacobians to be computed once per iteration level.
  • Exposure estimation: Real-time exposure estimation fixes τ0 = 1, making subsequent inverse exposure times relative to the first frame and avoiding degeneracy when all values are zero.

IX. EXPERIMENT RESULTS

FAST-LIVO2 is evaluated on public benchmarks with controlled implementations and configurations, alongside a private dataset targeting extreme sensing conditions. It achieves the strongest reported average benchmark accuracy while module ablations identify exposure estimation and reference-patch updates as important contributors.

  • Dataset and setup: FAST-LIVO2’s private dataset contains 20 sequences spanning extreme conditions including LiDAR degeneration, low illumination, drastic exposure changes, and absent LiDAR measurements.The dataset, hardware, and synchronization scheme are released with the code.
  • Benchmark results: 0.044 m average RMSE gives FAST-LIVO2 the highest overall accuracy across 25 NTU-VIRAL and Hilti sequences, versus 0.137 m for FAST-LIVO.FAST-LIVO2 performs best on most sequences, while FAST-LIO2 is slightly better in “Outside Building” and “Large Room (dark)” because blurred images add little value.
  • Ablation results: Removing real-time exposure estimation decreases average accuracy by 6 mm compared with the default configuration.The paper attributes this change to the module’s compensation for illumination changes.
  • Ablation results: Removing reference-patch updates decreases average accuracy by 44 mm compared with the default configuration.The update strategy selects higher-resolution patches and avoids outlier patches.
  • Ablation results: Normal refinement improves average accuracy by 1 mm, but gains are inconsistent and mainly occur in simple structured scenes with good image observations.

C. LiDAR Degenerated and Visually Challenging Environments

FAST-LIVO2 is tested in LiDAR-degenerated, visually challenging, mapping, and onboard-application scenarios. The reported results emphasize robustness during prolonged degeneration, accurate colored mapping, efficient processing, and real-time autonomous UAV integration.

  • LiDAR-degenerated environments: FAST-LIVO2 maintains robust, high-precision colored mapping during long-term LiDAR degeneration, including an approximately 800-meter wall-facing “HIT Graffiti Wall” sequence.FAST-LIVO produced blurred textures, while R3LIVE showed degraded geometry and texture clarity.
  • Visually challenging environments: In “HKU Cultural Center”, FAST-LIVO2 avoids the distorted maps, blurred textures, and drifts exceeding 1 m reported for FAST-LIVO and R3LIVE.
  • High-precision mapping: FAST-LIVO2 achieves an end-to-end mapping error of less than 0.01 m in evaluated structured, richly textured scenes.The comparison includes FAST-LIO2, R3LIVE, and FAST-LIVO on “SYSU 01”, “HKU Landmark”, and “CBD Building 01”.
  • Computational efficiency: Inverse compositional sparse alignment and plane-prior affine warping reduce pyramid-level iterations from 10 to 3 while retaining superior accuracy over FAST-LIVO.
  • Applications: The applications include autonomous UAV navigation, airborne mapping, textured mesh generation, and 3D Gaussian splatting reconstruction.
  • Applications: FAST-LIVO2 supports onboard autonomous UAV flights by providing localization at 10 Hz and dense registered point clouds to onboard planning and control.The planner, MPC, and FAST-LIVO2 operate in real time on the onboard computer.

2) UAV Autonomous Navigation:

FAST-LIVO2 supports real-time UAV navigation, airborne mapping, and downstream 3D rendering from dense colored point maps. Experiments demonstrate robust operation during LiDAR degeneration, obstacle avoidance, and severe illumination changes while remaining computationally efficient.

  • UAV Autonomous Navigation: Four onboard UAV experiments demonstrate autonomous navigation, obstacle avoidance, and stable localization during manual flights with limited LiDAR measurements.“Basement” and “Woods” run planning, MPC, and FAST-LIVO2 autonomously; “Narrow Opening” and “SYSU Campus” run MPC and FAST-LIVO2 without planning.
  • UAV Autonomous Navigation: 53.47 ms average processing time per LiDAR scan and image frame remains below the 100 ms frame period during concurrent onboard control and planning.MPC runs at 100 Hz and planning at 10 Hz, constraining resources available to FAST-LIVO2.
  • Airborne Mapping: 0.64 m and 0.27 m APE (RMSE) are achieved on “HKisland01” and “HKairport01”, compared with 2.76 m and 0.52 m for R3LIVE.The airborne maps capture fine structures and sharp coloring in forests, islands, buildings, roads, curbs, tree crowns, and rocks.
  • Supporting 3D Scene Applications: FAST-LIVO2 provides dense colored point maps that support meshing, texturing, and 3D Gaussian Splatting rendering applications.The paper demonstrates mesh generation for “CBD Building 01” and texture mapping for “CBD Building 01” and “Retail Street”.
  • Supporting 3D Scene Applications: 21 s versus 9 hours reduces the time to obtain dense point clouds and poses compared with COLMAP, while PSNR is slightly higher for FAST-LIVO2 inputs.Training time increases from 10 min and 59 s to 15 min and 30 s because denser point clouds introduce more parameters.
  • Conclusion: FAST-LIVO2 achieves fast, accurate, and robust state estimation while reconstructing maps online and tolerating severe LiDAR and visual degeneration.The authors identify sequential ESIKF updates, raw measurements, plane priors, and a unified voxel map as key design elements.
  • Conclusion: Long-distance drift remains a scope boundary for FAST-LIVO2 as an odometry system.The authors propose integrating loop closure and sliding-window optimization in future work.

I. SYSTEM MODULE VALIDATION

Module evaluations validate FAST-LIVO2’s plane-based warping, reference-patch updates, raycasting, exposure estimation, and sequential ESIKF update. The experiments show improved alignment, robustness in sparse-measurement conditions, and a favorable accuracy–efficiency trade-off.

  • Affine Warping and Normal Refinement: Plane priors and refined plane normals are evaluated against constant-depth warping on “CBD Building 02” and “Office Building Wall”.The comparison uses mapping results and drift metrics for “Constant depth”, “Plane prior”, and “Plane normal refined”.
  • Affine Warping and Normal Refinement: Plane-prior warping produces projection effects that are evaluated through reconstructed images formed by projecting reference patches onto current frames.Good affine warping and pose estimation should yield aligned projected patches and the expected RGB appearance.
  • Reference Patch Update: Reference-patch experiments cover structured and unstructured regions using multiple 40 × 40 observations, while normal convergence is assessed from LiDAR-initialized normals.Convergence curves measure angle changes between initial and optimized normals across iterations.
  • On-Demand Raycasting: On-demand raycasting is tested in a 1.9 m-wide tunnel where close-proximity blind zones leave current and recent LiDAR scans with few or no points.The evaluation targets localization robustness when the sensor faces a weakly textured wall during a turn.
  • Exposure Time Estimation: Estimated exposure time follows ground-truth values closely across sequences with fixed, auto-exposure, and auto-gain settings.Occasional mismatches are attributed to an unmodeled response function and vignetting factor.
  • ESIKF Sequential Update: “Synchronous (sequential update)” produces accurate mountain-road reconstruction without layering, unlike the other evaluated update strategies.The asynchronous standard update has the lowest accuracy, while the synchronous standard update is the most time-consuming.
  • ESIKF Sequential Update: 0.68 m APE (RMSE) and 23.1 ms processing time are achieved by “synchronous (sequential update)”, versus 3.12 m and 27.6 ms asynchronously.The synchronous standard update reaches 2.45 m and 49.9 ms; its higher cost comes from fusing all LiDAR measurements at each image-pyramid level.

II. ADDITIONAL INFORMATION

Additional materials document the private and public evaluation datasets, navigation modes, challenging environmental conditions, and conventions used to interpret reported results. These include lighting changes, degeneration, aggressive motion, and large-scale scenes.

  • Dataset and Experiment Information: The FAST-LIVO2 private dataset is summarized in Table S1, while Table S2 summarizes UAV autonomous navigation experiments.The supplied materials identify both tables but do not provide their row-level contents.
  • Dataset and Experiment Information: Evaluation sequences include looped trajectories, camera exposure ground truth, LiDAR blind-zone cases, indoor–outdoor transitions, and significant lighting variations.These categories are defined in the supplementary sequence notes.
  • UAV Experiment Modes: Autonomous mode runs FAST-LIVO2, planning, and MPC, whereas manual mode runs FAST-LIVO2 and MPC with planning disabled.This distinction applies to the UAV navigation experiments.
  • Timing Comparisons: Table S3 reports average processing time per frame for FAST-LIO2 across different datasets.The supplied table entry provides the table’s purpose but not its numerical values.
  • Result Conventions: A multiplication sign denotes that a system totally failed in the supplementary result tables.This convention is defined explicitly in the supplementary notes.
Loading 2408.14035v2…