FAST-LIO2: Fast Direct LiDAR-Inertial Odometry

Wei Xu, Yixi Cai, Dongjiao He, Jiarong Lin, Fu Zhang · The University of Hong Kong · 2022

Summary

A tightly-coupled LiDAR-inertial odometry system with incremental kd-tree mapping, enabling real-time state estimation for UAVs and mobile robots.

Abstract Summary

FAST-LIO2 is a highly efficient direct LiDAR-inertial odometry framework built on top of ikd-Tree, a novel incremental kd-tree data structure. It addresses the fundamental challenge of real-time 3D state estimation in dynamic environments by tightly fusing LiDAR point clouds and IMU measurements through an iterated Kalman filter. The system is designed to operate onboard UAVs and ground robots, providing centimeter-level localization accuracy at well above sensor frame rates. The key innovation of FAST-LIO2 lies in its incremental kd-tree mapping backend, which enables direct point cloud registration without requiring explicit feature extraction. By incrementally updating the map structure as new points arrive, the system avoids the costly global map- rebuilding bottleneck of traditional dense approaches. This makesFAST-LIO2 particularly well-suited for high-rate LiDAR sensors such as the Livox Mid-360, where point densities vary dramatically across the field of view. From a systems perspective, the codebase is lightweight, well-structured, and extensively tested on both aerial and legged platforms. The repository includes ROS wrappers, launch files, and configuration examples for several popular LiDAR models (Livox, Ouster, Velodyne). The ikd-Tree component is maintained as a standalone library, making it reusable in other geometric pipelines beyond odometry. Real-world benchmarks demonstrate that FAST-LIO2 achieves state-of-the-art accuracy while running at over 100 Hz on a portable computation unit. The broader impact is significant for autonomous navigation: accurate real-time odometry is the foundation of localization, mapping, and loop closure. FAST-LIO2 is widely adopted in the UAV research community and serves as the default odometry backend for several well-known mapping systems (such as R3LIVE and FAST-LIVO). Its efficiency and robustness make it an excellent starting point for researchers entering the LiDAR-inertial SLAM space.

Key Points

Tightly-coupled iterated Kalman filter fusing LiDAR and IMU at high frequency.
Novel incremental ikd-Tree enables fast direct point registration without feature extraction.
Runs at over 100 Hz onboard UAVs; tested on Livox, Ouster, and Velodyne sensors.
Open-source ROS package with modular C++ backend and standalone ikd-Tree library.
Widely adopted as odometry core in aerial, legged, and mobile manipulation platforms.

System Overview

The pipeline of FAST-LIO2 is shown in Fig. 1 . The sequentially sampled LiDAR raw points are first accumulated over a period between $\displaystyle 10ms$ (for $\displaystyle 100Hz$ update) and $\displaystyle 100ms$ (for $\displaystyle 10Hz$ update). The accumulated point cloud is called a scan.

In order to perform state estimation, points in a new scan are registered to map points (i.e., odometry ) maintained in a large local map via a tightly-coupled iterated Kalman filter framework (big dashed block in red, see Section. IV ).

System Overview — FAST-LIO2: Fast Direct LiDAR-Inertial Odometry — Fig. 1: System overview of FAST-LIO2.

State Estimation

The state estimation of FAST-LIO2 is a tightly-coupled iterated Kalman filter inherited from FAST-LIO [ 22 ] but further incorporates the online calibration of LiDAR-IMU extrinsic parameters. Here we briefly explain the essential formulations and workflow of the filter and refer readers to [ 22 ] for more details. We first derive the system model, which consists of a state transition model and a measurement model.

Mapping

In this section, we describe how to incrementally maintain a map (i.e., insertion and delete) and perform $\displaystyle k$-nearest search on it by ikd-Tree . In order to prove the time efficiency of ikd-Tree theoretically, a complete analysis of time complexity is provided. The map points are organized into an ikd-Tree , which dynamically grows by merging a new scan of point cloud at the odometry rate.

Benchmark Results

In this section, extensive experiments in terms of accuracy, robustness, and computational efficiency are conducted on various open datasets. We first evaluate our data structure, i.e., ikd-Tree , against other data structures for $\displaystyle k$NN search on 18 dataset sequences of different sizes. Then in Section. VI-C , we compare the accuracy and processing time of FAST-LIO2 on 19 sequences.

All the sequences are chosen from 5 different datasets collected by both solid-state LiDAR [ 15 ] and spinning LiDARs.

Benchmark Results — FAST-LIO2: Fast Direct LiDAR-Inertial Odometry — Fig. 7: Large-scale scene experiment.

Real-World Experiments

Besides the benchmark evaluation where the datasets are mainly collected on the ground, we also test our FAST-LIO2 in a variety of challenging data collected by other platforms (see Fig.

6 ), including a 280 $\displaystyle mm$ wheelbase quadrotor for the application of UAV navigation, a handheld platform for the application of mobile mapping, and a GPS-navigated 750 $\displaystyle mm$ wheelbase quadrotor UAV for the application of aerial mapping.

Real-World Experiments — FAST-LIO2: Fast Direct LiDAR-Inertial Odometry — Fig. 9: The flip experiment. (a) the small scale UAV; (b) the onboard camera showing first person view (FPV) images during the flip; (c) the third person view images of the UAV during the flip; (d) the estimated UAV pose with FAST-LIO2.

Sources and Demonstrations

Figures are reproduced from Xu et al. See the full paper for experimental details and the project page, when the paper links one, for demonstrations and videos.