MyRoboPath
autonomous slam17 min readUpdated 2026-03-12Advanced

3D LiDAR-Inertial Odometry & Mapping with FAST-LIO2

Implement high-speed, direct 3D LiDAR-inertial state estimation using FAST-LIO2: ikd-Tree dynamic spatial indexing, Iterated Error-State Kalman Filter (IESKF), and Livox/Ouster point clouds.

Dr. Liam Sterling
Dr. Liam Sterling
Head of Autonomous Navigation

Key Engineering Takeaways

  • FAST-LIO2 processes raw 3D LiDAR point clouds at 100Hz+ with sub-millisecond computation per frame.
  • The ikd-Tree supports dynamic point insertion and deletion in O(log N) time without rebuilding the full 3D spatial index.
  • Tight coupling between 200Hz IMU and LiDAR point-to-plane residuals yields drift-free odometry even during aggressive drone maneuvers.
Prerequisites
  • State estimation
  • 3D Point Cloud basics

The FAST-LIO2 Architectural Breakthrough

Traditional 3D SLAM algorithms (LOAM, LIO-SAM) extract feature points (edges and planar surfaces), which fails in degenerate unstructured environments (tunnels, forests). FAST-LIO2 directly matches raw points to local planes using an incremental kd-tree (**ikd-Tree**), achieving unprecedented speed and robustness.
Tags:#FAST-LIO#3D SLAM#LiDAR-Inertial#ikd-Tree#IESKF#Point Cloud