FAR-LIO: Fast LiDAR-Inertial Odometry

• FAR-LIO is a CUDA-accelerated LiDAR-Inertial Odometry framework that achieves low latency and high accuracy in dynamic, high-speed scenarios.
• It integrates a GPU-centric LiDAR scan pipeline with a CUDA voxel hashmap, adaptive sparsity-aware GICP, and an EKF backend with delay compensation.
• Extensive evaluations across racing and public datasets show FAR-LIO delivers bounded runtime, reduced odometry errors, and improved robustness over competing methods.

FAR-LIO, short for Fast, Accurate, and Robust LiDAR-Inertial Odometry, is a CUDA-accelerated LiDAR–Inertial Odometry (LIO) framework designed to deliver low-latency, accurate, and robust odometry in highly dynamic conditions, including autonomous racing at speeds of up to 250 km/h. It combines a GPU-centric LiDAR scan pipeline, a CUDA voxel hashmap for parallelized nearest-neighbor search and map maintenance, a sparsity-aware Generalized Iterative Closest Point formulation with adaptive thresholding and robust loss, and an Extended Kalman Filter backend with upsampling and delay compensation to produce a smooth high-frequency odometry stream (Leitenstern et al., 24 Jun 2026).

1. Problem setting and design objectives

FAR-LIO is motivated by the requirement that high-speed autonomy imposes strict latency and robustness constraints. At 250 km/h, even 50–100 ms of odometry lag translates to multiple meters of control error, which can destabilize closed-loop controllers. The target regime is additionally characterized by highly non-stationary motion, strong scan distortion, and substantial IMU noise, particularly in racecar deployments.

The framework is therefore organized around a specific systems objective: reducing odometry computation time while also making it bounded and predictable, without sacrificing registration accuracy under strong motion, varying point densities, and outliers. This emphasis distinguishes latency as a first-class design variable rather than a by-product of algorithmic efficiency.

A plausible implication is that FAR-LIO treats odometry not merely as an estimation problem but as a closed-loop control dependency. In that reading, bounded runtime, low jitter, and explicit delay handling are as important as scan-to-submap alignment accuracy.

2. End-to-end architecture

FAR-LIO consists of a GPU-centric LiDAR scan pipeline and a CPU EKF backend. The LiDAR pipeline performs preprocessing and undistortion, scan-to-submap registration, submap update, and LiDAR-frame velocity computation. The EKF runs at a fixed 100 Hz and fuses asynchronous IMU measurements with delayed LiDAR pose and velocity updates.

During preprocessing and undistortion, FAR-LIO fits linear regression to recent velocity history, both linear and angular, to model continuous motion during scan acquisition. Per-point motion correction is then evaluated in CUDA, and points are undistorted to the frame timestamp corresponding to the final point time. This step directly targets the heavy distortion induced by aggressive maneuvers.

Registration aligns the incoming scan $P$ with a local adaptive-density submap $Q$. The initial guess is the EKF pose $T_L$, and the scan is transformed accordingly:

$P^0 = \{ p_i^0 = T_L p_i \}.$

An iterative optimization then alternates between correspondence construction through the CUDA voxel map and robust covariance-aware alignment. The loop stops when $\| \Delta T \|_2 \le 5\times 10^{-3}$ or when registration time reaches $2/f_L$, i.e., at most twice the LiDAR period.

After registration, the current scan is inserted into the local submap under adaptive per-voxel capacity constraints, newly added points have their covariances recomputed, and voxels outside a 1 km radius are removed to bound memory and latency. LiDAR velocity is computed from successive registered poses and passed to the EKF for fusion (Leitenstern et al., 24 Jun 2026).

3. CUDA voxel hashmap and adaptive local submap

A central component of FAR-LIO is cuVoxelMap, a CUDA voxel hashmap built on cuCollections cuco::static_map with open addressing and linear probing using step size 1. Keys are 3D integer voxel indices $(i,j,k)$, and values reference fixed-capacity voxel records that store up to $N_{\max}$ points. Spatial hashing follows Niessner et al. voxel hashing to map voxel indices to buckets in device memory, with each voxel represented as a cube of size $v \times v \times v$.

The adaptive-density design is explicit. Voxel size is set to $v = 4$ m for stable local geometry. Within each voxel, FAR-LIO retains at most $Q$0 regularly spaced points, enforcing approximately 0.65 m minimum spacing. Before map update, it computes the average points-per-voxel in the vicinity of the current scan and applies a linear decay over range to compensate for reduced density at long distances, with a lower bound of $Q$1 points needed for covariance estimation. This produces an adaptive per-voxel capacity denoted $Q$2.

For parallel nearest-neighbor search, each query point searches the containing voxel and its 26 neighbors, for 27 voxels total. Each CUDA thread handles one query point, iterates over the 27 voxel keys, fetches voxel point lists, and computes nearest-neighbor distances in parallel. Because per-voxel capacity is bounded, the search cost is reported as

$Q$3

on average per point, while overall registration scales linearly with scan size.

The kernel design uses one thread per query point and a structure-of-arrays memory layout for points, x[], y[], z[], and timestamps[], to maximize coalesced loads. Collision resolution in the hash map uses linear probing; inserts use atomic compare-and-swap, and per-voxel insertion uses atomic counters to enforce $Q$4 under concurrent writes. Out-of-range voxels satisfying $Q$5 m are culled to bound memory, update time, and latency.

This GPU data structure is directly linked to the reported callback times of 9.59 ms for a single LiDAR on public datasets and 19.23 ms for three concatenated LiDARs in racing conditions, while maintaining a tight runtime distribution with few outliers (Leitenstern et al., 24 Jun 2026).

4. Sparsity-aware GICP and registration model

FAR-LIO employs a sparsity-aware Generalized Iterative Closest Point algorithm with adaptive thresholding and a robust Cauchy loss. For explanatory purposes, the point-to-plane residual is written as

$Q$6

Its generalized ICP objective is covariance-aware and robust:

$Q$7

where $Q$8 is a source point, $Q$9 is a reference point, $T_L$0, and $T_L$1 is the Cauchy robust kernel.

For each source point, covariance is estimated from the local $T_L$2-neighborhood with $T_L$3 gathered via the voxel map. The estimate is regularized using Frobenius-norm scaling:

$T_L$4

This regularization stabilizes the inverse covariance used in Mahalanobis distances and mitigates degenerate local structures.

Outlier rejection follows adaptive gating in the style of KISS-ICP. Let $T_L$5. FAR-LIO uses a time-varying gate $T_L$6 based on the deviation between the initial guess and the latest registered pose. A common form is

$T_L$7

The threshold adapts over iterations, expanding when the initial guess is poor and shrinking as residuals settle. The robust kernel is

$T_L$8

with dynamically adjusted scale $T_L$9.

The “sparsity-aware” component is a special case for regions in which $P^0 = \{ p_i^0 = T_L p_i \}.$0 cannot be reliably estimated because of insufficient neighbors:

$P^0 = \{ p_i^0 = T_L p_i \}.$1

This reduces the objective locally to a point-to-point metric with unit covariance, preventing unstable covariance estimates and improving convergence in sparsely observed areas. The paper explicitly notes that extreme sparsity or degenerate geometry can still challenge covariance estimation; SA-GICP mitigates this, but may reduce local accuracy.

This suggests that FAR-LIO does not enforce a uniform geometric model across the scene. Instead, it switches between covariance-aware and sparsity-aware regimes depending on local observability, which is a practical compromise for mixed-density environments (Leitenstern et al., 24 Jun 2026).

5. EKF backend, upsampling, and delay compensation

The backend state is

$P^0 = \{ p_i^0 = T_L p_i \}.$2

FAR-LIO uses a purely kinematic model with Euler-angle kinematics $P^0 = \{ p_i^0 = T_L p_i \}.$3, rotation $P^0 = \{ p_i^0 = T_L p_i \}.$4, centripetal and gravity compensation, and static biases $P^0 = \{ p_i^0 = T_L p_i \}.$5 and $P^0 = \{ p_i^0 = T_L p_i \}.$6 calibrated during standstill. Prediction follows the standard EKF form:

$P^0 = \{ p_i^0 = T_L p_i \}.$7

The measurement vector combines LiDAR odometry and stabilizing references:

$P^0 = \{ p_i^0 = T_L p_i \}.$8

where $P^0 = \{ p_i^0 = T_L p_i \}.$9 are roll and pitch reference angles computed per Tseng/Goblirsch to stabilize vertical drift. The EKF update is

$\| \Delta T \|_2 \le 5\times 10^{-3}$0

$\| \Delta T \|_2 \le 5\times 10^{-3}$1

$\| \Delta T \|_2 \le 5\times 10^{-3}$2

A defining aspect of the backend is explicit delay compensation. LiDAR measurements arrive asynchronously and with delay relative to EKF time. Using Larsen’s projection, a delayed measurement at time $\| \Delta T \|_2 \le 5\times 10^{-3}$3 is projected to current time $\| \Delta T \|_2 \le 5\times 10^{-3}$4 as

$\| \Delta T \|_2 \le 5\times 10^{-3}$5

FAR-LIO then upsamples the LiDAR correction across multiple EKF cycles so that the effect of delayed updates is smoothly distributed rather than injected as a discrete state jump. The output is a high-frequency, low-jitter odometry stream at 100 Hz.

The ablation results emphasize the role of this backend. On yas_10 and yas_20, removing EKF and delay compensation produces APE values of 66.32 and 192.70, with RPE values of 6.94 and 26.83. Adding EKF improves performance, adding delay compensation further reduces drift, adding motion undistortion lowers RPE, and adding SA-GICP and ASMD yields the full-system values of 15.68 / 25.62 APE and 1.67% / 1.84% RPE (Leitenstern et al., 24 Jun 2026).

6. Sensor setups, evaluation protocol, and reported results

FAR-LIO is evaluated across four LiDAR–IMU configurations using both public datasets and autonomous racing data. The tested setups are summarized below.

Setup	LiDAR / IMU	Context
yas_10, yas_20	3× Seyond Falcon; VectorNav VN-310 at 800 Hz	A2RL Yas Marina
mon_20	3× Luminar Iris; VN-310 and Epson G370N	IAC Monza
KITTI Odometry	Velodyne HDL-64; OXTS RT 3003 at 10 Hz	KITTI sequences 00–10
MulRan	Ouster OS1-64; Xsens MTi-300 at 100 Hz	DCC, KAIST, Riverside, Sejong

Extrinsic calibration and time synchronization are assumed as part of dataset preparation, and FAR-LIO uses the provided transforms. The same parameter set is used across KITTI, MulRan, and racing datasets.

On target hardware consisting of a dSPACE AUTERA Autobox with Intel Xeon D-2166NT and NVIDIA RTX A5000, with algorithms limited to 4 CPU cores at 3 GHz or the GPU and running in ROS, FAR-LIO reports average LiDAR callback times of 19.23 ms for racing with three LiDARs and 9.59 ms for public single-LiDAR datasets. GPU utilization is reported as below 10% on public datasets and approximately 20% on racing workloads with three LiDARs. Preallocated VRAM is approximately 2 GB.

The principal runtime comparison reports:

• FAR-LIO: 19.23 ms on racing data and 9.59 ms on public datasets.
• Faster-LIO: 50.01 ms on racing data and 18.55 ms on public datasets.
• Fast-LIO2, D-LIO, and KISS-ICP: unable to meet sensor frequency or diverge on racing data.

For accuracy, the reported highlights include:

• yas_10: FAR-LIO 15.68 m / 1.67%; Fast-LIO2 37.24 m / 2.14%; Faster-LIO 36.00 m / 3.30%; D-LIO 19.70 m / 4.34%; KISS-ICP diverged.
• yas_20: FAR-LIO 25.62 m / 1.84%; Fast-LIO2 50.13 m / 2.08%; Faster-LIO 55.71 m / 3.07%; D-LIO 45.23 m / 3.31%; KISS-ICP diverged.
• mon_20: FAR-LIO 297.39 m / 2.74%; Fast-LIO2 262.16 m / 4.44%; Faster-LIO 475.28 m / 7.06%; D-LIO and KISS-ICP diverged.

On KITTI, FAR-LIO ranks best or second across most sequences, with examples including kitti_05 at 3.45 m / 0.94%, kitti_09 at 8.05 m / 0.89%, and kitti_00 at 9.79 m / 1.31%. Fast-LIO2 and Faster-LIO fail on sequences 01 and 10. On MulRan, FAR-LIO is reported as best overall, with examples including DCC at 18.23 m / 3.02%, KAIST at 28.11 m / 2.71%, Riverside at 65.95 m / 2.91%, and Sejong at 2280.18 m / 6.54%, while KISS-ICP remains competitive in RPE on KAIST and Sejong.

Aggregated over the benchmarked settings, FAR-LIO reports an average 6.9% reduction in positional error and 38.4% lower runtime compared with baselines on target hardware. Simulated additional delays indicate that APE remains stable up to approximately 60–100 ms and then grows sharply toward divergence, whereas FAR-LIO’s actual runtime remains well within that region (Leitenstern et al., 24 Jun 2026).

7. Relation to prior work, limitations, and deployment considerations

FAR-LIO is positioned relative to several established LIO and ICP systems. FAST-LIO2 and Faster-LIO rely on iKD-tree and iVox for CPU-based map structures. The reported comparison states that these methods can be fast but often fail to meet latency and robustness targets on dense scans and low-rate IMUs, with KITTI 01 and 10 failures noted for FAST-LIO2 and Faster-LIO. FAR-LIO keeps the voxel paradigm but moves it to the GPU with constant-time hashing, with the stated objective of reducing latency variance and delivering bounded runtime. D-LIO uses GICP and a geometric observer, and is reported to achieve good APE in some sequences but to diverge on long or high-speed runs while lacking explicit latency handling. KISS-ICP is identified with simplicity and adaptive thresholding; FAR-LIO adopts similar adaptive gating but augments it with covariance-aware GICP, sparsity handling, and an EKF with delay compensation.

The framework also has explicit limitations. Extreme sparsity or degenerate geometry can challenge covariance estimation, even though SA-GICP mitigates the issue. Large pitch misalignment between $\| \Delta T \|_2 \le 5\times 10^{-3}$6 and the submap can degrade registration, which is reported for MulRan Sejong RPE spikes. Severe dust, rain, or strong vibrations affect LiDAR returns and IMU measurements; robust loss and EKF references help, but cannot fully compensate for missing structure. Sensitivity to voxel parameters is also noted: overly small $\| \Delta T \|_2 \le 5\times 10^{-3}$7 or $\| \Delta T \|_2 \le 5\times 10^{-3}$8 increases compute and noise sensitivity, whereas overly large values reduce geometric fidelity. The time limit of $\| \Delta T \|_2 \le 5\times 10^{-3}$9 can cause occasional frame discard; this increases robustness, but drift can increase if discards become frequent.

For deployment, the recommended single parameter set is approximately $2/f_L$0 m, $2/f_L$1, $2/f_L$2 neighbors for covariance, and a prune radius of approximately 1000 m. The registration convergence threshold is $2/f_L$3, with time cap $2/f_L$4. The EKF is run at 100 Hz with gravity compensation enabled. IMU biases $2/f_L$5 and $2/f_L$6 are calibrated during standstill and are not estimated online in high-noise settings. Tuning guidance includes adapting the linear decay slope in $2/f_L$7 to sensor range characteristics, increasing $2/f_L$8 or reducing $2/f_L$9 when scans are extremely dense, and ensuring that the undistortion regression window spans recent velocity variations during aggressive maneuvers. For control integration, the recommended signal is the EKF output at 100 Hz rather than raw LiDAR registration.

The implementation is open-source at https://github.com/TUMFTM/FAR-LIO, with usage notes specifying a CUDA-capable GPU, calibrated extrinsics, synchronized timestamps, and default parameters that generalize across KITTI, MulRan, and racing data (Leitenstern et al., 24 Jun 2026).