Super-LIO: Efficient LiDAR-Inertial Odometry

• Super-LIO is a LiDAR-Inertial Odometry system that integrates compact OctVox mapping with heuristic-guided KNN for efficient, real-time scan-to-map alignment.
• The OctVox structure provides O(1) point insertion with strict density control, reducing memory usage and computational overhead.
• The HKNN approach accelerates correspondence searches, delivering a 3.7× speedup and competitive accuracy on both x86 and ARM platforms.

Super-LIO is a robust and efficient LiDAR-Inertial Odometry (LIO) system designed for high performance and accuracy on resource-constrained platforms such as aerial robots and mobile autonomous systems. Central to Super-LIO is the OctVox map structure—a compact octo-voxel-based approach enforcing strict spatial density constraints—paired with a heuristic-guided KNN (HKNN) strategy for rapid correspondence search. These innovations produce significant runtime and memory efficiency while maintaining competitive or superior odometric accuracy compared to contemporary LIO systems. Super-LIO integrates smoothly with standard LIO pipelines, is open-source, and demonstrates platform compatibility across X86 and ARM architectures (Wang et al., 6 Sep 2025).

1. System Architecture and Processing Pipeline

Super-LIO employs a tightly coupled LiDAR–IMU odometry scheme based on the iterated error-state Kalman filter (IESKF). The real-time odometry update loop comprises four major stages:

1. IMU Propagation: At high rate, the filter reads IMU samples $(\mathbf{a}_k, \boldsymbol\omega_k)$ and applies midpoint integration to compute the propagated state $\hat{\mathbf X}_t^-$ and covariance $\mathbf P_t^-$.
2. LiDAR Preprocessing: For each scan, raw points $\mathbf p_i^{\mathcal L}$ undergo de-skewing relative to IMU poses at the scan anchor time $t_k$:

$$\mathbf p_i^{\mathcal I_k} = \mathbf R_{\mathcal I_i}^{\mathcal I_k} \left( \mathbf R_{\mathcal L}^{\mathcal I} \mathbf p_i^{\mathcal L} + \mathbf p_{\mathcal L}^{\mathcal I} \right) + \mathbf p_{\mathcal I_i}^{\mathcal I_k}$$

The preprocessed scan is further downsampled using a center-based filter to ensure uniform spatial coverage.

1. Odometry Estimation (Scan-to-Map): Each downsampled point $\mathbf p_j^{\mathcal I_k}$ is transformed to the world frame:

$$\mathbf p_j^{\mathcal G} = \mathbf R_{\mathcal I}^{\mathcal G}(t_k)\, \mathbf p_j^{\mathcal I_k} + \mathbf p_{\mathcal I}^{\mathcal G}(t_k)$$

For each such point: - $K$ nearest neighbors are retrieved from the OctVox map via HKNN. - Principal Component Analysis (PCA) fits a local plane. - A point-to-plane residual $$r_j = \mathbf n^\top (\mathbf p_j^{\mathcal G} - \bar{\mathbf p})$$ is computed.

The residuals are stacked and used in the IESKF observation update to yield the posterior state $\hat{\mathbf X}_t^-$0.

2. Map Update: Each de-skewed, world-frame point is inserted into the OctVox map for incremental averaging and noise suppression.

A condensed per-frame processing pseudocode illustrates this loop:

$\mathbf p_i^{\mathcal L}$2

2. OctVox Map: Data Structure and Denoising

OctVox represents the global map as a sparse hash table of voxels $\hat{\mathbf X}_t^-$1 (edge length $\hat{\mathbf X}_t^-$2), each subdivided into eight subvoxels of size $\hat{\mathbf X}_t^-$3. Every subvoxel $\hat{\mathbf X}_t^-$4 maintains:

• An accumulated mean $\hat{\mathbf X}_t^-$5
• A counter $\hat{\mathbf X}_t^-$6 capped at $\hat{\mathbf X}_t^-$7

Point-to-voxel and subvoxel indexing for each world-frame point employs bitwise and arithmetic operations to maintain constant O(1) time complexity.

Incremental Fusion and Density Control:

On insertion, if a subvoxel is empty, it is initialized with $\hat{\mathbf X}_t^-$8. If occupied and $\hat{\mathbf X}_t^-$9 and $\mathbf P_t^-$0, an incremental mean is computed:

$\mathbf P_t^-$1

Otherwise, the point is dropped, enforcing a strict density and providing per-subvoxel denoising, with statistical variance decaying as $\mathbf P_t^-$2.

The following pseudocode summarizes this operation:

$\mathbf p_i^{\mathcal L}$3

Efficiency: Each insertion has an expected O(1) complexity due to the hash structure (Wang et al., 6 Sep 2025).

3. Heuristic-Guided KNN (HKNN) Search

The HKNN module accelerates the nearest-neighbor correspondence crucial for scan-to-map alignment. HKNN operates via two phases:

Precomputation:

• Defines a canonical origin subvoxel.
• Enumerates all candidate subvoxels inside a maximum radius $\mathbf P_t^-$3.
• Computes minimal Euclidean distances $\mathbf P_t^-$4 across all subvoxel pairs and groups them as $\mathbf P_t^-$5, sorted in ascending order to create a traversal list $\mathbf P_t^-$6.

Runtime Query:

• For each transformed query point:
  • Computes its parent voxel/subvoxel.
  • Iteratively explores the sorted subvoxel groups up to the pre-set search radius, leveraging octant symmetry for efficient candidate indexing.
  • Maintains a max-heap of size $\mathbf P_t^-$7 for minimal distance subvoxel means.

Pseudocode excerpt: $\mathbf p_i^{\mathcal L}$4

Complexity: HKNN inspects subvoxel groups by increasing shell distance and breaks early if no closer points can be found. In practice, it examines orders of magnitude fewer entries than standard approaches (≪ $\mathbf P_t^-$8), resulting in near-O(1) empirical query time.

4. Integration with LiDAR-Inertial Odometry Frameworks

Super-LIO is designed for seamless integration with existing error-state Kalman filter-based LIO systems. Key modifications include:

• Substituting the traditional raw-point or KD-tree map container with the OctVox hash-voxel structure.
• Replacing the generic KNN search algorithm with HKNN for point-to-map correspondence.
• Adopting center-based downsampling as the primary LiDAR preprocessing step to optimally feed the OctVox structure.
• Maintaining identical observation models and filter mathematics (such as point-to-plane constraints in IESKF).
• Retaining all IMU propagation logic (IESKF or IEKF).

Parameters related to voxel size, density thresholds ($\mathbf P_t^-$9), neighborhood size ($\mathbf p_i^{\mathcal L}$0), and maximal search radius ($\mathbf p_i^{\mathcal L}$1) are exposed for tuning (Wang et al., 6 Sep 2025).

5. Experimental Evaluation

Super-LIO has been extensively assessed on both public and private datasets:

• Public: M2DGR (ground vehicles), NCLT (long-term outdoor), MCD (solid-state LiDAR), NTU (aerial systems)
• Private: 10 sequences from Livox MID360, covering diverse environments

Metrics: Root Mean Squared Error (RMSE), Relative Pose Error, per-frame runtime (ms), CPU load (%), memory footprint.

Performance Summary:

Method	Avg Time (ms)	CPU %
Super-LIO	2.66	51
Super-LIO*	3.05	65
FAST-LIO2	9.92	62
Faster-LIO	7.57	54
iG-LIO	21.07	62

• Super-LIO offers a 3.7× speedup vs FAST-LIO2 and 11% lower CPU usage on x86 platforms.
• On ARM devices (Orin NX), Super-LIO achieves ∼9.4 ms per frame, 49% CPU utilization, and a 4.2× speedup over FAST-LIO2.
• Accuracy (RMSE = 0.74 m avg) matches or surpasses FAST-LIO2, Faster-LIO, iG-LIO.
• Ablation removing HKNN (Super-LIO*) yields 5–10% reduced accuracy.
• OctVox provides ∼20% smaller hash table size compared to KD-tree or iKD-tree storage; per-point map update remains strictly O(1) with no batch rebuilds.

6. Implementation and Open-Source Availability

Super-LIO is fully open-source and plug-and-play compatible for a range of LiDAR sensors and system platforms. The implementation is modular:

• src/ieskf/: IESKF filter (propagation/update)
• src/octvox/: OctVox mapping (subvoxel indexing, hash table methods)
• src/hknn/: HKNN (group precomputation and runtime query)
• src/preprocess/: LiDAR deskewing, center-based downsampling
• launch/: ROS2 launch files for various sensor configurations

System setup (Ubuntu 20.04, ROS2 Foxy) and examples: $\mathbf p_i^{\mathcal L}$5 Threading and resource allocation are automatically adapted to detected CPU cores. Configuration (e.g., voxel size, merge threshold, neighborhood parameters) is provided in cfg/params.yaml.

Super-LIO’s OctVox and HKNN deliver a concise yet adaptable mapping and search engine, markedly enhancing efficiency on both desktop and embedded computing architectures while retaining or surpassing state-of-the-art odometric fidelity (Wang et al., 6 Sep 2025).