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

TL;DR

This paper introduces CT-ICP, a real-time LiDAR odometry method that improves accuracy and robustness by combining continuous scan matching with a novel loop closure technique, enabling effective SLAM in dynamic environments.

Contribution

The paper presents a novel continuous-time ICP approach with elastic scan distortion and a fast loop detection method for LiDAR SLAM, achieving real-time performance and high accuracy.

Findings

Achieved 0.59% RTE on KITTI odometry benchmark.

Operates at 60ms per scan on a single CPU thread.

Demonstrated robustness across diverse datasets and scenarios.

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.