PaperlayerView exact revision on arXiv

Source-linked AI summary

CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure

Pierre Dellenbach, Jean-Emmanuel Deschaud, Bastien Jacquet, François Goulette

arXiv:2109.12979v2cs.ROcs.CV

TL;DR

LiDAR odometry must account for motion during scan acquisition while remaining robust to rapid changes, and loop detection remains difficult for complete SLAM. CT-ICP combines elastic continuous-time scan matching with discontinuities between scans and adds LiDAR loop closure, achieving 0.09% RTE on KITTI-CARLA versus 0.81% for the best other odometry and 0.59% RTE on the KITTI benchmark.

  • Problem

    Existing LiDAR odometry can struggle with large direction changes, fast acceleration, or motion irregularities, while LiDAR loop detection remains difficult and costly.

  • Method

    CT-ICP elastically models a continuous-time trajectory within each scan using two poses while allowing discontinuities between scans, then integrates fast loop detection into pose-graph SLAM.

  • Results

    0.09% RTE for CT-ICP versus 0.81% for the best of three other odometries on KITTI-CARLA, and 0.59% RTE on the KITTI benchmark.

  • Takeaways & Limitations

    CT-ICP provides real-time LiDAR-only odometry and complete SLAM evaluated across seven datasets spanning driving and high-frequency motion scenarios.

Abstract

from arXiv · show

Multi-beam LiDAR sensors are increasingly used in robotics, particularly with autonomous cars for localization and perception tasks, both relying on the ability to build a precise map of the environment. For this, we propose a new real-time LiDAR-only odometry method called CT-ICP (for Continuous-Time ICP), completed into a full SLAM with a novel loop detection procedure. The core of this method, is the introduction of the combined continuity in the scan matching, and discontinuity between scans. It allows both the elastic distortion of the scan during the registration for increased precision, and the increased robustness to high frequency motions from the discontinuity. We build a complete SLAM on top of this odometry, using a fast pure LiDAR loop detection based on elevation image 2D matching, providing a pose graph with loop constraints. To show the robustness of the method, we tested it on seven datasets: KITTI, KITTI-raw, KITTI-360, KITTI-CARLA, ParisLuco, Newer College, and NCLT in driving and high-frequency motion scenarios. Both the CT-ICP odometry and the loop detection are made available online. CT-ICP is currently first, among those giving access to a public code, on the KITTI odometry leaderboard, with an average Relative Translation Error (RTE) of 0.59% and an average time per scan of 60ms on a CPU with a single thread.

I. INTRODUCTION

CT-ICP introduces real-time LiDAR odometry that elastically models motion within each scan while allowing discontinuities between scans. The method extends this odometry into SLAM with loop detection and a pose-graph backend.

  • Motivation: The discontinuity between scans targets large direction or velocity changes that constant-velocity and smoothing-based continuous-time methods do not handle well.Earlier approaches either assume constant velocity or smooth high-frequency motions, potentially reducing robustness or precision.
  • Core method: CT-ICP models continuous pose evolution within a scan and discontinuous poses between adjacent scans.The trajectory uses two poses per scan, with a proximity constraint between consecutive scans.
  • Core method: Elastic scan-to-map registration jointly optimizes scan-start and scan-end poses, interpolating point poses by timestamp.This elastically deforms the current scan to align it with the map.
  • System and evaluation: The system combines a sparse-voxel dense local map with experiments across seven datasets and driving and high-frequency-motion scenarios.The experiments and code are intended to be reproducible with public, permissive open-source implementations.
  • SLAM system: The complete SLAM system adds fast LiDAR loop detection and a pose-graph backend to the odometry.The loop-detection method is integrated into pyLiDAR-SLAM.

II. RELATED WORK

Prior LiDAR odometry commonly uses ICP-based scan-to-map registration, but fixed distortion and smooth continuous-time models have limitations under rapid motion. CT-ICP instead combines intra-scan continuity with inter-scan discontinuity, while LiDAR loop closure remains difficult because existing approaches can be costly or sensitive to drift and environmental change.

  • ICP-based odometry: ICP and point-to-plane ICP underpin many LiDAR odometry methods, which commonly register scans against a map.Representations include range images, surfels, keypoints, and voxel grids.
  • Continuous-time motion modeling: Fixed distortion based on a constant-velocity model is effective in many driving scenarios but is not robust to sudden orientation changes or fast acceleration.The distortion remains fixed during ICP iterations.
  • Continuous-time motion modeling: Continuous-time trajectories using multiple poses or splines can model scan motion, but earlier formulations were not real time or could smooth high-frequency motion.Real-time methods introduced before CT-ICP include Elastic LiDAR Fusion and MARS LiDAR odometry.
  • CT-ICP: CT-ICP keeps the trajectory continuous during each scan but discontinuous between scans to compensate for motion irregularities that interpolation cannot capture.Two poses parameterize each scan, and the beginning pose need not equal the previous scan’s ending pose.
  • Loop closure: LiDAR loop detection remains challenging because direct registration suits only small, low-drift trajectories, individual-scan recognition is environment-sensitive, and global alignment is costly.These constraints limit opportunities to discover loops across longer or changing environments.

III. CT-ICP ODOMETRY

CT-ICP models each LiDAR scan with continuous-time interpolation between optimized start and end poses, while allowing discontinuity between scans to better handle rapid motion. Its elastic scan-to-map registration jointly estimates these poses and uses geometric residuals, planarity weighting, and motion-consistency constraints.

  • Trajectory formulation: CT-ICP parameterizes each scan with optimized poses at its beginning and end, interpolating sensor poses for timestamped measurements.The interpolation transforms points from the LiDAR frame to the world frame and elastically deforms the scan during registration.
  • Trajectory formulation: The method enforces continuity within scans but only proximity between adjacent scans, preserving robustness to high-frequency sensor motion.A proximity constraint keeps the previous scan’s end pose close to the current scan’s beginning pose without requiring full inter-scan continuity.
  • Registration objective: CT-ICP registers sampled scan keypoints against a dense local map, jointly optimizing two poses before transforming and inserting the scan’s points.The local map is built from previously registered scans and stored in a sparse voxel grid.
  • Registration objective: The scan-to-map objective uses robust point-to-plane ICP residuals weighted by neighborhood planarity.Normals and planarity weights are computed from the dense local map and refined at each ICP iteration.
  • Regularization and optimization: Location and velocity constraints limit inter-scan discontinuity and rapid acceleration, with iterations stopping at a parameter-step threshold or five iterations.The experiments use βl = βv = 0.001; typical stopping thresholds are 0.1 cm in translation and 0.01° in rotation.

B. Local map and robust profile

CT-ICP uses a dense local point-cloud map stored in sparse voxels for fast neighborhood access, supplemented by a robust profile for difficult registrations and rapid orientation changes.

  • Local map: The local map stores world-frame points in a sparse voxel grid, providing constant-time neighborhood access instead of kd-tree logarithmic access.Voxel size controls both the neighborhood-search radius and the map’s level of detail.
  • Local map: Each voxel stores up to 20 points separated by at least 10 cm, limiting redundancy from dense measurements along scan lines.Once a voxel is full, additional points are not inserted.
  • Local map: For each point, CT-ICP selects k = 20 nearest map neighbors from the 27 surrounding voxels to compute normals and planarity weights.After registration, scan points are added to the map, while distant voxels are removed based on distance from the latest scan center.
  • Robust profile: The robust profile detects fast orientation changes and registration failures, then retries with more keypoints and a larger neighborhood search.For orientation changes of ≥5°, it avoids inserting the new scan into the map, trading increased robustness for runtime.

IV. LOOP CLOSURE AND BACK-END

The loop-closure system converts aggregated local-map windows into elevation images, matches them with robust 2D features, and adds validated constraints to a pose-graph backend. Its current formulation assumes mostly planar sensor motion and ground-aligned calibration.

  • Loop detection: The loop module aggregates windows of Nmap scans into point clouds and projects each window into a 2D elevation grid.Each pixel retains the point with maximum elevation, which is clipped between zmin and zmax to form an elevation image.
  • Loop detection: New elevation images are matched against stored images using rotation-invariant features, RANSAC, and an inlier threshold.Validated matches receive Open3D ICP refinement on the elevation-grid point clouds to produce accurate 6-DoF loop constraints.
  • Pose-graph backend: The pose graph adds odometry constraints continuously but globally optimizes the trajectory only after detecting a loop constraint.The loop-closure trajectory is updated after pose-graph optimization.
  • Scope boundary: The procedure currently requires mostly planar ground motion and extrinsic calibration with the z axis aligned to the ground normal.Knowing gravity or the local ground plane could address this projection limitation.

V. EXPERIMENTS

The paper evaluates CT-ICP across seven datasets covering driving and high-frequency motion scenarios, using only per-point geometry and timestamps for elastic scan distortion.

  • Datasets: CT-ICP is evaluated on KITTI, KITTI-raw, KITTI-360, KITTI-CARLA, ParisLuco, Newer College Dataset, and NCLT.The evaluation spans both driving and high-frequency motion scenarios.
  • Inputs: The method requires only xyz geometry and per-point timestamps to elastically distort scans.For KITTI-raw and KITTI-360, timestamps are estimated linearly from azimuth angles.

1) Driving scenarios:

The driving-scenario evaluation uses several LiDAR datasets spanning corrected and uncorrected scans, synthetic motion, and varied acquisition setups.

  • KITTI provides 11 Velodyne HDL64 car sequences with GPS/IMU ground truth and motion-corrected benchmark scans.
  • KITTI-raw retains uncorrected scans, while KITTI-360 contains eight much longer uncorrected sequences ranging from 3000 to 15000 scans.
  • KITTI-corrected, KITTI-raw, and KITTI-360 receive an intrinsic angle correction of 0.205 ◦ for all scans.
  • KITTI-CARLA contains seven synthetic 5000-scan sequences with simulated 64-channel LiDAR, relative acquisition motion, precise ground truth, and timestamps.
  • ParisLuco is a 4 km, 12751-scan central-Paris sequence acquired with a vertically mounted Velodyne HDL32, with translation-only GPS/IMU-derived ground truth.

2) High-frequency motion scenarios:

The high-frequency-motion evaluation targets unstable platforms, long sequences, and abrupt orientation changes, where CT-ICP is compared using KITTI-style drift metrics.

  • NCLT contains 27 sequences of at least 20000 scans from a Velodyne HDL32 on a two-wheeled Segway, whose abrupt rotations challenge classical ICP odometry.
  • NCD contains approximately 15000- and 26000-scan handheld Ouster 64-channel sequences collected across Oxford on a stick-mounted sensor.
  • RTE averages trajectory drift over segments from 100 m to 800 m, with multi-sequence averages computed across all segments.
  • On KITTI-corrected, methods are very close, whereas competing methods degrade strongly on KITTI-raw and KITTI-360 while CT-ICP remains near corrected-scan performance.
  • 0.59% RTE placed CT-ICP first among methods with public code on the KITTI odometry benchmark.
  • 0.09% RTE for CT-ICP versus 0.81% for the best competing odometry was reported on KITTI-CARLA.
  • CT-ICP overtook IMLS-SLAM on ParisLuco despite using a much sparser map.
  • NCD results were comparable to KITTI, while NCLT produced much lower RTE than pyLiDAR F2M across challenging edge cases.

C. Loop closure experiments

The loop-closure evaluation measures global trajectory changes with ATE and reports loop detections across one sequence per dataset.

  • CT-ICP+LC uses elevation-grid matching with Nmap = 100 and Noverlap = 30 across all datasets.
  • Loop-closure quality is evaluated with ATE because RTE can deteriorate after loop closure; trajectories are first aligned by a best rigid transform.
  • Table II reports ATE, detected-loop counts, and LiDAR-odometry results for one sequence from each dataset.
  • Loop closure greatly improves ATE on KITTI-raw, KITTI-360, and ParisLuco, with a stronger effect on the longer KITTI-360 sequence.
  • KITTI-CARLA changes little after loop closure because CT-ICP already achieves 21 cm absolute trajectory precision in its simple geometry.
  • The method detects many loop constraints, with counts controlled by elevation-grid frequency through Noverlap and Nmap.

VI. CONCLUSION

CT-ICP is a real-time LiDAR-only odometry method evaluated across seven datasets spanning driving and high-frequency motion scenarios. The authors release code and datasets for reproducibility while identifying backend extension as future work.

  • CT-ICP goes beyond the state of the art on seven datasets covering driving and high-frequency motion scenarios.
  • The method elastically distorts each new scan during optimization to compensate for motion during acquisition.
  • The authors publish code and datasets to support reproduction of the reported results.
  • Future work will extend the continuous formulation beyond scan matching and improve use of the loop-closure procedure in the backend.
Loading 2109.12979v2…