FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry (T-RO 2025)

Created on June 16, 2026

2026   ·   lio   slam   lidar-visual   ·   paper-review

Item Details
Authors 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 (HKU MaRS Lab, The University of Hong Kong)
Venue IEEE Transactions on Robotics (T-RO), vol. 41, pp. 326-346, 2025 (arXiv:2408.14035, 2024)
Links arXiv · IEEE Xplore · Code

One-line Summary

FAST-LIVO2 is a LIVO system that couples LiDAR, IMU, and camera in a single sequential-update ESIKF, processes both LiDAR and vision in a direct manner, and reuses the planes built by LiDAR (plane priors) for visual registration on top of a single unified voxel map. On 25 public benchmark sequences it set a new SOTA with an average RMSE of 0.044m (about 3 times more accurate than the runner-up) at 30ms per frame (Intel i7). It is the successor of FAST-LIVO from HKU-MARS, and directly fixes five weaknesses of its predecessor.

FAST-LIVO2 is the culmination of the “direct + iterated Kalman filter” line of HKU-MARS (Fu Zhang’s group).

  • FAST-LIO2 (T-RO 2022): direct LIO using only LiDAR and IMU. Raw points are registered directly to an ikd-Tree map without feature extraction. It is the prototype of the FAST-LIVO2 LIO subsystem, but this paper adopts VoxelMap’s hash+octree adaptive voxels instead of the ikd-Tree.
  • R2LIVE (RA-L 2021) → R3LIVE (ICRA 2022) → R3LIVE++ (2022): LIVO from the same group. R3LIVE builds geometry with LIO and paints texture (color) with VIO, and the two subsystems each fuse with the IMU to jointly estimate the state. Its two-stage structure of per-pixel dense direct VIO and frame-to-frame optical flow depends heavily on the initial value and is computationally expensive. FAST-LIVO2 simplifies and refines this into patch-level sparse direct, single-stage frame-to-map alignment.
  • FAST-LIVO (IROS 2022): the immediate predecessor. It first proposed sparse-direct LIVO but had five weaknesses: asynchronous updates of LiDAR and vision, the unreasonable assumption that a whole patch shares the same depth, weak constraints from choosing the patch closest to the current viewpoint as the reference, no exposure-time compensation, and no consideration of the LiDAR close-range blind zone.
  • LVI-SAM (ICRA 2021): factor-graph based feature-based LIVO. It is a baseline in this paper, and failed on 9 sequences because features cannot be found when structure and texture are weak.

Problem and Motivation

Single-sensor SLAM has its own limits. Vision cannot observe depth directly and is vulnerable to illumination and lack of texture, while LiDAR has no color and degenerates in geometry-poor environments such as tunnels or single walls. Fusion is the answer, but LIVO itself is hard.

  1. Hundreds of thousands of LiDAR points per second and high-resolution images must be processed in real time with onboard resources.
  2. LIO and VIO subsystems usually extract features separately, so features run dry in structure-less and texture-less environments and separate engineering is required for each scanning pattern.
  3. Designing a unified map that holds heterogeneous measurements together is tricky.
  4. A clean colored point cloud requires pixel-level pose accuracy, which demands hardware synchronization, precise extrinsic calibration, and recovery of exposure time.

Key Ideas and Method

Scan recombination: keeping the beat

LiDAR raw points streaming in at 100 to 500kHz are cut at the camera sampling instants (e.g. 10Hz) and recombined into one scan. This makes LiDAR and camera update at the same instant and the same frequency. IMU forward propagation predicts the state and backward propagation compensates motion distortion.

Sequential-update ESIKF: resolving the dimensional mismatch

LiDAR residuals (point-to-plane) and image residuals (photometric) have different dimensions, and the image is updated per pyramid level. Instead of stacking them all at once as in a standard ESIKF, the IMU prior is first updated with LiDAR until convergence, and that posterior is then used as the prior for the subsequent image update. Under the assumption that the two measurement noises are independent, this is theoretically equivalent to a joint update.

As an analogy, rather than receiving two testimonies at once and combining them, one first listens to the accurate testimony (LiDAR) and then hears the second testimony (image) based on that conclusion. This replaces the unstable asynchronous update of FAST-LIVO.

Single unified voxel map: one map, two uses

The structure of VoxelMap is adopted. A hash table manages 0.5×0.5×0.5m root voxels, and each voxel is subdivided by an octree up to three levels, so that each leaf voxel holds one local plane (center, normal, uncertainty) and the raw points on it. LiDAR builds the geometry of this map, and the visual module attaches image patches (8×8, three-level pyramid) to some of those points to reuse them as “visual map points”. Memory is kept bounded with a ring buffer that retains only the vicinity of the current position.

Because LiDAR points are used as they are, without separate triangulation of visual features or running a depth filter, backend computation is greatly reduced.

Direct LiDAR registration + beam-divergence noise model

Without extracting edge/plane features, raw points are registered directly to the map via point-to-plane residuals. In addition, point uncertainty is modeled taking the laser beam divergence angle into account. The larger the angle between the bearing and the plane normal (the more obliquely the floor or wall is viewed), the larger the ranging uncertainty. Thanks to this precise noise model, the LIO alone already surpasses FAST-LIO2.

Sparse-direct visual registration + plane prior

The selected visual map points are projected into the current image, and the pose is aligned by minimizing the photometric error (direct brightness comparison) against the reference patches. This is the sparse direct approach of the SVO family. When deforming a patch from one viewpoint to another (affine warping), instead of assuming that “the whole patch has the same depth” as in FAST-LIVO, the plane normal given by LiDAR is used directly for exact warping.

For efficiency, the inverse compositional formulation is used. Since the reference-side coordinates do not change between iterations, the Jacobian needs to be computed only once. There is also a normal-refine option that further refines the normals with photometric error in a separate thread, but it is OFF by default, because the gain is as small as 1mm and it can be counterproductive in dark or blurry images.

Dynamic reference-patch update: choosing a good reference

One map point can have several patches. They are scored by NCC (degree of mutual similarity) and the orthogonality between the normal and the line of sight (frontality), and the highest-scoring patch is chosen as the reference. This selects a patch that looks similar to the other patches (avoiding dynamic objects and outliers) while viewing the plane head-on (preserving high-resolution texture). FAST-LIVO simply picked the patch closest to the current viewpoint, giving weak constraints. In the ablation, this module’s accuracy contribution of 44mm is the largest among all modules.

On-demand voxel raycasting: filling LiDAR gaps

Normally, the voxels hit by the current scan are looked up by hash to find the visual map points in view. However, LiDAR sometimes returns no points on objects that are too close (close-proximity blind zone) or does not cover the whole camera FoV. In that case the image is divided into a 30×30 grid, and for empty cells only, a backward ray is cast along the central pixel (sample point coordinates are precomputed) and map points are recovered by walking along the depth. In effect, corners of the image that LiDAR could not see are probed and filled with rays.

Real-time exposure-time estimation

The inverse exposure time \(\tau\) (relative to the first frame) is included in the state as a random walk and estimated jointly in the photometric equation. \(\tau_0 = 1\) is fixed to prevent degeneration to the trivial solution (all zeros). This stabilizes image alignment convergence under abrupt over-exposure and low-light changes such as moving from indoors to outdoors, and prevents over-exposure in the colored point cloud.

Outlier rejection

Among the map points collected by raycasting and voxel lookup, those with occlusion, depth discontinuity, or an excessive viewing angle (over 80°) are removed. A depth map is built from the current scan and compared within a 9×9 neighborhood to filter occluded points.

Experiments and Results

The implementation is in C++/ROS and is optimized for both Intel and ARM. It supports spinning multi-line and solid-state (non-repetitive scan) LiDAR, and pinhole and various fisheye cameras (Pinhole/MEI/ATAN/Scaramuzza/Equidistant projection models). The default configuration is exposure estimation ON, normal refine OFF, LiDAR temporal downsampling 1:3, root voxel 0.5m, three octree levels, and 8×8 registration patches.

The evaluation data are 25 public sequences (NTU-VIRAL, Hilti’22, Hilti’23), the authors’ own FAST-LIVO2 private dataset (20 sequences, 66.9 minutes, extreme environments such as single walls, tunnels, and abrupt light/dark changes), and MARS-LVIG (high-altitude aerial) for applications. Since Hilti ground truth is not public, scoring was done through the official site. The baselines are R3LIVE (dense direct LIVO), FAST-LIO2 (direct LIO), SDV-LOAM (semi-direct LV), LVI-SAM (feature-based LIVO), and FAST-LIVO (the predecessor), and ablations were run on exposure, normal refine, reference patch, raycasting, and sequential update. The computing platforms are a desktop i7-10700K/32GB and an ARM Qualcomm Kryo585/8GB (RB5).

The three key results are as follows.

  1. Accuracy SOTA. Average APE RMSE of 0.044m over the 25 public sequences (0.044 with normal refine ON, 0.045 by default). About 3 times more accurate than the runner-up FAST-LIVO at 0.137m. R3LIVE 0.278m, FAST-LIO2 0.151m, LVI-SAM 1.928m (9 failures), SDV-LOAM 7.416m.
  2. Overwhelming efficiency. Average 30.03ms per frame (LiDAR 17.13 + image 12.90, i7), i.e. real time at 10Hz. Far ahead of R3LIVE 108.36ms, LVI-SAM 108.45ms, and FAST-LIVO 41.43ms. Real time on ARM as well at 78.44ms. The inverse compositional formulation and the plane prior reduced the iterations per pyramid level from 10 to 3.
  3. Robustness under extreme degeneration. In “HIT Graffiti Wall” where LiDAR sees only a wall for about 800m, “CBD Building 03” where LiDAR and camera degenerate simultaneously, and the dark “Mining Tunnel”, it returned to the start point with a start-to-end error under 0.01m. R3LIVE and FAST-LIVO drifted by more than 1m or failed completely.
Comparison item FAST-LIVO2 FAST-LIVO R3LIVE FAST-LIO2 LVI-SAM
Average APE RMSE (m, 25 public sequences) 0.044 0.137 0.278 0.151 1.928 (9 failures)
Processing time per frame (ms, i7) 30.03 41.43 108.36 - 108.45

In the ablation, removing the reference-patch update worsens accuracy by 44mm, making it the largest contributing module; removing exposure estimation worsens it by 6mm; and normal refine gives only a 1mm improvement and is unstable. Aerial mapping APE is 0.64m on HKairport01 and 0.27m on HKisland01 (R3LIVE 2.76, 0.52m), with processing times of about 25ms and 22ms (R3LIVE about 110, 100ms).

Three applications were also shown: fully onboard autonomous UAV flight (localization at 10Hz + Bubble planner + on-manifold MPC running simultaneously on one NUC, 80.4ms total), aerial mapping (MARS-LVIG), and 3D rendering (mesh/texture via VDBFusion+OpenMVS, 3D Gaussian Splatting). Used as 3DGS input, it cut COLMAP’s 9 hours of point-cloud and pose generation to 21 seconds (training time increased from 11 to 15.5 minutes due to the dense point cloud, and PSNR was slightly higher).

There are five claimed contributions, each verified by ablation, so the claims match the measurements well: the sequential-update ESIKF, use of LiDAR plane priors, the reference-patch update strategy (largest contribution), real-time exposure-time estimation, and on-demand voxel raycasting. Honestly reporting the marginal effect of normal refine and leaving it OFF by default signals the absence of exaggeration. It should be borne in mind, however, that part of the accuracy advantage comes from the authors themselves adapting the baseline R3LIVE to fisheye cameras and an external IMU.

Conclusion and Significance

FAST-LIVO2 is the de facto reference LIVO that raises accuracy, robustness, and efficiency simultaneously through direct methods, a unified voxel map, and a sequential ESIKF. The core idea is “reuse LiDAR points as visual map points and use their plane normals directly in affine warping”, and by eliminating the separate visual backend (triangulation, sliding window, depth filter) entirely it achieves onboard real-time performance. Because the colored point cloud is of high quality, it also serves well as input for mesh/texture/3DGS, acting as a bridge between SLAM and 3D reconstruction.

From a robotics practice perspective, the takeaways are as follows.

  • Where it can be used: about 30ms/frame on an Intel i7 class CPU and about 78ms on ARM (Kryo585 class), i.e. real time and embedded-friendly. On a UAV, it fits within 80ms even when the planner and MPC run on the same NUC. Since robustness was demonstrated in extreme environments such as tunnels, single walls, and abrupt light/dark changes, it is a strong odometry candidate for autonomous mobile-robot navigation in multi-floor indoor buildings or on unstructured outdoor terrain. The clean colored point cloud makes it a good input for 3DGS or mesh reconstruction pipelines.
  • Reproduction conditions: the code, datasets, and parameters are public, so benchmark reproduction is easy. Collecting one’s own data, however, requires hardware synchronization (STM32 trigger) and precise extrinsic calibration, and aligning the ROS1 and Sophus/Vikit build dependencies is an entry barrier. Reproduction difficulty is moderate, and the 3DGS/mesh applications require separate pipelines.
  • Limitation 1, long-range drift: with no loop closure or sliding window, it is pure odometry. The authors list this explicitly as future work.
  • Limitation 2, hardware dependence: accuracy and robustness depend heavily on hardware synchronization, precise extrinsics, and the exposure model. If synchronization or calibration is sloppy, the pixel-level advantage collapses.
  • Limitation 3, model assumptions: normal refine causes negative optimization in dark and blurry images and needs adaptive on/off; the sequential update relies on the assumption of independent measurement noise, so optimality in correlated-noise settings needs separate examination. Generalization to semantics and dynamic environments also remains unresolved. The license is GPLv2, so commercial use requires contacting the authors.

References

  • Paper: arXiv:2408.14035, IEEE Xplore (DOI 10.1109/TRO.2024.3502198)
  • Code: hku-mars/FAST-LIVO2 (GPLv2)
  • FAST-LIO2: Fast Direct LiDAR-Inertial Odometry (T-RO 2022) — root of the LIO subsystem
  • VoxelMap (RA-L 2022) — hash+octree adaptive voxels and probabilistic plane model, the foundation of the map structure
  • FAST-LIVO (IROS 2022) — the immediate predecessor
  • R3LIVE (ICRA 2022) — dense direct LIVO, the main baseline
  • SVO (ICRA 2014, T-RO 2016) — source of patch photometric registration and the inverse compositional formulation