Exciting new paper - They say when the world moves, ICP fails. Not with Robotics@JMUW!
27.02.2026We introduce Dynamic-ICP, a Doppler-aware odometry framework designed for highly dynamic environments. By extracting motion directly from FMCW LiDAR—without calibration or auxiliary sensors—our method separates ego-motion from dynamic objects, predicts their behavior, and aligns scans with a novel geometry–Doppler objective. Across multiple real-world datasets, Dynamic-ICP significantly outperforms existing approaches in both stability and accuracy.
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 translational velocity 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 resid- ual 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 real-world datasets-HeRCULES, HeLiPR, AevaScenes-focusing on highly dynamic scenes. Dynamic-ICP consistently improves rotational stability and translation accuracy over the state-of-the-art meth- ods. To encourage further research, the code is available at: https://github.com/JMUWRobotics/Dynamic-ICP.
