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

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.

• The paper introduces a novel hierarchical voxel map with pre-computed surfels that achieves O(1) point-to-surfel correspondence, drastically improving processing efficiency.
• It leverages Morton-code hashing in a two-level voxel structure to minimize CPU cache misses and eliminate expensive nearest neighbor searches and plane fitting during IEKF updates.
• Empirical evaluations show high processing speeds (up to 690 FPS) and reliable pose estimation comparable to state-of-the-art methods, validating its practical viability.

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

Introduction

Surfel-LIO addresses the computational bottlenecks in tightly-coupled LiDAR-Inertial Odometry (LIO) systems, particularly those related to correspondence search and plane fitting in iterative estimation frameworks. Existing state-of-the-art systems such as Fast-LIO2 and Faster-LIO have advanced map structures for efficient point-to-plane registration, but still require expensive nearest neighbor (NN) search across multiple spatial partitions and repeated plane parameter estimation during each Iterated Extended Kalman Filter (IEKF) iteration. Surfel-LIO overcomes these inefficiencies by leveraging a hierarchical voxel map with pre-computed surfel representations and spatially-local Morton code (Z-order) based hashing for $O(1)$ point-to-surfel correspondence retrieval.

Core Methodology

Hierarchical Voxel Map and Surfel Pre-computation

The central innovation is a two-level hierarchical voxel structure (hVox). The finest granularity, L0 voxels, accumulate incoming points incrementally by storing only centroids; higher-level L1 voxels aggregate multiple L0 centroids and encode local surface geometry as a surfel, defined by its centroid, normal vector (from PCA), and planarity score.

Figure 2: Hierarchical voxel structure and surfel representation. (A) L1 voxels aggregate $3 \times 3 \times 3$ grids of L0 children; (B) each L0 voxel holds only its centroid; (C) L1 surfels are computed by PCA over L0 centroids for efficient planar representation.

This decoupling eliminates the need for on-the-fly neighbor enumeration and plane estimation during IEKF updates. Instead, when a query point is transformed into world coordinates, it is rapidly matched to a pre-computed surfel in $O(1)$ time. Surfel recomputation is triggered only during map update when underlying child voxels are modified.

Z-order (Morton) Hashing for Spatial Locality

Both L0 and L1 voxels are indexed in hash tables using Morton codes, which interleave coordinate bits to preserve the spatial locality of LiDAR sampling patterns. The process involves: (A) quantizing world coordinates to voxel indices, (B) computing the Morton code through fast bit operations, and (C) directly indexing the hash table for immediate surfel retrieval. This design both minimizes CPU cache misses and aligns hash access patterns with physical point cloud structure.

Figure 3: The Surfel-LIO pipeline, including IMU and LiDAR fusion via IEKF (top), correspondence finding by Morton code hashing (bottom left), and lazy surfel recomputation strategy using PCA on L0 centroids (bottom right).

This hash structure, combined with open addressing (Robin Hood hashing), achieves high computational throughput by reducing latency and avoiding costly data structure traversals needed by k-d trees or multi-voxel neighbor search required in earlier LIO map designs.

IEKF-based State Estimation with Surfel Correspondences

Surfel-LIO employs a standard error-state IEKF that fuses IMU and registered LiDAR observations. During each LiDAR scan processing stage, every point is matched to its corresponding surfel, and a point-to-plane residual is computed analytically (no SVD or dynamic neighbor gathering per query). The IEKF then iteratively minimizes these residuals until convergence, yielding efficient and robust pose and velocity estimates.

Experimental Evaluation and Empirical Results

Comprehensive experiments were conducted on the M3DGR dataset, which provides challenging ground robot sequences captured using Livox AVIA and Mid-360 LiDAR sensors. Surfel-LIO was compared to Fast-LIO2 (ikd-Tree) and Faster-LIO (iVox) under identical settings of voxel resolution and map extent.

Figure 1: Benchmark comparison on M3DGR: Surfel-LIO achieves significantly higher throughput than Fast-LIO2 and Faster-LIO with comparable Absolute Pose Error across LiDAR modalities.

Key quantitative findings:

• NN search time per point: 0.05 μs for Surfel-LIO vs. 1.42 μs (Fast-LIO2) and 2.76 μs (Faster-LIO).
• Plane estimation time per point: 0.01 μs for Surfel-LIO, an order of magnitude improvement.
• Map update cost: Comparable across all methods (≈0.03 ms per frame).
• Frame rates: On high-density Livox AVIA, Surfel-LIO achieves 531 FPS (vs. 125 [Fast-LIO2] and 184 [Faster-LIO]); on Livox Mid-360, it processes at 690 FPS.
• Estimation accuracy (APE): Surfel-LIO demonstrates similar root mean square errors (0.365 m on AVIA; 0.342 m on Mid-360), indicating that its geometric abstraction preserves registration fidelity.

This performance gain directly results from obviating per-query nearest neighbor and SVD-based plane fitting steps—operations that scale with the number of points and rapidly dominate computation on modern high-rate LiDAR sensors.

Theoretical and Practical Implications

Surfel-LIO demonstrates that hierarchical voxel maps with surfel pre-computation can optimally amortize expensive geometric reasoning to map update phases, yielding $O(1)$ correspondence and plane estimation costs during state estimation. This fundamentally alters the computational scaling of LIO, especially during repeated IEKF iterations, and enables high-frequency odometry pipelines without sacrificing estimation robustness.

The structure’s locality-preserving hashing provides architectural compatibility with CPUs, optimizing cache utilization and reducing effective memory bandwidth. This is particularly critical for deployment on edge and real-time platforms where memory and processing resources are constrained.

From a theoretical perspective, the method formalizes a separation of concerns—map update and geometric abstraction are independent, and registration leverages static, rapidly accessible summaries (surfels) rather than raw point neighborhoods. However, this comes at the cost of adopting strong local planarity assumptions, which may limit applicability in highly non-planar or sparse environments.

Future Research Directions

Potential extensions include:

• Adaptive Voxel and Surfel Resolution: Dynamically scaling L0/L1 sizes based on local point cloud density or geometric complexity could reconcile the planarity assumption with arbitrary scene structure.
• Higher-order Geometric Primitives: Extending the abstraction to include lines/corners or hybrid surface models may capture richer geometry.
• Global Consistency: Integrating loop closure or global pose graph optimization remains an open avenue for extending Surfel-LIO beyond local odometry.
• Cross-modal Generalization: Assessing system behavior on spinning, solid-state, or even multi-modal fusion (e.g., vision-LiDAR-IMU) scenarios.
• Real-world Resource Constraints: Evaluating Surfel-LIO’s memory and power efficiency on embedded or mobile platforms, especially UAVs and handheld SLAM systems.

Conclusion

Surfel-LIO establishes a new paradigm for LIO system design by leveraging hierarchical surfel abstraction and Z-order voxel hashing to achieve $O(1)$ correspondences and plane retrieval in real-time odometry. Empirical results on diverse LiDAR modalities show this approach yields substantial processing speedups while maintaining state estimation fidelity. The surfel-based representation provides a compelling blueprint for next-generation odometry and mapping solutions, with broad implications for efficient, scalable sensor fusion in robotics and autonomous systems.