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

This paper introduces Dynamic-ICP, a real-time, Doppler-aware registration framework for FMCW LiDAR that robustly estimates ego-motion and aligns scans in highly dynamic environments by integrating point-to-plane geometry with rotation-only Doppler residuals, eliminating the need for external sensors or calibration.

The Gist

Imagine you are trying to take a photo of a busy city street while riding a bicycle. If you stand still, the photo is clear. But if you are moving fast, and there are cars and pedestrians zooming past you, a normal camera (or a standard robot navigation system) gets confused. It tries to match the moving car in your first photo with the same car in your second photo, but because the car moved on its own, the robot thinks it (the robot) moved in a weird, impossible way. This causes the robot to get lost or spin in circles.

This is the problem Dynamic-ICP solves. It's a new "smart camera" system for robots that uses a special type of laser scanner (FMCW LiDAR) to see not just where things are, but how fast they are moving.

Here is how it works, broken down into simple steps with some fun analogies:

1. The Problem: The "Moving Target" Confusion

Standard robot navigation (called ICP) works like a puzzle solver. It takes a picture of the world, then takes another picture a split-second later. It tries to match the pieces of the second picture to the first one to figure out how the robot moved.

• The Flaw: This assumes everything in the picture is a static building or a tree. If a car drives by, the puzzle solver gets tricked. It thinks the car is a building that suddenly teleported, causing the robot to calculate the wrong path.

2. The Superpower: Seeing "Speed" (Doppler)

The secret weapon here is Doppler velocity. You know how a siren sounds higher as an ambulance approaches and lower as it drives away? That's the Doppler effect.

• The Analogy: Standard lasers are like a photographer taking a still photo. Dynamic-ICP is like a photographer who can also see the speedometer of every single pixel in the photo. It knows exactly how fast every point of light is moving toward or away from the robot.

3. The Solution: The "Dynamic-ICP" Workflow

The paper describes a four-step process to fix the confusion:

Step A: The "Self-Check" (Ego-Motion Estimation)

First, the robot asks: "Am I moving?"
It looks at all the stationary things (buildings, trees) and calculates its own speed based on how the "speedometer" readings change. It's like a runner checking their watch against the stationary trees to know their own pace.

Step B: The "Crowd Control" (Clustering)

Next, it looks at the moving things. It groups them together.

• The Analogy: Imagine a busy dance floor. The robot separates the stationary walls from the dancers. It groups the dancers into couples or groups (like a car with four wheels moving together). It ignores the random noise and focuses on the "objects" that are moving as a unit.

Step C: The "Crystal Ball" (Prediction)

This is the magic trick. Before trying to match the pictures, the robot uses its "crystal ball" (a constant-velocity model) to guess where those moving dancers will be in the next split-second.

• The Analogy: Instead of trying to match a car in Photo A to a car in Photo B (where the car has moved), the robot says, "I know that car is moving at 30 mph. I will mentally move the car in Photo A forward to where it will be in Photo B." Now, the two photos line up perfectly.

Step D: The "Double-Check" (Doppler-Aware Matching)

Finally, it snaps the puzzle pieces together. But it doesn't just look at the shape (geometry); it also checks the speed.

• The Analogy: It's like a bouncer at a club. A standard bouncer checks your ID (the shape of the building). Dynamic-ICP checks your ID and your speed. If a point claims to be a building but is moving at 20 mph, the bouncer says, "Nope, you're a car, get out of the building section!" This prevents the robot from getting confused by moving objects.

Why is this a big deal?

• No Extra Gear: You don't need to attach extra cameras or GPS to the robot. It does it all with one special laser scanner.
• Stability in Chaos: It works great in tunnels (where there are no features to grab onto) and in heavy traffic (where everything is moving).
• Real-Time: It's fast enough to run on a robot driving down the highway without lagging.

The Bottom Line

Dynamic-ICP is like giving a robot a pair of glasses that let it see the future position of moving objects. Instead of getting confused by a busy street, the robot predicts where the cars and people will be, aligns its view perfectly, and knows exactly where it is, even when the world around it is a chaotic blur.

Technical Summary

Here is a detailed technical summary of the paper "Dynamic-ICP: Doppler-Aware Iterative Closest Point Registration for Dynamic Scenes."

1. Problem Statement

Standard Iterative Closest Point (ICP) algorithms are the backbone of LiDAR odometry and SLAM but rely on the assumption that the environment is quasi-static between frames. In highly dynamic scenes (e.g., heavy traffic, moving pedestrians) or geometrically repetitive/low-texture environments (e.g., tunnels, bridges), standard ICP fails due to:

• Spurious Correspondences: Moving objects create false matches that bias pose estimation.
• Degeneracy: Lack of unique geometric features leads to drift and rotation estimation failures.
• Motion Artifacts: Fast ego-motion combined with object motion violates the rigid-body assumption.

While Frequency-Modulated Continuous-Wave (FMCW) LiDARs provide per-point Doppler velocity (radial velocity), existing methods often treat moving points as outliers to be discarded or rely on complex, training-based scene flow estimation. There is a need for a lightweight, real-time registration method that actively models and utilizes dynamic motion rather than ignoring it.

2. Methodology: Dynamic-ICP

The authors propose Dynamic-ICP, a Doppler-aware registration framework that explicitly models scene dynamics to predict point positions before alignment. The pipeline consists of four key modules:

A. Ego-Motion Estimation & Velocity Filtering

• Principle: For static points, the measured Doppler velocity ($s_i$) is solely induced by the ego-vehicle's motion projected onto the line-of-sight (LOS).
• Implementation: The system estimates the ego's translational velocity ($\mathbf{v}$) via robust regression (e.g., Huber loss) over all points in the scan, solving $\min_{\mathbf{v}} \sum \rho(s_i + (\mathbf{u}_i)^\top \mathbf{v})$.
• Filtering: A distance-adaptive threshold is applied to the residual ($r_i = s_i + (\mathbf{u}_i)^\top \hat{\mathbf{v}}$). Points with residuals exceeding the threshold are flagged as dynamic; others form the static set.

B. Dynamic Point Clustering & Velocity Reconstruction

• Clustering: Dynamic candidates are clustered using HDBSCAN based on spatial proximity and velocity consistency.
• Velocity Reconstruction: For each cluster (assumed to be a rigid moving object), the system reconstructs the full 3D translational velocity ($\hat{\mathbf{v}}_k$) by solving a linear least-squares problem using the ego-compensated Doppler velocities of the points within the cluster.
• Validation: Clusters are discarded if the LOS directions are too parallel (ill-conditioned) or if the inlier ratio is too low, ensuring only reliable velocity estimates are used.

C. Dynamic Point Prediction

• Motion Model: A constant-velocity model is applied to the validated dynamic clusters.
• Prediction: Points belonging to a dynamic cluster $k$ are warped to the next frame ($t+\Delta t$) using the reconstructed velocity: $\tilde{\mathbf{p}}^{t+1}_i = \mathbf{p}^t_i + \hat{\mathbf{v}}_k \Delta t$.
• Static Points: Points in the static set remain unchanged.
• Result: The source point cloud is "motion-compensated," predicting where dynamic objects will be in the target frame, thereby aligning moving objects correctly before correspondence search.

D. Doppler-Aware ICP Matching

The alignment step minimizes a composite objective function combining geometry and Doppler constraints:
$$\min_{R, t} \sum \left[ (1-\lambda_v)\rho_g(r_{g,ij}^2) + \lambda_v \rho_v(r_{v,ij}^2) \right]$$

1. Geometry Residual ($r_{g,ij}$): Standard point-to-plane error, ensuring the transformed source points lie on the target surface.
2. Doppler Residual ($r_{v,ij}$): A novel term enforcing consistency between the rotated source Doppler velocity and the target Doppler velocity along the LOS: $r_{v,ij} = \mathbf{u}_j^\top R (s_i \mathbf{u}_i) - s_j$.
   • Key Advantage: This term is translation-invariant (depends only on rotation $R$). It provides a strong rotational constraint even in low-texture or repetitive environments where geometric features are ambiguous.

3. Key Contributions

1. Doppler-Aware Framework: A novel ICP variant that integrates per-point Doppler velocity to model, cluster, and predict dynamic objects rather than discarding them.
2. Object-Wise Velocity Reconstruction: A method to recover full 3D translational velocities of moving objects from radial Doppler measurements without requiring external sensors (IMU/GNSS) or sensor-vehicle extrinsic calibration.
3. Rotation-Invariant Doppler Residual: The introduction of a Doppler-based loss term that specifically targets rotational errors, significantly improving stability in degenerate geometric scenarios.
4. Training-Free & Real-Time: The approach is purely geometric/analytical (no deep learning), operates directly on raw FMCW data, and maintains real-time efficiency.

4. Experimental Results

The method was evaluated on three real-world datasets (HeRCULES, HeLiPR, AevaScenes) and one simulated dataset (DICP), focusing on highly dynamic and low-texture scenarios.

• Performance: Dynamic-ICP consistently achieved State-of-the-Art (SOTA) or tied-SOTA performance in both Relative Translation Error (RTE) and Relative Rotation Error (RRE).
• Dynamic Scenes: In high-speed highway and city driving, it significantly outperformed standard ICP, KISS-ICP, and GICP, particularly in rotation stability.
• Low-Texture/Repetitive Scenes: In tunnel-like "Curved Walls" scenarios, it maintained high accuracy where geometric-only methods failed due to drift.
• Ablation Studies:
  • Removing the Velocity Filter (VF) caused errors to spike as dynamic points corrupted static alignment.
  • Removing Dynamic Prediction (DPP) reduced accuracy by failing to align moving objects.
  • Removing the Doppler Residual (DR) degraded rotation accuracy, confirming the term's role in stabilizing orientation.
• Efficiency: The method converges in fewer iterations (avg. 13.5) than standard ICP variants and achieves ~13.3 FPS, comparable to lightweight baselines like KISS-ICP.

5. Significance

• Robustness in Dynamic Environments: Dynamic-ICP solves a fundamental limitation of traditional SLAM: the inability to handle moving objects without discarding data. By predicting motion, it turns dynamic noise into a useful signal.
• Hardware Independence: Unlike some Doppler-based methods (e.g., Doppler-ICP) that require precise sensor-vehicle calibration, Dynamic-ICP operates solely on the LiDAR's range and Doppler data, making it easier to deploy.
• Geometric Degeneracy Solution: The translation-invariant Doppler residual offers a unique solution to the "tunnel effect" (drift in repetitive geometry), providing a rotation cue independent of surface normals.
• Open Source: The authors released the code, facilitating further research in Doppler-based odometry and dynamic scene understanding.

In summary, Dynamic-ICP represents a significant step forward in autonomous navigation by leveraging the unique capabilities of FMCW LiDAR to create a robust, real-time odometry system capable of operating in the most challenging, dynamic, and geometrically ambiguous environments.