FAST-LIO2: 100Hz LiDAR Odometry Without Features

TL;DR

FAST-LIO2 is a high-speed, high-accuracy LiDAR-IMU Odometry system developed by HKU MARS Lab. Two core innovations: (1) Direct raw point cloud registration to the map — no feature extraction, (2) Incremental k-d Tree (ikd-Tree) for O(log n) real-time map updates. Result: 100Hz on an Intel i7, real-time performance even on ARM embedded boards. Outperforms LIO-SAM, LOAM, and LILI-OM on 19 benchmark sequences in both accuracy and speed. Published in IEEE Transactions on Robotics 2022.

Background: The Long-Standing Trade-off in LiDAR Odometry

The history of LiDAR odometry has largely been a story of trade-offs.

LOAM family (2014~): Extracts edge and planar features, then performs scan-to-map matching. Accurate, but feature extraction is expensive — and brittle in feature-poor environments like tunnels, corridors, and open plazas.

LIO-SAM (2020): IMU pre-integration + factor graph + LiDAR feature matching + loop closure. Capable, but heavyweight — and requires re-tuning parameters for each different LiDAR model.

FAST-LIO (2021, predecessor): Still feature-based, but uses an iterated Kalman filter to reduce compute. The direct precursor to FAST-LIO2.

This progression naturally leads to a question:

"Can we build a system that skips feature extraction entirely, and keeps the map updated in real time?"

FAST-LIO2 is the answer.

Core Architecture: Two Key Innovations

Fig.1. FAST-LIO2 system pipeline — LiDAR scan + IMU input, state estimation via iterated Kalman filter, direct registration to ikd-Tree map

Innovation 1: Direct Point Registration (No Feature Extraction)

LOAM and LIO-SAM extract edge and planar features from raw scans before matching. FAST-LIO2 eliminates this step entirely.

Instead, each raw point is matched against its k nearest neighbors in the map to compute a planar residual:

residual = uⱼᵀ · (T̂_GI · T̂_IL · pʲ_L − qʲ)

• uⱼ: local plane normal vector from the map
• pʲ_L: point in LiDAR frame
• qʲ: centroid of nearest neighbors in the map

Advantages:

• No environment-specific feature extraction parameter tuning
• Same pipeline works for indoors, outdoors, corridors, and open areas
• LiDAR-agnostic — Livox solid-state and Velodyne spinning both work without code changes

Fig.2. Measurement model — residual computation between map plane normals and incoming LiDAR points

Innovation 2: ikd-Tree (Incremental k-d Tree)

How do you manage a map that's constantly growing? Existing approaches have problems:

• Static k-d tree: Adding points over time requires periodic full rebuilds → latency spikes
• Voxel grid: Fixed resolution, fast but accuracy-limited

The ikd-Tree solves both simultaneously:

Key mechanisms:

• α-balanced: Automatic rebalancing when tree height becomes uneven
• α-deleted: Garbage collection when deleted node ratio exceeds threshold
• Lazy deletion: Nodes are flagged rather than immediately removed
• Parallel rebuilding: Subtree reconstruction runs in a background thread — never blocks the main estimation loop

Fig.3. Map region management — points outside the LiDAR FOV are removed via box-wise delete; new points are inserted in O(log n)

Fig.4. ikd-Tree subtree re-building — unbalanced subtrees are reconstructed in a parallel thread without blocking the main thread

Iterated Extended Kalman Filter (iEKF)

State vector (24-dimensional, on manifold SO(3)×ℝ¹⁵×SO(3)×ℝ³):

x = [R ∈ SO(3), p ∈ ℝ³, v ∈ ℝ³, bω ∈ ℝ³, ba ∈ ℝ³, g ∈ ℝ³, R_IL ∈ SO(3), t_IL ∈ ℝ³]

State propagation on manifold:

xᵢ₊₁ = xᵢ ⊞ (Δt · f(xᵢ, uᵢ, wᵢ))

Motion distortion correction: Each LiDAR point has a precise capture timestamp. IMU back-propagation interpolates the pose at that exact moment, eliminating point cloud distortion even at high rotational speeds.

Online LiDAR-IMU extrinsic calibration: R_IL and t_IL are included in the state vector and estimated simultaneously with odometry. No offline calibration required — provide a rough initial guess and it converges during operation.

Evaluation: 19 Sequences, 5 Datasets

Accuracy: Lowest RMSE on the majority of 19 sequences versus LOAM, LIO-SAM, LINS, and LILI-OM.

Speed: Over 8x faster than LIO-SAM. Sustains 100Hz on an Intel i7-8550U.

Embedded operation: Real-time confirmed on ARM Cortex-A73 (Khadas VIM3, 4GB RAM). Onboard drone deployment becomes practical.

Key Experiments

Stress Tests

High angular velocity robustness: Stable pose estimation up to 1000 deg/s. LOAM-family systems diverge at this speed. IMU back-propagation is what makes this possible.

UAV deployment: Demonstrated real-time odometry on a quadrotor with Livox Avia + DJI Manifold 2-C. Stable under motor vibration and aggressive maneuvers.

Sensor agnosticism: Livox Horizon (non-uniform solid-state scan) and VLP-16 (uniform spinning scan) both processed with the same code and same parameters. Feature-based methods inevitably require per-LiDAR parameter tuning — FAST-LIO2 eliminates this overhead entirely.

ikd-Tree vs. Existing Data Structures

The paper benchmarks ikd-Tree against octree, R*-tree, and nanoflann k-d tree:

• Insert/delete speed: ikd-Tree > all others
• Search speed: ikd-Tree ≈ nanoflann (optimized static k-d tree)
• Dynamic updates: ikd-Tree only

Limitations — A Field Engineer's Perspective

Limitations acknowledged in the paper:

1. No loop closure: Pure odometry system. Drift accumulates over long distances; the map won't close when returning to the start point
2. Local map size is bounded: Points beyond a certain range are deleted. Not suitable for large-scale persistent mapping
3. Static initialization required: Needs a stationary start to estimate IMU biases. Cold start while moving is not supported

Additional field observations:

• Degenerate geometry: In environments where surface normals concentrate in one direction — long corridors, open plazas — estimation becomes unstable. FAST-LIO2 is not immune to geometry degeneracy. Deploying a separate degeneracy detection layer is recommended for production
• No dense map: The ikd-Tree stores a sparse point map for odometry purposes. Dense 3D reconstruction or voxel occupancy maps require extension systems like R3LIVE or FAST-LIVO2
• Solid-state scan edge cases: Livox's non-uniform scan pattern can produce uneven nearest-neighbor search results within a single frame in some edge cases
• Not fully parameter-free: No feature extraction, but ikd-Tree α-balance/α-delete thresholds and point downsampling resolution still need environment-specific tuning for optimal performance

The Lineage — What FAST-LIO2 Unlocked

What FAST-LIO2 proved: Direct registration without features + incremental map = fast + accurate + generalizable. This combination became the baseline for every subsequent HKU-MARS LIO system.

Summary — Key Takeaways

1. Dropping feature extraction makes you more accurate, not less — Direct raw point registration improves both adaptability and accuracy. Challenges the prevailing belief that feature-based pipelines are inherently more stable
2. ikd-Tree is why 100Hz is possible — O(log n) real-time map updates with no latency spikes. Parallel rebalancing is the critical enabler
3. LiDAR-IMU extrinsic as a state variable — No separate calibration step; the system converges during normal operation. A major operational convenience win
4. Real-time on ARM embedded — Validated on Khadas VIM3 (ARM, 4GB). Onboard deployment on drones and mobile robots is now realistic
5. It's odometry, not SLAM — No loop closure. For long-range mapping applications, extensions like SC-FAST-LIO2 are needed. Know what you're deploying

📄 Paper: arXiv:2107.06829 🐙 GitHub: hku-mars/FAST_LIO

Next post: Octo — open-source generalist robot policy, the democratization of VLA