Surfel-LIO: Fast LiDAR-Inertial Odometry with Pre-computed Surfels and Hierarchical Z-order Voxel Hashing

TL;DR

Surfel-LIO introduces a two-level hierarchical voxel map (hVox) with pre-computed surfels and Z-order hashing to achieve near-constant-time correspondence and plane estimation in LiDAR-inertial odometry. By storing surfel geometry at map update time and using cache-friendly Morton-encoded hashing, it eliminates runtime neighbor searches and repeated plane fitting during IEKF iterations. Experimental results on the M3DGR dataset show substantial speedups over Fast-LIO2 and Faster-LIO with comparable pose accuracy, highlighting practical gains for real-time systems. The approach is released publicly to support reproducibility and further research.

Abstract

LiDAR-inertial odometry (LIO) is an active research area, as it enables accurate real-time state estimation in GPS-denied environments. Recent advances in map data structures and spatial indexing have significantly improved the efficiency of LIO systems. Nevertheless, we observe that two aspects may still leave room for improvement: (1) nearest neighbor search often requires examining multiple spatial units to gather sufficient points for plane fitting, and (2) plane parameters are typically recomputed at every iteration despite unchanged map geometry. Motivated by these observations, we propose Surfel-LIO, which employs a hierarchical voxel structure (hVox) with pre-computed surfel representation. This design enables O(1) correspondence retrieval without runtime neighbor enumeration or plane fitting, combined with Z-order curve encoding for cache-friendly spatial indexing. Experimental results on the M3DGR dataset demonstrate that our method achieves significantly faster processing speed compared to recent state-of-the-art methods while maintaining comparable state estimation accuracy. Our implementation is publicly available at https://github.com/93won/lidar_inertial_odometry.

Figures (3)

• Figure 1: Benchmark comparison on M3DGR dataset zhang2025m3dgr. Our method achieves comparable accuracy with significantly higher throughput compared to Fast-LIO2 and Faster-LIO across different LiDAR sensors.
• Figure 2: Overview of the Surfel-LIO pipeline and correspondence finding procedure. Top: the system fuses IMU propagation and LiDAR measurements through IEKF to estimate pose. Bottom left: correspondence finding via hVox with Z-order hashing, consisting of (A) point-to-voxel key conversion, (B) Morton code computation through bit interleaving, and (C) O(1) hash table lookup to retrieve the pre-computed surfel. Bottom right: map update procedure where new points update L0 centroids, triggering lazy surfel recomputation via PCA when children change. The Morton code example uses 3-bit coordinates for illustration; the actual implementation uses 21 bits per axis to support large-scale environments.
• Figure 3: Hierarchical voxel structure and surfel representation. (A) An L1 voxel encompasses a $3 \times 3 \times 3$ grid of L0 child voxels, where shaded cells indicate occupied voxels. (B) Each occupied L0 voxel stores only its centroid (red points), computed incrementally as points accumulate. (C) A surfel is derived by applying PCA to the L0 centroids, yielding a compact plane representation $(\mathbf{n}, \mathbf{c})$ for point-to-plane registration.