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

Authors: Pierre Dellenbach, Jean-Emmanuel Deschaud, Bastien Jacquet, François Goulette · ICRA 2022

Licences

Open Access
Paper: CC BY 4.0

Everything below draws on the full paper under its CC BY 4.0 licence; its figure is credited to the authors.

TL;DR

Scan-to-map ICP with a begin and an end pose per scan and allowed jumps between scans, plus elevation-image loop closure; 0.59% RTE on the KITTI leaderboard at 60 ms per scan.

Why it matters

The method needs only point coordinates and per-point timestamps and handles car-mounted, Segway-mounted and handheld LiDAR within one framework. Its driving-profile odometry runs at 60-80 ms per scan on a single CPU thread, and the full SLAM uses LiDAR data only.

Problem

A spinning multi-beam LiDAR records each scan while it moves, so the points must be corrected for sensor motion. Most odometry methods de-skew a scan once with a constant-velocity model, which fails under sudden orientation or acceleration changes, while continuous-time methods with smooth trajectories cannot follow the high-frequency, shaky motion of real platforms.

Proposed solution

CT-ICP describes each scan by a begin pose and an end pose, interpolates a pose for every point from its timestamp (slerp for rotation, linear for translation), and solves a point-to-plane scan-to-map ICP with weak constraints that keep the begin pose near the previous end pose and limit acceleration. The local map is a dense point cloud in a sparse voxel grid (up to 20 points per voxel), and a loop-closure module matches 2D features on elevation images of aggregated local maps, refines matches with ICP and optimises a g2o pose graph.

Key insights

Letting the begin pose of a scan differ from the end pose of the previous scan absorbs high-frequency motion that smooth continuous-time trajectories cannot represent.
Elastic intra-scan distortion matters on raw data: replacing it with constant-velocity de-skewing raises RTE from 0.55% to 0.79% on KITTI-raw and from 0.45% to 0.60% on KITTI-360.
Normals and planarity weights are computed from the dense local map rather than the current scan, giving richer neighbourhoods for the point-to-plane residuals.
A sparse voxel map gives constant-time neighbourhood access instead of the logarithmic access of kd-trees, which keeps a dense map usable in real time.
On motion-corrected KITTI scans all compared methods are close; the differences appear on raw, uncorrected scans.

Main results

KITTI odometry leaderboard (test sequences 11-21): 0.59% RTE, first among methods with public code at the time.
Average RTE on sequences with ground truth: 0.53% on motion-corrected KITTI (60 ms/scan), 0.55% on KITTI-raw (65 ms) and 0.45% on KITTI-360 (70 ms); IMLS-SLAM, MULLS and pyLiDAR F2M reach 0.71-1.61% on KITTI-raw and 0.68-1.55% on KITTI-360.
KITTI-CARLA (simulated): 0.09% RTE versus 0.81% for the best other odometry; ParisLuco: 1.11% versus 1.26% for IMLS-SLAM.
High-frequency motion profile: 0.55% RTE on Newer College (pyLiDAR F2M 2.20%) and 1.17% on NCLT 2012-01-08 (pyLiDAR F2M 19.80%), at 430 ms and 180 ms per scan.
Loop closure lowers ATE from 6.22 m to 0.66 m on KITTI-raw 00, from 29.87 m to 1.07 m on KITTI-360 00 and from 29.16 m to 9.65 m on ParisLuco; on KITTI-CARLA Town01 it stays near 0.2 m (0.21 m to 0.26 m).
Each elevation-grid build and match takes 1.1 s on average, plus 1.2 s for pose-graph optimisation when a loop is found.

Limitations

Loop closure requires the sensor motion to be mostly planar and the extrinsic calibration to align the z axis with the ground normal, which limits supported sensor setups and motions.
Loop closure runs on the main thread (1.1 s per elevation grid plus 1.2 s per pose-graph optimisation), so the complete SLAM is not real time yet; the authors state that separate threads would make it so.
The local map does not forget points in a sliding window, so a bad registration pollutes it; the robust profile that guards against this increases runtime (430 ms per scan on Newer College, 180 ms on NCLT).
The Gauss-Newton solver used for all experiments has hand-computed Jacobians, so its constraints cannot be changed; the more flexible Ceres option needs several threads for real-time use.
The number of loop-closure candidates (10) must be adapted to the expected drift, and on KITTI-CARLA the ATE did not improve after loop closure.

Field impact

ICRA 2022 Outstanding Paper finalist, with 301 citations. The paper releases the CT-ICP odometry in C++ (github.com/jedeschaud/ct_icp), which it describes as public and permissive open-source code, puts the loop detection and pose-graph back-end into pyLiDAR-SLAM (github.com/Kitware/pyLiDAR-SLAM), and makes the datasets used available online in the form it describes.

Abstract

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.

Open Source Robotics