FAST-LIVO: Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry (IROS 2022)

Created on June 17, 2026

2026   ·   lio   slam   lidar-visual   ·   paper-review

Item Details
Authors Chunran Zheng, Qingyan Zhu, Wei Xu, Xiyuan Liu, Qizhi Guo, Fu Zhang (HKU MaRS Lab, The University of Hong Kong)
Venue IROS 2022 (arXiv:2203.00893)
Links arXiv · Code

One-line Summary

FAST-LIVO is a LiDAR-Inertial-Visual Odometry that tightly couples LiDAR, IMU, and camera at the measurement level inside a single error-state iterated Kalman filter (ESIKF). Both the LiDAR and visual modules are direct methods that extract no features; in particular, image patches are attached to the points of the point-cloud map built by the LIO and reused for visual registration. As a result, separate visual feature extraction, triangulation, and the sliding-window backend disappear entirely, and the system achieves higher accuracy than the feature-based R2LIVE with less computation.

FAST-LIVO sits at the center of the “direct + iterated Kalman filter” lineage of HKU-MARS (Fu Zhang’s group). FAST-LIO (2021) established the prototype of tightly-coupled IEKF-based LIO, and FAST-LIO2 (2022) introduced feature-free direct point-to-plane registration and the ikd-Tree incremental map. FAST-LIVO (2022, this paper) adopts the LIO subsystem of FAST-LIO2 almost unchanged and stacks a direct VIO subsystem on top of it. In other words, it extends the FAST-LIO2 philosophy of “direct LiDAR registration” into the visual domain, and this line later continues into FAST-LIVO2 (T-RO 2025).

The contemporary competing LIVO lineage is as follows.

  • R2LIVE (HKU, 2021): from the same HKU-MARS family but feature-based. It extracts LiDAR features and image features, fuses them in an IEKF via re-projection error, and keeps a sliding-window optimization backend on the VIO side. FAST-LIVO is the direct counterpart that replaces R2LIVE’s “feature extraction + backend” with a direct approach.
  • R3LIVE (HKU, 2022): the successor of R2LIVE. It separates the geometric (LIO) map from the radiance (RGB) map to build RGB-colored maps, and is HKU’s other LIVO line from the same period as FAST-LIVO.
  • LVI-SAM (MIT, 2021): couples LIO-SAM and VINS-Mono through factor-graph smoothing and includes loop closure. Being feature-based and optimization-based, its computational burden is high.

Clarifying the relation to the successor FAST-LIVO2 in advance makes this paper’s position clearer. FAST-LIVO maintains the LiDAR map (ikd-Tree) and the visual map (voxel hash) separately, and uses a single update that stacks the LiDAR residuals and photometric residuals together. FAST-LIVO2 stores LiDAR geometry and image patches together in one unified voxel map, resolves the dimensional mismatch between LiDAR (high-dimensional) and image (low-dimensional) measurements with sequential updates, injects a LiDAR plane prior into image alignment, and adds real-time exposure-time estimation and on-demand voxel raycasting. In one line: FAST-LIVO is “two maps + simultaneous fusion”, while FAST-LIVO2 is “unified voxel map + sequential update + plane prior + exposure estimation”.

Problem and Motivation

Single-sensor SLAM collapses depending on the environment. Camera-only systems degenerate in front of texture-less white walls, and LiDAR-only systems degenerate in structure-less long corridors or tunnels, making robust pose estimation difficult. There is no disagreement that multi-sensor fusion is the answer.

The problem is how existing LIVO systems fuse. R2LIVE and LVI-SAM keep LIO and VIO as two subsystems that share the state vector but process each sensor’s data separately. There is no coupling at the measurement level, and because feature extraction and sliding-window optimization are attached on the visual side, the computational load is high.

The goal of FAST-LIVO is to combine sparse direct image alignment (the SVO approach) with direct raw-point registration (the FAST-LIO2 approach) to build a tightly-coupled LIVO that raises accuracy and robustness while reducing computation, without feature extraction or a backend.

Key Ideas and Method

Overall skeleton: two direct subsystems, one ESIKF

The LIO and VIO share the same 18-dimensional state (attitude in SO(3), position, velocity, gyroscope bias, accelerometer bias, gravity). The IMU produces a prior through forward propagation; when a LiDAR scan arrives the same Kalman filter is updated with LiDAR residuals, and when an image arrives it is updated with photometric residuals. Simultaneous arrival of LiDAR and image is not assumed; updates are performed asynchronously as each arrives.

State estimation is an on-manifold ESIKF. Gauss-Newton iterations are carried out on the \(SO(3)\times\mathbb{R}^{15}\) manifold using the \(\boxplus\)/\(\boxminus\) operators, and the classical result that the iterated Kalman filter is equivalent to Gauss-Newton (Bell & Cathey, 1993) is the basis for solving the MAP optimization as a filter.

LIO subsystem: direct point-to-plane from FAST-LIO2

No edge/plane features are extracted; raw points are used as they are. After compensating in-scan motion distortion by backward propagation, each point is projected into the global frame with the predicted pose, a plane (normal \(\mathbf{u}\), center \(\mathbf{q}\)) is fitted to the five nearest points in the ikd-Tree, and a point-to-plane residual is formed.

\[r_l = \mathbf{u}^\top(\mathbf{T}\,\mathbf{p} - \mathbf{q})\]

The ikd-Tree handles incremental insertion, deletion, downsampling, and rebalancing of the map.

VIO subsystem: sparse-direct visual alignment

This is the core of the paper. As an analogy, small photo fragments (8×8 patches) saying “this point looked like this in a previous picture” are attached like stickers to the 3D points that the LIO has already built. When a new image arrives, those points are re-projected into the image with the current pose, and the pose is fine-tuned so that the attached stickers overlap best, in brightness (photometric) terms, with the corresponding locations in the current picture.

In equations, the photometric residual between the reference patch \(Q\) and the current image \(I_k\) is minimized. \(A\) is an affine illumination correction.

\[r_c = I_k\big(\pi(\mathbf{T}\,\mathbf{p})\big) - A\,Q\]

It is important that no visual features are extracted at all. Because LIO map points are reused, triangulation, feature alignment, depth filters, and the sliding window are all unnecessary. A three-level coarse-to-fine pyramid is used so that the alignment converges even under large motion, and when several patches from different viewing angles are attached to one point, the patch whose viewing angle is closest to the current viewpoint is chosen as the reference.

Outlier rejection: filtering occluded and boundary points

Points that are occluded in the current frame or that lie on depth discontinuities (edge boundaries) get mixed into the visual submap and break the VIO. They are filtered in two stages.

  1. Submap points are projected into the current frame, and only the nearest (lowest-depth) point is kept per 40×40-pixel grid cell.
  2. The latest LiDAR scan is projected, and any map point that is occluded by a point in front of it within a 9×9 neighborhood is rejected as occluded.

When adding new points, edge points with high local curvature are also excluded from the visual map. This mechanism is the practical key to stabilizing the direct VIO, but the paper offers only qualitative figures and no ablation.

Map management: two maps

The LiDAR global map is the ikd-Tree of FAST-LIO2 unchanged. The visual global map stores the LiDAR points in axis-aligned voxels with a hash table, and each point carries patch pyramids from several reference images together with the camera poses at those times. To quickly find points inside the current FoV, voxels are looked up using the points of the latest LiDAR scan, which relies on the assumption that the camera and LiDAR FoVs are roughly aligned. Patches are updated only for points that are more than 20 frames old or have moved more than 40 pixels and have a large photometric error, and new points are added by picking the point with the maximum gradient in each 40×40 cell to preserve texture information.

Experiments and Results

The hardware consisted of a DJI Manifold-2c (Intel i7-8550u, 8GB), two industrial cameras (MV-CA013-21UC), and a Livox Avia solid-state LiDAR, with 10Hz hardware synchronization via an STM32 timer. Both multi-line spinning LiDAR (Ouster OS1) and solid-state LiDAR (Avia) are supported. The benchmark is the nine NTU-VIRAL sequences (Ouster OS1-16 + built-in IMU), and the baselines are R2LIVE (feature-based LIVO), FAST-LIO2 (direct LIO), SVO2.0 (semi-direct VIO), and DVL-SLAM (direct LiDAR-visual, loop closure removed). The same parameters were used for all sequences.

The three key results are as follows.

  1. Lowest RMSE on 8 of the 9 NTU-VIRAL sequences. The only exception, sbs_01, is marginally behind FAST-LIO2. This was a case where the high-speed UAV produced heavily blurred images, so fusing low-quality images was counterproductive.
  2. Computation is overwhelmingly lower than R2LIVE. Per frame, R2LIVE takes about 104ms (front-end 45.16ms + VIO backend 59.27ms), whereas FAST-LIVO takes 36.75ms in total. This is the effect of removing the backend entirely.
  3. Demonstrated robustness to degeneration. In the 30m white-wall experiment (LiDAR-degenerate), FAST-LIO2 and SVO2.0 drift, but FAST-LIVO has an end-to-end drift of 0.05m. On a 79.52m route mixing indoor and outdoor scenes, two aggressive motions, and a texture-less white wall, the return error is 0.04m.

Looking at the detailed numbers, FAST-LIVO’s NTU-VIRAL ATE RMSE (m) is mostly in the 0.17 to 0.29 range, e.g. eee01 0.28, eee02 0.17, nya01 0.19. For comparison, FAST-LIO2 is 0.22 to 0.54, R2LIVE 0.21 to 0.97, SVO2.0 fails on several sequences, and DVL-SLAM is 1.08 to 3.08.

The time breakdown (ms, Intel i7 / ARM RB5 Kryo585) is as follows.

Module Intel i7 ARM Kryo585
Visual submap 1.63 3.81
Outlier rejection 2.07 2.58
Sparse-direct alignment 3.19 3.88
ESIKF update 0.42 0.52
Visual map update 2.92 3.03
VIO total 10.23 13.82
LIO subsystem 26.52 51.51

There is real-time headroom even on ARM. As a side result, real-time reconstruction of an RGB-colored dense point cloud of the HKU campus was also shown.

Checking the claimed contributions against what was actually delivered: the “compact dual-direct tightly-coupled LIVO framework” was indeed implemented, but since the LIO is adopted almost unchanged from FAST-LIO2, the center of novelty lies on the VIO side. The “direct VIO reusing LIO map points” is the most essential and substantive contribution, and the numbers confirm that measurement-level coupling and backend removal are the source of the computational savings. Real-time operation on Intel/ARM and simultaneous support for spinning/solid-state LiDAR are also backed by experiments. However, for NTU-VIRAL only the sequences with good ground truth were reported.

Conclusion and Significance

FAST-LIVO combines direct LiDAR registration (FAST-LIO2) and sparse-direct visual alignment (SVO) at the measurement level in a single ESIKF, breaking the conventional wisdom that “features and an optimization backend are needed to raise accuracy”. By showing that it beats the feature-based SOTA (R2LIVE) in both accuracy and speed without a backend, it pulled the LIVO design paradigm toward “direct + map reuse”, and became an intermediate milestone in the practical SLAM line running from FAST-LIO2 to FAST-LIVO2. Its RGB-colored dense mapping later became the data foundation for the LiDAR-visual neural reconstruction trend (M2Mapping and others).

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

  • Where it can be used: at about 37ms per frame on an Intel i7 with CPU only, and real-time even on ARM, it suits small onboard platforms without a GPU (UAVs, small ground robots). Since it showed cm-level return errors in indoor environments where either LiDAR or camera degenerates, such as white walls and long corridors, it is worth considering as an odometry candidate for autonomous mobile-robot navigation in multi-floor indoor buildings.
  • What it presupposes: hardware-synchronized LiDAR-IMU-camera and precise extrinsic calibration are mandatory. Online time/extrinsic calibration is not supported, so synchronization and calibration are the biggest hurdles when running it on one’s own sensors. The synchronization CAD and STM32 firmware are provided in the authors’ LIV_handhold repository.
  • Limitation 1, no loop closure: being pure odometry, accumulated drift remains on large-scale routes. Unlike LVI-SAM, there is no global optimization.
  • Limitation 2, adverse effect of low-quality images: under strong blur (high-speed UAV), visual fusion can actually lower accuracy, performing worse than LIO-only (sbs_01). There is no explicit handling of exposure or blur; this was later addressed by the exposure-time estimation in FAST-LIVO2.
  • Limitation 3, structural assumptions: the assumption that the camera FoV lies within the LiDAR FoV, the maintenance of two separate maps (ikd-Tree + visual voxel), and the lack of ablation for outlier rejection and the patch update policy remain. The license is GPLv2, free for academic use but requiring separate contact with the authors for commercial use, and the dependencies (Ubuntu 16.04 to 20.04, ROS1, the non-templated version of Sophus, Vikit) raise the build difficulty. Reproduction difficulty is moderate.

References

  • Paper: arXiv:2203.00893
  • Code: hku-mars/FAST-LIVO (GPLv2)
  • FAST-LIO2: Fast Direct LiDAR-Inertial Odometry (T-RO 2022) — direct source of the LIO subsystem
  • SVO: Fast Semi-Direct Monocular Visual Odometry (ICRA 2014) — origin of sparse direct image alignment
  • R2LIVE (2021) — feature-based tightly-coupled LIVO, the main comparison target
  • DVL-SLAM (ICRA 2018 / AuRo 2020) — the closest prior work, tracking by projecting LiDAR points into images with photometric error
  • NTU-VIRAL (IJRR 2021) — quantitative evaluation benchmark