Graph-based Online Lidar Odometry with Retrospective Map Refinement

Lidar-only odometry aims to estimate the trajectory of a mobile platform from a stream of lidar scans. Traditional scan-to map approaches register each scan against a single, evolving map, which propagates registration errors over time. To mitigate this, we propose a multitude-of-maps approach where the current scan is registered against multiple overlapping submaps instead of a single static map. By optimizing the resulting constraints in a pose graph, our method enables not only precise estimation of the current pose but also retrospective refinement of the submaps’ anchor points, which improves short-term consistency and long-term accuracy. We demonstrate that our approach achieves competitive and often superior accuracy on a variety of automotive datasets while maintaining real-time performance. Ablation studies confirm the critical role of multiple registrations and retrospective refinement of the map as core factors for our accuracy gains. Code and raw results are available on our public GitHub at https://github.com/Fusion-Goettingen/IROS_2026_Kurda_Graph.

💡 Research Summary

This paper tackles a fundamental limitation of lidar‑only odometry: the accumulation of registration errors when each incoming scan is aligned against a single, ever‑growing map. The authors propose a “multitude‑of‑maps” strategy in which the current scan is independently registered to several overlapping sub‑maps (keyframes). Each registration, performed with a robust ICP variant, yields a relative pose correction and an information matrix. These N constraints are inserted into a pose‑graph that contains the current pose and all previous keyframe poses. By optimizing this graph, the method simultaneously obtains an optimal estimate for the current pose and retrospectively refines the anchor poses of the sub‑maps, thereby reducing error propagation.

Key technical components include: (1) a two‑stage voxel‑hash preprocessing that first creates a coarse voxelized cloud for map updates (0.4 m voxels) and then a finer subsample (1.8 m voxels) for ICP; (2) a constant‑velocity motion model that predicts the current pose and serves as the initial guess for each ICP registration; (3) a robust ICP implementation using nearest‑neighbour search within 3 m, Gauss‑Newton optimization, and a Huber‑like kernel to mitigate outliers, with the Hessian used as an approximation of the registration covariance; (4) a gating mechanism in the graph optimizer that discards constraints whose residual exceeds a threshold equal to the map voxel size, protecting the solution from poorly overlapped registrations; (5) a lightweight pose‑graph formulation where each sub‑map contributes only a single edge per scan, keeping the graph sparse and enabling fast Gauss‑Newton optimization (typically under five iterations); and (6) a keyframe insertion and marginalization scheme that adds a new keyframe when the vehicle has moved more than 6 m, otherwise merges the constraints of the current scan into a single composite edge, preserving information while preventing graph growth.

The method is evaluated on three public automotive lidar datasets—KITTI, MulRan, and the Odyssey dataset—using standard metrics such as Relative Pose Error (RPE) and the KITTI evaluation suite. Results show that the multi‑map registration alone improves accuracy by roughly 15–30 % compared with single‑map baselines, especially in feature‑poor environments like tunnels. Adding retrospective refinement further reduces long‑term drift; for example, in KITTI loop‑closure sequences the translational drift drops below 0.12 % per 100 m. The system runs in real time, processing scans at >10 Hz on a typical CPU, with graph optimization accounting for less than 5 % of total compute time. Compared to the recent FORM approach, which constructs a densely connected graph with a constraint for every point‑to‑point or point‑to‑plane correspondence, the proposed method achieves comparable or superior accuracy while using far fewer constraints, resulting in significantly lower computational load.

The authors acknowledge limitations: sub‑map selection is based solely on spatial proximity, which may introduce noisy constraints in highly dynamic scenes; the approach relies exclusively on lidar, so extreme rotations or high‑speed motion could challenge the constant‑velocity predictor and ICP convergence. Future work is suggested to incorporate dynamic‑object filtering, adaptive sub‑map management, and multi‑sensor fusion (e.g., IMU, GNSS) to further improve robustness and accuracy.

In summary, this work presents a novel, lightweight graph‑based lidar odometry framework that blends online pose estimation with retrospective map refinement. By leveraging multiple overlapping sub‑maps and a sparse pose‑graph, it achieves state‑of‑the‑art accuracy on several benchmark datasets while maintaining real‑time performance, thereby advancing the practical applicability of lidar‑only odometry for autonomous navigation.