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

TL;DR

Dynamic-ICP addresses the core challenge of estimating ego-motion and registering scans in highly dynamic environments where traditional ICP struggles due to moving objects and repetitive geometry. It introduces a Doppler-aware framework that leverages per-point Doppler velocities from FMCW LiDAR to cluster dynamic objects, reconstruct their 3D velocities, predict next-frame positions, and perform a Doppler-consistent ICP with a two-term objective: a geometry-based point-to-plane residual and a rotation-focused Doppler residual $r_{v,ij}$. Key contributions include ego-motion estimation from static points, object-wise velocity reconstruction via velocity consistency, dynamic point prediction with a constant-velocity model, and a translation-invariant Doppler residual that stabilizes rotation estimation in challenging scenes. The method operates without external sensors or calibration, runs in real time, and achieves state-of-the-art or competitive results on HeRCULES, HeLiPR, and AevaScenes, highlighting its practical impact for robust registration in dynamic environments $^{ ext{(code available)}}$.

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.

Figures (5)

• Figure 1: Workflow of Dynamic-ICP for dynamic objects. Left: FMCW LiDAR scan in highway scenarios. Dynamic points are colored and clustered. Right: Velocity reconstruction, prediction, and matching of dynamic objects. The raw dynamic points are colored red, while the predicted points are colored green. Black arrows and boxes represent the object's velocity and bounding box, respectively.
• Figure 2: Pipeline of Dynamic-ICP consists of four main modules: (i) Ego-Motion Estimation (Sec. \ref{['sec:ego']}); (ii) Dynamic Points Clustering (Velocity Filter, Clustering and Velocity Reconstruction) (Sec. \ref{['sec:cluster']}); (iii) Dynamic Points Prediction (Sec. \ref{['sec:predict']}); and (iv) Doppler-aware ICP Matching (Sec. \ref{['sec:match']}). The figure illustrates the workflow on point cloud data: starting from a raw scan, ego velocity (arrows) is estimated from per-point Doppler velocity. The velocity filter distinguishes dynamic points (red) from the static background (gray) and clusters the dynamic set into individual objects (colored). For each cluster, the object velocity is reconstructed from its points’ Doppler velocities, yielding a velocity vector and a bounding box (black arrow and box). These velocities are then used to predict object states to the next frame. Finally, the scan (green) at time $t$ (predicted dynamic points, together with the static points) are aligned to the scan (red) at time $t + \Delta t$ via Doppler-aware ICP.
• Figure 3: Velocity reconstruction of dynamic objects. Left: FMCW LiDAR scan with dynamic points highlighted in red and synchronized camera view. Right: Top-down view of LiDAR scan. Dynamic points on the moving rigid body (car) satisfy the Doppler velocity consistency condition. This means that the velocity component of the object (black arrow) in the direction of the line-of-sight (blue dashed line) equals the compensated Doppler velocity (purple arrow) at that point.
• Figure 4: Doppler residual of Doppler-aware ICP matching. Red points and arrows denote the source frame, and blue points and arrows denote the target frame. Arrows show line-of-sight directions whose lengths are proportional to Doppler (radial) velocity. From left to right, the diagram illustrates the matching process: source Doppler rays are rotated to align with target rays, after which correspondences are established by combining point-to-plane geometry with the Doppler residual.
• Figure 5: Qualitative comparison. For clarity, we plot only the two best-performing methods per sequence, including our Dynamic-ICP.