FAST-LIO2: Direct LiDAR-Inertial Odometry Framework

• FAST-LIO2 is a direct LiDAR-inertial odometry framework that performs real-time raw point registration using an iterated Extended Kalman Filter for robust motion estimation.
• It leverages an efficient incremental ikd-Tree for dynamic local mapping, enabling fast point insertions, deletions, and re-balancing across diverse LiDAR sensor modalities.
• Extensive benchmarks on UAVs, handheld devices, and complex environments demonstrate its high accuracy, low latency, and adaptability in practical SLAM applications.

FAST-LIO2 is a direct LiDAR-inertial odometry framework characterized by tightly coupled, iterated extended Kalman filtering with raw point registration, and highly efficient incremental mapping based on the ikd-Tree. It is designed for fast and robust state estimation and mapping, eliminating hand-engineered feature extraction, optimizing computational pathways for point cloud registration, and providing broad adaptability to multiple LiDAR modalities and robotic platforms. Its rapid state update cycle, real-time performance on both x86 and ARM hardware, and open-source release have established it as a benchmark system for high-fidelity Simultaneous Localization and Mapping (SLAM) applications in both research and field robotics contexts.

1. Direct Raw Point Registration and Tightly Coupled Inertial Fusion

FAST-LIO2’s central innovation is the direct registration of all raw LiDAR points to the map without the explicit feature extraction step typical in prior approaches. Rather than reducing the input point cloud to edge or planar feature subsets, each scan’s raw points are motion-compensated (to account for within-scan motion distortion using high-rate inertial measurements) and immediately registered against the current map. This process exploits subtle structures in the acquired data, yielding higher mapping and localization accuracy, especially in feature-sparse or highly dynamic environments.

The fusion of LiDAR and Inertial Measurement Unit (IMU) data is performed in a tightly-coupled manner using an iterated Extended Kalman Filter (iEKF). The iEKF models nonlinear system evolution on the Lie group manifold and repeatedly linearizes the measurement model (residual between scan and map) to ensure robust convergence, especially in high-dynamic regimes or in the presence of measurement degeneracy. The error state $\delta x$ is defined on the manifold, and the filter’s update step alternates between forward IMU propagation and repeated measurement updates, aligning the state estimate with the most recent LiDAR scan.

For a LiDAR point $p_j$ (motion-compensated to the scan’s reference time and transformed via the current state estimate), the measurement residual is defined as the distance to its associated planar patch in the map, specifically

$$z_j^{\kappa} = {}^G u_j^T ({}^G\widehat{T}^{\kappa}_{I_k} {}^I\widehat{T}^{\kappa}_{L_k} {}^L p_j - {}^G q_j),$$

where ${}^G u_j$ is the map plane normal and ${}^G q_j$ the corresponding point on the plane (Xu et al., 2021).

2. Incremental Mapping via ikd-Tree

FAST-LIO2 utilizes an incremental k-d tree (ikd-Tree) for dynamic local map maintenance. Unlike traditional static trees, ikd-Tree supports rapid per-point insertion, deletion, on-tree downsampling, and dynamic re-balancing, all without global re-building. This approach allows:

• Real-time, incremental updates for sliding window maps during robot motion.
• O(log n) complexity for point insertion and nearest neighbor queries, supporting tens of thousands of map points per scan at rates exceeding 100 Hz on standard hardware and ~6 ms per scan update on embedded ARM platforms.
• Natural compatibility with a variety of LiDAR scanning patterns, including multi-beam spinning and non-repetitive field-of-view solid-state sensors.

The ikd-Tree’s performance is quantitatively superior to alternatives such as octree, R*-tree, and nanoflann-based k-d tree methods, as demonstrated by detailed timing analyses (often less than 2 ms per scan on x86, below 6 ms on ARM) (Xu et al., 2021). Downsampling and box-wise deletions (required for sliding maps) are supported natively.

3. Mathematical Formulation and Computational Efficiency

The iEKF update exploits a computationally efficient Kalman gain formulation. While the classical gain $K = P H^\top (H P H^\top + R)^{-1}$ is prohibitive when the measurement dimension (i.e., number of LiDAR points) is large, FAST-LIO2 instead uses

$K = (H^\top R^{-1} H + P^{-1})^{-1} H^\top R^{-1},$

which has complexity dictated by the state dimension (typically 18), not the measurement size. This leverages the matrix inversion lemma and directly aligns with the MAP solution for the nonlinear pose estimation problem, enabling real-time iteration even with over 1,000 point-to-plane measurements per scan (Xu et al., 2020).

The entire pipeline, including IMU propagation, point de-skewing, direct map registration, and incremental mapping, routinely achieves scan update times of 2 ms (mapping) and 25 ms (full iEKF iteration, including forward and iterative update) for over 1,200 points per scan. This allows update frequencies up to 100 Hz, supporting both high-speed UAV and handheld robotics deployments.

4. Experimental Validation and Benchmarks

FAST-LIO2 has undergone exhaustive benchmark testing on 19 sequences derived from open datasets (covering spinning and solid-state LiDAR sensors) as well as proprietary challenging scenarios. Reported results include:

• Consistent reduction in translational RMSE versus LIO-SAM, LILI-OM, and LINS, particularly at map sizes relevant to robotic applications (1,000–2,000 m).
• Robust performance on a 280 mm quadrotor (Livox Avia LiDAR) during aggressive flips (angular velocities up to 1,200°/s), producing dense, drift-free 3D maps.
• Handheld mapping at speeds up to 7 m/s along 81 m footbridges (drift less than 0.06 m).
• Outdoor UAV mapping in complex environments (Hong Kong Wetland Park), with fine-structure detail consistent with ground truth GPS/IMU.
• Real-time operation verified on x86 (Intel) and ARM-based onboard computers, with the software stack (ROS + C++) open-sourced for direct benchmarking (Xu et al., 2021).

5. Applications and Adaptability

FAST-LIO2’s direct point registration model and general-purpose incremental map are naturally suited for:

• Autonomous UAV navigation in GPS-denied, cluttered, or low-feature environments.
• High-precision 3D mapping from mobile robots or handheld mappers, including urban and vegetated scenes.
• Real-time environment reconstruction for obstacle avoidance, path planning, and situational awareness in micro aerial vehicles and robots.
• Immediate adaptation to both established spinning LiDAR architectures and new solid-state, limited-FoV sensors due to the absence of hand-crafted, sensor-specific feature extraction routines.
• Cross-platform (x86/ARM) deployment for both high-end and resource-constrained robots.

6. Open Source Impact and Derivative Advancements

By releasing both the core FAST-LIO2 framework and the ikd-Tree mapping library as open-source projects, the authors have enabled reproducibility and expedite downstream research (Xu et al., 2021). Derived systems have incorporated FAST-LIO2 as a frontend for Bayesian ICP PoseSLAM and graph-based SLAM optimization, and served as the baseline for hybrid localization/mapping systems in ground vehicle inspection tasks and for evaluation against multi-sensor invariant filtering schemes (Chen et al., 2022, Alamanos et al., 28 Mar 2024, Nisticò et al., 29 Apr 2025). The adoption of direct point-cloud registration, fast incremental data structures, and efficient iEKF computation has influenced subsequent LiDAR-inertial(-visual) odometry systems.

7. Current Trends and Research Extensions

FAST-LIO2’s core principles have inspired further research in:

• Degeneration-aware adaptive sensor fusion, where visual and other exteroceptive cues are selectively incorporated according to the strength of local geometric constraints (Zhou et al., 23 Jan 2025).
• Hybrid mapping with hierarchical or memory-efficient map maintenance for large-scale or long-term operations (Zhou et al., 23 Jan 2025).
• Extension to multi-sensor scenarios, including visual and GPS-aided odometry in challenging terrains; group-affine filtering and smoothing frameworks for proprioceptive/exteroceptive integration (Nisticò et al., 29 Apr 2025).
• Structural advances in mapping (plane pre-fitting, skeleton tracking, voxelization, etc.) for further acceleration and real-time DLO-SLAM (Chen et al., 2023, Liu et al., 2023).
• Support for tightly coupled, direct LiDAR-inertial-visual odometry in unified voxel and patch-based maps (Zheng et al., 26 Aug 2024).

FAST-LIO2 thus stands as a foundational framework for robust, high-speed, feature-agnostic LiDAR-inertial odometry and mapping, integrating innovation in algorithm design, computational efficiency, and open research practice.