LiDAR Odometry

LiDAR: Light Detection and Ranging

Follow this topic Ask about this topic

Momentum

9 papers in the last four weeks, with none the four weeks before. 0.1% of all new papers.

Jul 13Week of Sep 28

Latest papers 32

Oct 4, 2026cs.RO

GR-LIO: A Local Ground-Aware LiDAR-Inertial Odometry System Using Body-to-Ground Height

LiDAR-inertial odometry (LIO) is widely used for state estimation in ground-based autonomous mobile robots. However, the geometric constraints provided by the local ground surface remain largely underexploited in existing LIO systems. This paper proposes a filter-based local ground-aware LIO framework that explicitly incorporates local ground plane geometry into the state estimation process to improve both localization accuracy and computational efficiency. Specifically, a local ground plane is parameterized by the robot orientation and the body-to-ground (B-G) height and continuously propagated within the state estimation process. Based on the proposed B-G geometry model, a propagated local ground plane enables efficient and reliable ground segmentation. The segmented ground points are then incorporated into the filter update through point-to-plane geometric constraints, improving both state estimation accuracy and efficiency. Furthermore, a planar motion update is introduced to exploit the propagated local ground plane as an additional geometric constraint, effectively suppressing vertical drift and improving estimation robustness. To address the initially unknown B-G height, an efficient initialization strategy is developed, followed by an online calibration procedure for continuous refinement. The proposed system is evaluated on several public benchmark datasets and self-collected real-world datasets covering diverse operating scenarios. Experimental results demonstrate that the proposed method consistently outperforms representative LIO methods in terms of both localization accuracy and computational efficiency.

Zhixin Zhang, Yang Song, Liang Zhao +3

Department of Electrical and Electronic Engineering, University of Manchester, Manchester M13 9PL, U.K. · School of Informatics, University of Edinburgh, Edinburgh EH8 9AB, U.K. · Department of Mechanical, Aerospace and Civil Engineering, University of Manchester, Manchester M13 9PL, U.K.

LiDAR Odometry Robot State Estimation

Sep 29, 2026cs.RO

Degeneracy-Orthogonal Geometric Constraints for LiDAR SLAM

Autonomous robot navigation relies on simultaneous localization and mapping (SLAM) to estimate motion and maintain an accurate pose within an environment. However, in axially uniform corridors such as long tunnels and pipelines, LiDAR odometry is fundamentally limited by unconstrained drift along the feature-weak travel direction. This structural degeneracy cannot be resolved by local scan matching alone. To address this challenge, we propose the Degeneracy-orthogonal Contour Offset Descriptor (DeCOD), a structure-aligned geometric descriptor for cross-sectional landmarks. Cross-sectional boundaries, such as pipe joints and structural rings, provide metric constraints along this degenerate axis, but distinguishing individual landmarks requires capturing subtle surface variations across nearly identical profiles. The descriptor parameterizes signed normal deviation from estimated boundary contours, and matching explicitly resolves heading ambiguity and decouples first-order contour errors by distortion estimation. Matched landmarks yield geometric factors that enforce agreement in cross-section position and corridor axis alignment during pose-graph optimization, correcting longitudinal drift while leaving rotation about the common axis unconstrained. On a public benchmark and in field experiments, DeCOD achieves robust landmark retrieval over standard 3D descriptors and successfully stabilizes trajectories across different odometry frontends, reliably constraining longitudinal drift under geometric degeneracy.

Minseo Kim, Yina Kim, Jinhwa Hwang +1

Department of Mechanical Systems Engineering, Sookmyung Women’s University, 100 Cheongpa-ro 47-gil, Yongsan-gu, Seoul 04310, Republic of Korea

Simultaneous Localization and Mapping LiDAR Odometry

Sep 24, 2026cs.RO

Free-Init: Scan-Free, Motion-Free, and Correspondence-Free Initialization for Doppler LiDAR-Inertial Systems

Robust initialization is crucial for online systems. In the letter, a high-frequency and resilient initialization framework is designed for LiDAR-inertial systems, leveraging both inertial sensors and Doppler LiDAR. The innovative FMCW Doppler LiDAR opens up a novel avenue for robotic sensing by capturing not only point range but also Doppler velocity via the intrinsic Doppler effect. By fusing point-wise Doppler velocity with inertial measurements under non-inertial kinematics, the proposed framework, Free-Init, eliminates reliance on motion undistortion of LiDAR scans, excitation motions, and map correspondences during the initialization phase. Free-Init is also plug-and-play compatible with typical LiDAR-inertial systems and is versatile to handle a wide range of initial motions when the system starts, including stationary, dynamic, and even violent motions. The embedded Doppler-inertial velocimeter ensures fast convergence and high-frequency performance, delivering outputs exceeding 10 kHz. Comprehensive experiments on diverse platforms and across myriad motion scenes validate the framework's effectiveness. The results demonstrate the superior performance of Free-Init, highlighting the necessity of fast, resilient, and dynamic initialization for online systems.

Mingle Zhao, Jiahao Wang, Tianxiao Gao +2

The authors are with the University of Macau.

Inertial Odometry LiDAR Odometry Multimodal Sensor Fusion

Sep 24, 2026cs.RO

FMCW-LIO: A Doppler LiDAR-Inertial Odometry

Conventional LiDAR-inertial odometry (LIO) or simultaneous localization and mapping (SLAM) methods heavily rely on geometric features of environments, as LiDARs primarily provide range measurements instead of motion measurements. From now on, however, the situation changes thanks to the novel Frequency Modulated Continuous Wave (FMCW) Doppler LiDARs. FMCW Doppler LiDARs not only offer the point range with high resolution but also capture the instant point Doppler velocity through the Doppler effect. In the letter, we propose FMCW-LIO, a novel and robust LIO, leveraging intrinsic Doppler measurements from FMCW Doppler LiDARs. To correctly exploit Doppler velocities, a motion compensation method is designed, and a Doppler-aided observation model is applied for on-manifold state estimation. Then, dynamic points can be effectively removed by the Doppler criteria, deriving more consistent geometric observations. FMCW-LIO eventually achieves accurate state estimation and static mapping, even in structure-degenerated environments. Extensive experiments in diverse scenes are performed and FMCW-LIO outperforms other algorithms on both accuracy and robustness.

Mingle Zhao, Jiahao Wang, Tianxiao Gao +2

The authors are with the University of Macau.

Inertial Odometry LiDAR Odometry

Sep 22, 2026cs.RO

Unsigned Distance Maps on 2D Point Cloud Registration

2D point cloud registration arises in laser odometry and Simultaneous Localization and Mapping (SLAM) for mobile robots. Iterative Closest Point (ICP) is one of the most widely used approaches. Still, its iterative procedure recomputes correspondences via nearest-neighbor search at every iteration, whereas correspondence-free alternatives focus on scan-to-map alignment. This paper proposes a 2D point cloud registration approach based on unsigned distance maps, precomputing the Euclidean distance to the nearest reference point, along with its spatial derivatives, over a discrete grid, replacing the per-iteration search with O(1) lookups. Moreover, point-to-point and point-to-plane error formulations are derived on the SE(2) manifold and solved via Gauss-Newton optimization. On a synthetic benchmark and the real-world IILABS 3D dataset, the precomputed point-to-point variant outperforms its analytical counterparts, achieving competitive laser-odometry drift compared to point-to-plane formulations, as the precomputed gradient regularizes correspondences in the presence of sensor noise.

Ricardo B. Sousa, Giorgio Grisetti, Héber Miguel Sobreira +2

Faculty of Engineering, University of Porto (FEUP). R. Dr. Roberto Frias, 4200-465 Porto, Portugal. · INESC TEC – Institute for Systems and Computer Engineering, Technology and Science. R. Dr. Roberto Frias, 4200-465 Porto, Portugal. · Sapienza University of Rome. Piazzale Aldo Moro 5, 00185 Roma RM, Italy. +1

LiDAR Odometry Point Cloud Registration

Sep 22, 2026cs.RO

MOLA LiDAR-Inertial Odometry (MOLA-LIO) on the COMFORT Localization Benchmark

This short report documents our entry to the COMFORT Localization Benchmark (IROS 2026), evaluated on the GrandTour dataset recorded with the Boxi payload. It extends MOLA-LO into a LiDAR-inertial system that also ingests IMU and, optionally, legged kinematic odometry. We describe the architecture, the streams consumed, the local protocol that selected the submitted configuration, and the measurements backing our real-time claim.

Jose Luis Blanco-Claraco

Engineering Department University of Almer´ıa, Spain

LiDAR Odometry Multimodal Sensor Fusion

Sep 15, 2026cs.RO

LiLi: Lie Theory Based 3D LiDAR Scan Alignment Degeneracy Detection

In this paper, we study 3D LiDAR scan alignment in challenging scenarios with degeneracies, such as straight corridors or flat fields, where the alignment solution is not unique and compromises localization and mapping accuracy. Existing degeneracy detection methods that neglect the potential for reassociating data points are prone to being sensitive to noise and complex degeneracies. Therefore, we propose LiLi - a novel method that leverages Lie theory to identify the full set of degenerate transformations within the SE(3) Lie group of rigid transformations. The method employs perturbations of the optimized solution and compares the resulting optimized poses to ensure robust detection of degeneracies. By leveraging generators from the Lie algebra se(3), the method provides a systematic approach to describing the set of degenerate transformations. Quantitative evaluations on synthetic data show significant improvement over the state-of-the-art Hessian-based method, reducing alignment error by 50%, with more significant improvements for datasets featuring noise. In the real-world degenerate datasets, the proposed method integrated into LiDAR-based odometry yields superior localization performance compared to the reference solution based on the Hessian-based degeneracy detector on a 260 m long trajectory, and succeeds on a 430 m long round-trip tunnel trajectory where the reference fails.

Vsevolod Hulchuk, Jan Bayer, Jan Faigl

Faculty of Electrical Engineering, Czech Technical University, Technická 2, 166 27, Prague, Czech Republic

LiDAR Odometry Lie Group Methods Point Cloud Registration LiDAR Localization

Sep 14, 2026cs.RO

Parameter Sensitivity Analysis for Aerial LiDAR-Inertial Odometries in low-altitude flights

LiDAR-based SLAM (Simultaneous Localization and Mapping) and LIO (LiDAR-inertial odometry) algorithms are often used for precise navigation of unmanned aerial vehicles, especially during interactions with the aerial robot's environment. However, the performance of these algorithms is greatly dependent on the scenario, LiDAR, and robot motion characteristics, often requiring an intensive tuning process to achieve the desired performance. To aid these tuning efforts, this paper analyzes the influence on performance of the parameters of an EKF-based LIO algorithm (FAST-LIO2) and the LIO module of a graph-based SLAM algorithm (Cartographer) on aerial LiDAR SLAM datasets recorded using different LiDARs in low-to-moderate-altitude flights in diverse environments. The analysis is conducted on the absolute trajectory error (ATE) resulting from processing the datasets with the LIO algorithms configured with each combination of parameters obtained in an exhaustive grid search. The relationship between individual parameters and the ATE results is assessed using Pearson's correlation, while the influence of each parameter is assessed using random forest permutation importance analyses with random forest models trained to predict the resulting ATE values based on the choice of parameters. The performed analysis obtains for Cartographer and FAST-LIO2: i) the identification of parameters with stronger influence in performance, ii) a simplified tuning procedure, and iii) tuning recommendations. Using the proposed tuning recommendations, both algorithms obtain on the analyzed datasets ATE values within 5 cm to the optimal performance found in the grid search procedure in 94% of the analyzed cases.

Robert Milijas, Jose Ramiro Martinez-de Dios, Stjepan Bogdan

CoE MARBLE - Centre of Excellence in Maritime Robotics and Technologies for Sustainable Blue Economy, Unska 3, Zagreb, 10000,, Croatia · GRVC Robotics Laboratory, Universidad de Sevilla, Camino de los Descubrimientos, Sevilla, 41092,, Spain · University of Zagreb Faculty of Electrical Engineering and Computing, Laboratory for Robotics and Intelligent Control Systems (LARICS), Unska 3, Zagreb, 10000,, Croatia

Simultaneous Localization and Mapping UAV Navigation LiDAR Odometry Hyperparameter Optimization

Sep 12, 2026cs.RO

Does Online Gravity Estimation Matter? Revisiting a Silent Design Split in LiDAR-Inertial Odometry

LiDAR-inertial odometry (LIO) systems differ in whether they continue estimating gravity after initialization. We compare four gravity-bias state configurations in each of FAST-LIO2 and LIO-SAM, then separately test a gravity-direction factor. Across 12 dataset sequences evaluated with FAST-LIO2, fixing gravity under continuous LiDAR correction produces mean paired changes in vertical and 3D position errors with 90% confidence intervals within

\pm 2\%

. Tests on 4 sequences with LIO-SAM likewise show no consistent benefit from online gravity. Multi-second LiDAR outages, unlike reduced range or field of view, reveal trajectory-dependent costs of fixing gravity. A history-matched 23D-to-21D switch places the repeatable 3D error increase after LiDAR updates resume. Under 5-s outages, a direction factor from the same IMU used for preintegration improves accuracy on Hall05 but worsens both errors with online gravity on TUHH. Dynamic-start tests also show fixed-bias failures at particular starting phases. We recommend keeping gravity and accelerometer bias online for robustness; use a direction factor only after verifying vertical and 3D accuracy gains under the intended operating conditions.

Jie Xu, Ziyi Jin, Kangjin Yu +5

Anyverse Dynamics

Inertial Odometry LiDAR Odometry

Aug 6, 2026cs.RO

KILVO: Kinematic-Inertial-LiDAR-Visual Odometry with Robust Multimodal Adaptation for Humanoid Robots

This article presents a kinematic-inertial-LiDAR-visual odometry for humanoid robots, called KILVO. Tailored to the platform features, requirements, and real-world complexity, it fully utilizes the sensors commonly equipped on humanoid robots, including joint encoders, IMU, LiDAR, and camera, within an asynchronous-sequential hybrid error-state iterated Kalman filter (ESIKF). Specifically, inertial data are used for prediction, leg kinematics are processed asynchronously at a high rate and provide proprioceptive constraints, while exteroception is updated sequentially, first by registering LiDAR points for geometric priors and then by updating the visual component via photometric errors. Moreover, the framework is elaborately designed with multimodal adaptation for resilience to sensor failures. A compact contact estimation module is also developed, sharing information with state estimation without additional sensors. Extensive experiments on public datasets and in the real world across multiple humanoid robots, gait patterns, and scenarios demonstrate that KILVO achieves highly competitive accuracy, efficiency, and output rates, with strong robustness against sensor degradation and failures, making it more suitable for humanoid robots than state-of-the-art fusion methods. Our code and datasets are released on GitHub.

Jixin Gao, Fucheng Liu, Teng Zhang +1

Kalman Filtering Inertial Odometry LiDAR Odometry Visual Odometry Robot State Estimation Multimodal Sensor Fusion

Jul 24, 2026cs.RO

Flight-Ready LiDAR-Inertial Odometry for Embedded Drone Platforms

Open-source LiDAR-inertial odometry (LIO) systems have achieved remarkable benchmark accuracy, yet current state-of-the-art implementations are primarily optimized for evaluation performance rather than the requirements of real-time closed-loop aerial control. When deployed onboard UAVs, this can introduce limitations that degrade flight performance. In this work, we identify five architectural deficiencies in a representative tightly coupled IESKF-based LIO implementation: odometry publishing tied to the LiDAR rate (10 Hz instead of the IMU's 200 Hz), missing velocity outputs, execution bottlenecks that block IMU processing, mutex contention, and synchronization race conditions. We introduce corresponding modifications including IMU-rate forward propagation, direct body-frame velocity publishing, SLERP-based smoothing, dual-executor isolation, and explicit synchronization protection. The resulting system increases odometry output from ~10 Hz to a stable 200 Hz, provides a complete Twist state at every IMU sample, and preserves continuity during transient LiDAR loss. Experiments on a Livox Mid-360 / Pixhawk 4 Mini autonomous UAV with motion-capture ground truth validate the approach. Since the underlying estimator (IESKF + ikd-Tree) remains unchanged, the proposed improvements can be directly applied to FAST-LIO2-derived implementations.

Alvaro J. Gaona, David Perez-Saura, Francisco J. Anguita +1

Computer Vision and Aerial Robotics Group at Centre for Automation and Robotics C.A.R. (UPM-CSIC), Universidad Politécnica de Madrid (CVAR-UPM), Calle Jose Gutierrez Abascal 2, 28006 Madrid, Spain

Inertial Odometry UAV Navigation LiDAR Odometry Robot Localization

Jul 15, 2026cs.RO

Improving Map Consistency in Graph-Based LiDAR SLAM Through Information-Aware Odometry and Retroactive Loop Closure

High-quality maps are fundamental for robotics tasks such as navigation and planning. Although modern graph-based LiDAR SLAM systems achieve good trajectory accuracies, a low trajectory error alone does not guarantee geometrically consistent maps, particularly at revisit locations where missed loop closures and residual drift can produce local misalignments. In this work, we address the problem of jointly improving global trajectory estimation and local map quality in 3D LiDAR SLAM. We first propose a framework to efficiently estimate geometry-dependent information matrices for ICP, enabling principled weighting of odometry constraints in a pose graph. We then introduce a hierarchical loop-closure module that decouples place recognition from geometric registration, together with a retroactive loop-closure module that exploits the optimized pose graph to recover missed loop closures. We also propose an evaluation protocol to measure map consistency at revisit locations. We evaluate our SLAM system on several datasets against state-of-the-art LiDAR SLAM systems. Experimental results demonstrate global trajectory accuracies on par with or better than existing methods while consistently improving local geometric map consistency at revisit locations. These results suggest that coupling uncertainty-aware odometry with geometry-guided loop-closure refinement leads to more accurate trajectories and higher-quality maps.

Saurabh Gupta, Niklas Trekel, Louis Wiesmann +1

University of Bonn, Center for Robotics · Lamarr Institute for Machine Learning and Artificial Intelligence

Pose Graph Optimization LiDAR Odometry Loop Closure Detection LiDAR Localization

Jul 15, 2026cs.RO

WNOJ-LIO: A White-Noise-on-Jerk Motion-Prior EKF for High-Dynamic LiDAR-IMU Fusion

LiDAR-inertial odometry (LIO) is a key component of autonomous navigation, but high-dynamic driving exposes two coupled challenges: intra-scan motion distortion and vibration-contaminated inertial measurements. Most real-time LiDAR-inertial pipelines propagate the system state by integrating raw IMU measurements and then use the propagated trajectory for point cloud de-distortion, thereby propagating inertial noise into both the corrected scan and the subsequent scan-to-map registration. This paper presents WNOJ-LIO, a LiDAR-IMU fusion framework based on a White-Noise-on-Jerk (WNOJ) Extended Kalman Filter (EKF). WNOJ-LIO employs a decoupled WNOJ prior on

\R^3 \times \SO(3)

for state prediction and treats the IMU as a high-frequency measurement source rather than the driver of state propagation. The resulting posterior state history is then used for LiDAR scan de-distortion and subsequent point-to-plane LiDAR updates. The decoupled process model enables closed-form covariance propagation, thereby bridging the gap between batch WNOJ Gaussian process (GP) trajectory priors and recursive filtering. Simulation results demonstrate improvements in acceleration and angular-velocity denoising, scan de-distortion, and localization accuracy over a FAST-LIO-style baseline. Real-world experiments were conducted using an autonomous racing car on four driving segments with maximum speeds ranging from 53 to 208~km/h, covering a wide range of vehicle vibration levels. The experiments further validate the proposed method and provide a comprehensive evaluation of its performance in estimating acceleration, angular velocity, body-frame linear velocity, attitude, and position under highly dynamic driving. The source code of WNOJ-LIO is publicly available at https://github.com/LvJohny/wnoj-ekf-lio.git.

Junning Lyu, Qizhi Guo, Xia Ning +2

School of Aerospace Engineering, Beijing Institute of Technology, Beijing 100081, China

Inertial Odometry LiDAR Odometry Robot State Estimation Multimodal Sensor Fusion Extended Kalman Filtering

Jul 10, 2026cs.RO

A Numerically-Robust ROS 2 Port of iG-LIO: Diagnosing and Fixing Toolchain-Induced Failures in Incremental GICP LiDAR-Inertial Odometry

iG-LIO is a tightly-coupled LiDAR-inertial odometry system fusing generalized-ICP and point-to-plane constraints in an iterated error-state Kalman filter over an incremental voxel map. We report an open-source ROS 2 Jazzy port of the original ROS 1 implementation and, more importantly, the diagnosis of environment-induced numerical failures that appear only after the port: a mechanically faithful migration -- estimation mathematics left unchanged -- compiled and ran, yet diverged with NaN internal values. Both causes trace to the modern ROS 2 toolchain, not the algorithm: a Quality-of-Service (QoS) mismatch that silently drops and reorders IMU samples, and an uninitialized parallel-reduce accumulator arising from the oneTBB + Eigen combination shipped with current distributions. We further correct Ouster point-field parsing to ensure correct point cloud undistortion with newer Ouster revisions, add Velodyne Velarray M1600 support, provide both a compile-time-gated Livox CustomMsg path and a driver-free path for Livox sensors publishing standard PointCloud2 (e.g. Mid-360), and expose the runtime via YAML. The result has been validated in an Ouster OS0 Rev7, an Ouster OS1 Rev 7, and a Livox MID-360. This report is a citable reference for the port itself, not a claim on the underlying algorithm [1]. The ROS 2 port of iG-LIO described in this document can be found at https://github.com/Forestry-Robotics-UC/ig_lio/tree/ros2-jazzy.

Afonso E. Carvalho, David Portugal, Paulo Peixoto

Robotics Institute (RI), Carnegie Mellon University (CMU), Pittsburgh, USA. · Institute of Systems and Robotics (ISR), University of Coimbra (UC), Coimbra, Portugal.

LiDAR Odometry Robotics

Jul 5, 2026cs.CV

Real-Time LiDAR Gaussian Splatting SLAM

We present a real-time LiDAR-based framework for Gaussian Splatting SLAM that tightly couples fast G-ICP registration with spherical rasterization-based dense mapping for large-scale sequences. Leveraging LiDAR geometry rather than appearance, we reuse tracking-estimated local covariances to initialize Gaussians with range-aware scales and to derive surface normals for geometry-aware map optimization. We further introduce a covariance-derived geometry score that measures local complexity and drives pruning in planar regions and selective densification in structurally rich areas, while optimized Gaussians and LiDAR-specific confidence cues are fed back to improve tracking robustness. On the Newer College dataset, our method achieves an F-score of 86.78% using purely online trajectories at real-time speed (

>

20 FPS), and additional experiments on other datasets confirm its stability and scalability.

Seungjun Tak, Yewon Jeon, Jaeik Hwang +3

Sungkyunkwan University, Suwon, South Korea

Simultaneous Localization and Mapping 3D Gaussian Splatting 3DGS SLAM LiDAR Odometry

Jun 29, 2026cs.RO

Self-supervised Geometry Reasoning for LiDAR Simultaneous Localization and Mapping

LiDAR simultaneous localization and mapping (SLAM) relies on local geometric quantities such as covariances, correspondences, and surface structures. However, most existing pipelines rely on hand-crafted estimates of local geometry and use them as fixed inputs to LiDAR SLAM, which can make the estimated local geometry noisy and unstable in sparse regions of a point cloud or when using low-resolution LiDAR. To address this issue, this paper introduces a self-supervised framework that learns an explicit symbolic representation of local geometry and uses it to improve LiDAR SLAM recursively. Specifically, each point is represented as a Gaussian distribution, allowing local geometry to be described by a covariance. Without dense geometry labels or ground-truth poses, the framework learns by maximizing the likelihood of local geometry, with self-supervision derived from consistency relations over symbolic geometric representations, including predicted covariances, correspondences, and trajectory from SLAM. The learned geometry is then fed back into LiDAR SLAM, forming a reciprocal loop in which improved geometry enhances localization and mapping, and improved localization provides cleaner supervision for subsequent geometry reasoning. This framework is backend-agnostic and can be plugged into existing LiDAR SLAM pipelines without architectural changes. Experiments on KITTI under varying LiDAR resolutions show that the proposed method improves both odometry and global registration.

Jiwoo Kim, Jinwoo Lee, Woojae Shin +2

Korea Advanced Institute of Science and Technology (KAIST), Daejeon, Republic of Korea · Daegu Gyeongbuk Institute of Science and Technology (DGIST), Daegu, Republic of Korea

Simultaneous Localization and Mapping Self-Supervised Learning LiDAR Odometry Geometric Representation Learning LiDAR Localization

Jun 24, 2026cs.RO

FAR-LIO: Enabling High-Speed Autonomy through Fast, Accurate, and Robust LiDAR-Inertial Odometry

Robust and accurate odometry estimation is essential in modern robotics. In environments characterized by highly dynamic motion and sensor noise, odometry estimation becomes increasingly challenging. Autonomous racing combines both factors in an unstructured setting, where minimizing odometry latency is essential for stable closed-loop control. This paper introduces FAR-LIO, a highly optimized CUDA-accelerated LiDAR-inertial odometry framework developed for Fast, Accurate, and Robust performance. Our system leverages a novel CUDA-based voxel hashmap to enable parallelized nearest-neighbor search and efficient map updates. We employ a sparsity-aware Generalized Iterative Closest Point algorithm with adaptive thresholding on top of the CUDA-based voxel hashmap with adaptive density to achieve low-latency without compromising accuracy. An Extended Kalman Filter serves as a robust backend. It utilizes an upsampling and delay compensation strategy to fuse the LiDAR odometry with high-frequency IMU data, thereby ensuring a robust and smooth odometry output. We evaluate FAR-LIO across four different sensor setups, using both public datasets and data from two autonomous racecars driving at speeds of up to 250 km/h. FAR-LIO achieves an average 6.9% reduction in the positional error and 38.4% lower runtime compared to state-of-the-art baselines on target hardware using a single parameter set. This demonstrates its computational efficiency and broad applicability. To build upon our work, our code is available open-source on https://github.com/TUMFTM/FAR-LIO.

Maximilian Leitenstern, Marcel Weinmann, Patrick Haft +3

Institute of Automotive Technology, School of Engineering and Design, Technical University of Munich, Germany · NVIDIA Corp., USA

Inertial Odometry LiDAR Odometry Point Cloud Registration

Jun 18, 2026cs.RO

Motor Angular Speed Preintegration for Multirotor UAV State Estimation

A precise state estimate is crucial for a tight feedback control that enables agile and near-obstacle flights of UAVs. The state-of-the-art methods fuse slow pose measurements with high-frequency inertial measurements to obtain a precise state estimate. However, the inertial measurements from the IMU onboard the UAV are degraded by vibrations from spinning propellers and the precision of the estimated state suffers. We propose a novel approach based on the preintegration of accelerations obtained from motor speeds. We show that the accelerations obtained in this manner can be used for state propagation on their own to achieve better precision without including the IMU. Further, we propose a factor composed of the preintegrated motor speeds that can be directly employed in factor graph optimization frameworks. We combine our factor with LiDAR measurements into the proposed Motor Angular Speed LiDAR Odometry (MAS-LO) algorithm for precise state estimation, which we open-source. Lastly, we evaluate the estimation precision against a state-of-the-art inertial algorithm LIO-SAM to show 28% improvement in position and 65% in velocity estimation accuracy, 14% lower measurement lag, and high robustness to wrong parameter values.

Matěj Petrlík, Filip Novák, Robert Pěnička +1

Department of Cybernetics, Faculty of Electrical Engineering, Czech Technical University in Prague, 166 36, Prague 6, Czech Republic

UAV Navigation LiDAR Odometry Robot State Estimation

Jun 8, 2026cs.CV

Generalized-CVO: Fast and Correspondence-Free Local Point Cloud Registration with Second Order Riemannian Optimization

We propose a fast and correspondence-free local point cloud registration method that leverages geometric surface structure and reproducing kernel Hilbert space (RKHS) embeddings. The method represents point clouds as continuous functions with point-wise anisotropic kernels that encode local geometry. This formulation improves alignment along surface normals while relaxing alignment along tangential directions. To solve the resulting registration problem, we propose a second-order on-manifold optimization scheme with approximate Riemannian Hessians, achieving a speedup of up to 10x over the first-order solvers used in prior correspondence-free RKHS-based methods. We demonstrate improved frame-to-frame LiDAR and RGB-D tracking accuracy across diverse indoor and outdoor datasets. On a LiDAR tracking registration task in the driving domain, we achieve a reduction of

>55\%

in both translational and rotational drift in challenging feature-sparse environments. On object registration benchmarks, we show improved robustness over ICP-based methods and further gains when refining global initialization, particularly under moderate misalignment.

Ray Zhang, Marcus Greiff, Thomas Lew +1

Toyota Research Institute, 4440 El Camino Real, Los Altos, California, USA

Riemannian Optimization LiDAR Odometry Kernel Methods Point Cloud Registration

Jun 2, 2026cs.RO

Semantic-weighted ICP for LiDAR Odometry: Class-Aware Residual Reweighting for Robust Scan Registration

LiDAR odometry is a fundamental component of autonomous robotic systems, relying on geometric registration between consecutive point clouds to estimate ego-motion. However, traditional geometric approaches often degrade in dynamic or unstructured environments due to unreliable correspondences caused by moving objects, sparse geometric features, vegetation, and semantically ambiguous structures. Existing works have shown that, some of these limitations can be addressed by introducing semantic information from the environment in the registration process. In this work, we build on this, and show that not all elements in the environment are equally relevant for registration. Hence, we propose a semantic class-weighted ICP for LiDAR odometry. Instead of strictly filtering out points belonging to specific semantic classes, the proposed approach weights the residuals of points belonging to semantic categories based on their expected geometric stability. This strategy enables informative but potentially unstable structures, to contribute to the registration process while mitigating the influence of dynamic objects. The experimental evaluation was conducted on the SemanticKITTI and RELLIS-3D datasets, which include urban, highway, rural, and off-road environments. The empirical results show that the proposed Semantic-weighted ICP improves pose estimation, especially in challenging off-road scenarios where conventional rigid features are scarce. Furthermore, the analysis reveals that the effectiveness of this weighting strategy is highly environment-dependent, influenced by the structural and semantic composition of the scene.

Vasco Carvalho, Tiago Barros, Urbano J. Nunes

Institute of Systems and Robotics University of Coimbra Coimbra, Portugal

LiDAR Odometry Point Cloud Registration

May 23, 2026cs.RO

Elevator-LIO: Robust LiDAR-Inertial Odometry for Multi-Floor Navigation under Elevator-Induced Non-Inertial Motion

This paper presents Elevator-LIO, a LiDAR-inertial odometry framework designed to achieve continuous robot localization during elevator travel, thereby supporting cross-floor robotic tasks. To address the state-estimation problem in non-inertial frames, Elevator-LIO establishes a decoupled state-estimation model that separately models the robot motion relative to the elevator and the elevator motion itself, and embeds it into a mode-dependent iterated error-state Kalman filter framework. This framework degenerates to conventional LIO estimation in ordinary indoor environments, while enabling the propagation and constrained update of elevator-related states in elevator non-inertial environments, thereby achieving continuous and stable localization. An elevator mode manager detects elevator entry and exit events using LiDAR ranging statistics and estimated states, and introduces event-triggered zero-velocity and zero-acceleration updates when the elevator stops to suppress accumulated vertical drift. In addition, this paper adopts an adaptive voxel downsampling strategy to maintain a stable number of effective points under significant environmental scale changes. We conduct extensive experiments on 20 real-world sequences containing 79 elevator rides, including practical challenges such as large-scale spaces, long vertical travel, dynamic pedestrian interference, and mirror reflections. The results show that Elevator-LIO maintains continuous localization accuracy in all sequences, with terminal height error below 1 cm in 17 sequences. In contrast, existing representative localization systems perform poorly on these elevator sequences. Tests on the Hilti 2022/2023 datasets further show that the proposed method remains competitive in standard indoor scenarios. The project page is available at https://xiaofan4122.github.io/Elevator_LIO_Page/.

Yifan Zhang, Yudong Huang, Yuchong Zhang +4

Shanghai Jiao Tong University, Shanghai, China

Kalman Filtering Inertial Odometry Robot Navigation LiDAR Odometry Robot Localization

May 20, 2026cs.RO

EllipseLIO: Adaptive LiDAR Inertial Odometry with an Ellipsoid Representation

LiDAR Inertial Odometry (LIO) is a critical component for many mobile robots that need to navigate without relying on external positioning (e.g., GPS). Platforms that operate autonomously in different environments and with heterogeneous LiDAR sensors require a LIO approach that can adapt to these different scenarios without human intervention. Existing LIO approaches can typically provide reliable and accurate odometry in scenarios with similar environments and sensors when suitably tuned. However, many approaches struggle to retain robust odometry across heterogeneous environments and sensors while using a consistent configuration. This paper presents EllipseLIO, a real-time LIO approach that generalises between scenarios by using methods for LiDAR scan filtering and registration that adapt to the sensor capabilities and environment without requiring scenario-specific tuning. Experiments with EllipseLIO and state-of-the-art LIO approaches on five datasets with diverse and challenging scenarios demonstrate that EllipseLIO is the best-performing approach overall. It achieves a 38% lower odometry error on average than the second-best approach and is the only approach that does not diverge in any experiment. An open-source version of EllipseLIO will be available at github.com/v4rl-ucy/ellipselio.

Rowan Border, Margarita Chli

Vision for Robotics Lab (V4RL), University of Cyprus, Cyprus · ETH Zurich, Switzerland

Inertial Odometry Mobile Robotics LiDAR Odometry Point Cloud Registration

May 17, 2026cs.RO

Stretch-ICP: A Continuous-Trajectory Registration and Deskewing Algorithm in Scenarios of Aggressive Motions

Robust robotic autonomy remains challenging in complex environments, where loss of stability on uneven or slippery terrain can induce extreme accelerations and angular velocities. Such motions corrupt sensor measurements and degrade state estimation, motivating the need for improved algorithmic robustness. To investigate this issue, we introduce the Tumbling-Induced Gyroscope Saturation (TIGS) dataset, which consists of recordings from a mechanical lidar and an Inertial Measurement Unit (IMU) tumbling down a hill. The dataset contains angular speeds up to four times higher than those in similar datasets and is publicly available. We then propose two complementary methods to improve Simultaneous Localization And Mapping (SLAM) robustness and evaluate them on TIGS. First, Saturation-Aware Angular Velocity Estimation (SAAVE) estimates angular velocities when gyroscope measurements become saturated during aggressive motions, reducing angular speed estimation error by 83.4%. Second, Stretch-ICP, a novel registration and deskewing algorithm, enables reconstruction of smoother 6-Degrees Of Freedom (DOF) trajectories under aggressive motions compared to classical Iterative Closest Point (ICP). Stretch-ICP reduces linear and angular velocity errors by 95.2% and 94.8%, respectively, at scan boundaries. Together, these contributions improve the robustness and consistency of lidar-inertial state estimation under aggressive motions.

Simon-Pierre Deschênes, Veronica Vannini, Philippe Giguère +1

Northern Robotics Laboratory, Université Laval, Quebec City, QC G1V 0A6, Canada

Simultaneous Localization and Mapping LiDAR Odometry

May 14, 2026cs.RO

SOCC-ICP: Semantics-Assisted Odometry based on Occupancy Grids and ICP

Reliable pose estimation in previously unseen environments is a fundamental capability of autonomous systems. Existing LiDAR odometry methods typically employ point-, surfel-, or NDT-based map representations, which are distinct from the semantic occupancy grids commonly used for downstream tasks such as motion planning. We introduce SOCC-ICP, a semantics-assisted odometry framework that jointly performs Semantic OCCupancy grid mapping and LiDAR scan alignment. Each map voxel encodes geometric and semantic statistics, enabling adaptive point-to-point or point-to-plane ICP based on local planarity. Further, the occupancy grid naturally filters dynamic objects through raycasting-based free-space updates. Across diverse evaluation scenarios, SOCC-ICP achieves performance competitive with state-of-the-art LiDAR odometry and remains robust in geometrically degenerate environments, even in the absence of semantic cues. When semantic labels are available, integrating them into map construction, downsampling, and correspondence weighting yields further accuracy gains. By unifying odometry and semantic occupancy grid mapping within a single representation, SOCC-ICP eliminates redundant map structures and directly provides a map suitable for downstream robotic applications.

Johannes Scherer, Sebastian Hirt, Henri Meeß

Fraunhofer IVI, 85049 Ingolstadt, Germany · Technische Hochschule Ingolstadt, 85049 Ingolstadt, Germany · Ancud IT-Beratung GmbH, 90478 Nürnberg, Germany

LiDAR Odometry Semantic Occupancy Mapping Point Cloud Registration Robot Localization

May 10, 2026cs.RO

Safety-Critical LiDAR-Inertial Odometry with On-Manifold Deterministic Protection Level

In safety-critical scenarios, the protection level of the autonomous navigation system is crucial for enabling mobile robots to perform safe tasks. However, existing studies on probabilistic navigation systems for robots usually perform offline accuracy evaluations using limited datasets and assume that the results can be applied to unknown real-world environments. As a result, current autonomous mobile robots often lack protection levels for online safety assessment. To fill this gap, we propose a safety-critical LiDAR-inertial odometry (LIO) that provides deterministic protection levels based on on-manifold deterministic state estimation. By adopting the unknown but bounded assumption, we derive a neat closed-form relationship between point cloud noise and the uncertainty of the estimation from the iterated closest point algorithm. Using this relationship, we design an on-manifold ellipsoidal set-membership filter and implement it within the LIO system. Leveraging the properties of the set-membership filter, our system offers the feasible sets of the estimated locations as the deterministic protection levels, serving as safety references for the robots' downstream autonomous operations. The experimental results show that our system can provide effective deterministic online safety references for diverse robots in various environments.

Yueqi Zhu, Yan Pan, Chufan Rui +3

School of Automation, Southeast University and Key Laboratory of Measurement and Control of CSE, Ministry of Education, Nanjing, China

LiDAR Odometry Robot State Estimation

Apr 17, 2026cs.RO

Environment-Adaptive Solid-State LiDAR-Inertial Odometry

Solid-state LiDAR-inertial SLAM has attracted significant attention due to its advantages in speed and robustness. However, achieving accurate mapping in extreme environments remains challenging due to severe geometric degeneracy and unreliable observations, which often lead to ill-conditioned optimization and map inconsistencies. To address these challenges, we propose an environment-adaptive solid-state LiDAR-inertial odometry that integrates local normal-vector constraints with degeneracy-aware map maintenance to enhance localization accuracy. Specifically, we introduce local normal-vector constraints to improve the stability of state estimation, effectively suppressing localization drift in degenerate scenarios. Furthermore, we design a degeneration-guided map update strategy to improve map precision. Benefiting from the refined map representation, localization accuracy is further enhanced in subsequent estimation. Experimental results demonstrate that the proposed method achieves superior mapping accuracy and robustness in extreme and perceptually degraded environments, with an average RMSE reduction of up to 12.8% compared to the baseline method.

Zhi Zhang, Chalermchon Satirapod, Bingtao Ma +1

School of Automation Chongqing University of Posts and Telecommunications Chongqing, China · Department of Survey Engineering, Faculty of Engineering Chulalongkorn University Bangkok, Thailand · School of Cyberspace Security Hangzhou Dianzi University Hangzhou, China

Simultaneous Localization and Mapping LiDAR Odometry

Apr 15, 2026cs.RO

BIEVR-LIO: Robust LiDAR-Inertial Odometry through Bump-Image-Enhanced Voxel Maps

Reliable odometry is essential for mobile robots as they increasingly enter more challenging environments, which often contain little information to constrain point cloud registration, resulting in degraded LiDAR-Inertial Odometry (LIO) accuracy or even divergence. To address this, we present BIEVR-LIO, a novel approach designed specifically to exploit subtle variations in the available geometry for improved robustness. We propose a high-resolution map representation that stores surfaces as voxel-wise oriented height images. This representation can directly be used for registration without the calculation of intermediate geometric primitives while still supporting efficient updates. Since informative geometry is often sparsely distributed in the environment, we further propose a map-informed point sampling strategy to focus registration on geometrically informative regions, improving robustness in uninformative environments while reducing computational cost compared to global high-resolution sampling. Experiments across multiple sensors, platforms, and environments demonstrate state-of-the-art performance in well-constrained scenes and substantial improvements in challenging scenarios where baseline methods diverge. Additionally, we demonstrate that the fine-grained geometry captured by BIEVR-LIO can be used for downstream tasks such as elevation mapping for robot locomotion.

Patrick Pfreundschuh, Turcan Tuna, Cedric Le Gentil +3

Autonomous Systems Lab, ETH Zürich, Switzerland · Robotic Systems Lab, ETH Zürich, Switzerland · Mobile Robotics Lab, ETH Zürich, Switzerland

Inertial Odometry LiDAR Odometry Point Cloud Registration

Mar 17, 2026cs.RO

GenZ-LIO: Generalizable LiDAR-Inertial Odometry Beyond Confined--Open Boundaries

For field robotic missions such as inspection, search-and-rescue, and exploration, light detection and ranging (LiDAR)-inertial odometry (LIO) can serve as a core component of autonomy by providing localization and mapping in GNSS-denied or unstructured environments. However, transitions between confined and open spaces, which are commonly encountered in field deployments, can induce substantial changes in scan density and local geometric structure, thereby reducing the robustness and computational efficiency of LIO. To address these issues, we present GenZ-LIO, a generalizable LIO framework designed to adapt to variations in spatial scale across confined and open environments. GenZ-LIO comprises three components: (i) scale-aware adaptive voxelization for regulating scan downsampling across spatial-scale changes, (ii) hybrid-metric state update for combining point-to-plane and point-to-point residuals under varying geometric structure, and (iii) voxel-pruned correspondence search for efficient point-to-point matching. We conduct a comprehensive evaluation using 42 sequences from nine public datasets and our newly collected NarrowWide dataset to analyze LIO performance under spatial-scale variations across diverse field scenarios. Across the evaluated sequences, GenZ-LIO maintains stable odometry estimation without divergence, indicating practical robustness under the tested field conditions. Our code is available at https://github.com/cocel-postech/genz-lio .

Daehan Lee, Hyungtae Lim, Seongjun Kim +7

Computational Control Engineering Laboratory (CoCEL), Department of Convergence IT Engineering and Electrical Engineering, Pohang University of Science and Technology (POSTECH), Pohang 37673, South Korea · Laboratory for Information & Decision Systems (LIDS), Massachusetts Institute of Technology, Cambridge, MA 02139, USA

Inertial Odometry Mobile Robotics LiDAR Odometry

Mar 10, 2026cs.RO

Degeneracy-Resilient Teach and Repeat for Geometrically Challenging Environments Using FMCW Lidar

Teach and Repeat (T&R) topometric navigation enables robots to autonomously repeat previously traversed paths without relying on GPS, making it well suited for operations in GPS-denied environments such as underground mines and lunar navigation. State-of-the-art T&R systems typically rely on iterative closest point (ICP)-based estimation; however, in geometrically degenerate environments with sparsely structured terrain, ICP often becomes ill-conditioned, resulting in degraded localization and unreliable navigation performance. To address this challenge, we present a degeneracy-resilient Frequency-Modulated Continuous-Wave (FMCW) lidar T&R navigation system consisting of Doppler velocity-based odometry and degeneracy-aware scan-to-map localization. Leveraging FMCW lidar, which provides per-point radial velocity measurements via the Doppler effect, we extend a geometry-independent, correspondence-free motion estimation to include principled pose uncertainty estimation that remains stable in degenerate environments. We further propose a curvature-enhanced degeneracy-aware localization method that leverages per-point curvature for improved data association and adaptive registration formulation, and unifies translational and rotational scales to enable consistent degeneracy detection. Closed-loop field experiments spanning environments with varying structural richness demonstrate that the proposed system reliably completes autonomous navigation, including in a challenging flat airport test field where a conventional ICP-based system fails. We release the implementation of this work at: https://opensource_code/place_holder.

Katya M. Papais, Wenda Zhao, Timothy D. Barfoot

Institute for Aerospace Studies, University of Toronto, Canada

Embodied Navigation Robot Navigation LiDAR Odometry LiDAR Localization Robot Localization

Mar 9, 2026cs.RO

FoMo: A Multi-Season Dataset for Robot Navigation in Forêt Montmorency

The Forêt Montmorency (FoMo) dataset is a comprehensive multi-season data collection, recorded over the span of one year in a boreal forest. Featuring a unique combination of on- and off-pavement environments with significant environmental changes, the dataset challenges established odometry and SLAM pipelines. Some highlights of the data include the accumulation of snow exceeding 1 m, significant vegetation growth in front of sensors, and operations at the traction limits of the platform. In total, the FoMo dataset includes over 64 km of six diverse trajectories, repeated during 12 deployments throughout the year. The dataset features data from one rotating and one hybrid solid-state lidar, a Frequency Modulated Continuous Wave (FMCW) radar, full-HD images from a stereo camera and a wide lens monocular camera, as well as data from two IMUs. Ground Truth is calculated by post-processing three GNSS receivers mounted on the Uncrewed Ground Vehicle (UGV) and a static GNSS base station. Additional metadata, such as one measurement per minute from an on-site weather station, camera calibration intrinsics, and vehicle power consumption, is available for all sequences. To highlight the relevance of the dataset, we performed a preliminary evaluation of the robustness of a lidar-inertial, radar-gyro, and a visual-inertial localization and mapping techniques to seasonal changes. We show that seasonal changes have serious effects on the re-localization capabilities of the state-of-the-art methods. The dataset and development kit are available at https://fomo.norlab.ulaval.ca.

Matěj Boxan, Gabriel Jeanson, Alexander Krawciw +5

Norlab, Universit´e Laval, Quebec City, Canada · University of Toronto Robotics Institute, Toronto, Canada

Simultaneous Localization and Mapping Robotic Data Collection Robot Navigation LiDAR Odometry Robot Localization