Dynamic-ICP: Doppler-Aware Iterative Closest Point Registration for Dynamic Scenes

TL;DR

Dynamic-ICP introduces a Doppler-aware registration method that enhances odometry accuracy in dynamic scenes by estimating ego motion and dynamic object velocities directly from FMCW LiDAR data, without external sensors.

Contribution

It presents a novel Doppler-aware ICP framework that estimates ego and object velocities, improving registration robustness in dynamic environments without additional sensors.

Findings

Improves rotational stability and translation accuracy over state-of-the-art methods.

Operates in real time on FMCW LiDAR data without external calibration.

Easily integrable into existing SLAM pipelines.

Abstract

Reliable odometry in highly dynamic environments remains challenging when it relies on ICP-based registration: ICP assumes near-static scenes and degrades in repetitive or low-texture geometry. We introduce Dynamic-ICP, a Doppler-aware registration framework. The method (i) estimates ego motion from per-point Doppler velocity via robust regression and builds a velocity filter, (ii) clusters dynamic objects and reconstructs object-wise translational velocities from ego-compensated radial measurements, (iii) predicts dynamic points with a constant-velocity model, and (iv) aligns scans using a compact objective that combines point-to-plane geometry residual with a translation-invariant, rotation-only Doppler residual. The approach requires no external sensors or sensor-vehicle calibration and operates directly on FMCW LiDAR range and Doppler velocities. We evaluate Dynamic-ICP on three datasets-HeRCULES, HeLiPR, AevaScenes-focusing on highly dynamic scenes. Dynamic-ICP consistently improves rotational stability and translation accuracy over the state-of-the-art methods. Our approach is also simple to integrate into existing pipelines, runs in real time, and provides a lightweight solution for robust registration in dynamic environments. To encourage further research, the code is available at: https://github.com/JMUWRobotics/Dynamic-ICP.