FAST-LIO2: Fast Direct LiDAR-inertial Odometry

Abstract: This paper presents FAST-LIO2: a fast, robust, and versatile LiDAR-inertial odometry framework. Building on a highly efficient tightly-coupled iterated Kalman filter, FAST-LIO2 has two key novelties that allow fast, robust, and accurate LiDAR navigation (and mapping). The first one is directly registering raw points to the map (and subsequently update the map, i.e., mapping) without extracting features. This enables the exploitation of subtle features in the environment and hence increases the accuracy. The elimination of a hand-engineered feature extraction module also makes it naturally adaptable to emerging LiDARs of different scanning patterns; The second main novelty is maintaining a map by an incremental k-d tree data structure, ikd-Tree, that enables incremental updates (i.e., point insertion, delete) and dynamic re-balancing. Compared with existing dynamic data structures (octree, R*-tree, nanoflann k-d tree), ikd-Tree achieves superior overall performance while naturally supports downsampling on the tree. We conduct an exhaustive benchmark comparison in 19 sequences from a variety of open LiDAR datasets. FAST-LIO2 achieves consistently higher accuracy at a much lower computation load than other state-of-the-art LiDAR-inertial navigation systems. Various real-world experiments on solid-state LiDARs with small FoV are also conducted. Overall, FAST-LIO2 is computationally-efficient (e.g., up to 100 Hz odometry and mapping in large outdoor environments), robust (e.g., reliable pose estimation in cluttered indoor environments with rotation up to 1000 deg/s), versatile (i.e., applicable to both multi-line spinning and solid-state LiDARs, UAV and handheld platforms, and Intel and ARM-based processors), while still achieving higher accuracy than existing methods. Our implementation of the system FAST-LIO2, and the data structure ikd-Tree are both open-sourced on Github.

• The paper introduces a fast LiDAR-inertial odometry framework that fuses raw LiDAR data with IMU inputs via an iterated Kalman filter.
• It employs an incremental k-d tree (ikd-Tree) for dynamic map management with efficient updates and rebalancing.
• Field tests demonstrate superior accuracy and speed compared to existing systems in various real-time robotic applications.

FAST-LIO2: Fast Direct LiDAR-inertial Odometry

FAST-LIO2 introduces a fast, robust LiDAR-inertial odometry framework leveraging a tightly-coupled iterated Kalman filter. It directly registers raw LiDAR points to the map without feature extraction, enhancing adaptability across LiDAR variants and improving accuracy by exploiting subtle environmental features. An incremental k-d tree structure (ikd-Tree) facilitates dynamic map management with efficient updates and rebalancing, outperforming existing data structures like octrees and nanoflann trees in computational efficiency and scalability.

System Architecture

FAST-LIO2 processes LiDAR inputs through a pipeline starting with accumulating raw point data, followed by state estimation via iterated Kalman filtering, and continuous map updating using ikd-Tree for odometry and mapping. The ikd-Tree allows efficient incremental updates (insertions, deletions) and k-nearest neighbor searches, critical for computationally constrained platforms.

Figure 1: System overview of FAST-LIO2, detailing the integrated pipeline.

State Estimation

The core of FAST-LIO2's robust performance is its tightly-coupled iterated Kalman filter, which processes IMU and LiDAR data to estimate states and compensate for motion distortions in sequentially sampled LiDAR points. The process involves continuous state propagation with IMU inputs, iterative state updates incorporating LiDAR measurements, and back-propagation techniques to enhance accuracy in dynamic environments.

Incremental k-d Tree (ikd-Tree)

The ikd-Tree structure efficiently manages the point cloud map, supporting real-time applications by minimizing computational load during updates and searches. It implements dynamic re-balancing through parallelized re-builds, ensuring tree operations remain efficient as the map grows.

Incremental Operations

• Insertion with On-Tree Downsampling: Simultaneously inserts points and manages map resolution, maintaining efficiency by selectively keeping the nearest point within a defined spatial resolution.
• Box-wise Delete: Deletes points in specified cuboidal regions, crucial for dynamic mapping and managed by lazy labels for efficient incremental updates.

  Figure 2: 2D demonstration of map region management with incremental updates.

Performance

Compared to traditional data structures, ikd-Tree consistently delivers superior performance in insertion, deletion, and k-nearest neighbor searches across various dataset sizes. Its parallel re-building capability addresses intermittent delays in dynamic map updates.

Evaluation

FAST-LIO2's capabilities were validated across multiple datasets, outperforming existing LiDAR-inertial odometry systems like LIO-SAM and LILI-OM in accuracy and speed. It maintains high accuracy with low computational requirements, demonstrated on platforms from UAVs to handheld devices, including ARM-based systems for embedded applications.

Figure 3: Data structure comparison over different tree sizes, highlighting ikd-Tree's time efficiency.

Real-world Application

Field tests, including UAV-based autonomous navigation, emphasized FAST-LIO2's robustness and adaptability. It reliably performed odometry and mapping in complex environments, offering fast processing times critical for real-time applications in robotics and autonomous navigation.

Conclusion

FAST-LIO2 provides high-performance LiDAR-inertial odometry through innovations in incremental data structures and computational efficiency. Its adaptability across LiDAR variants and robustness in various applications emphasize its applicability in real-time robotics and autonomous systems. Future developments may focus on integration with broader SLAM frameworks and further optimizations for specific hardware platforms.