VehicleMapping HD AV Spatial Data

Benchmarking ICP vs NDT Registration Accuracy

The registration method selection guide says ICP wins on dense structured scans and NDT holds up on sparse or poorly-initialized ones — but "holds up" needs a curve, not a claim. This task builds a reproducible sweep over scan density, initial-pose error, and noise, runs both methods from identical starts, and reports RMSE, convergence rate, and runtime. The result is the evidence that turns the ICP-or-NDT decision into a threshold. It draws on the accelerated ICP and NDT implementations of the point cloud registration stage.

A three-axis sweep drives both methods from identical starts and reports accuracy, robustness, and cost:

ICP vs NDT Benchmark Sweep A sweep-axes box lists density, initial error, and noise. It feeds a ground-truth-pair box, which feeds two method boxes ICP and NDT. Both feed a score box listing RMSE, convergence rate, and runtime. That feeds a threshold comparison box. Sweep axes density · init · noise GT pair known T ICP NDT Score RMSE·conv·time thresh

Prerequisites #

  • Python 3.10+, NumPy 1.24+, open3d 0.18+ (ICP + NDT via registration), pandas for aggregation.
  • Input: one or more representative LiDAR scans to perturb into benchmark pairs.
  • Upstream stage: ICP and NDT implementations available with a common interface returning transform, fitness, and RMSE.
  • Output: a table of RMSE, convergence rate, and runtime per sweep cell, and the derived crossover thresholds.

Step-by-Step #

1. Generate a ground-truth pair #

Apply a known transform to a scan; the true registration is its inverse. Perturb the initial guess to set the difficulty.

python
import numpy as np
from scipy.spatial.transform import Rotation

def make_pair(cloud, rot_deg, trans_m, noise_m, rng):
    axis = rng.normal(size=3); axis /= np.linalg.norm(axis)
    R = Rotation.from_rotvec(np.radians(rot_deg) * axis).as_matrix()
    t = rng.normal(size=3); t = t / np.linalg.norm(t) * trans_m
    T_true = np.eye(4); T_true[:3, :3] = R; T_true[:3, 3] = t
    moved = (cloud @ R.T) + t + rng.normal(scale=noise_m, size=cloud.shape)
    return cloud, moved, T_true

Key parameters: rot_deg/trans_m set the initial error the method must overcome; noise_m adds sensor noise. Vary index-derived seeds per trial for reproducibility without Math.random-style nondeterminism.

2. Register with both methods from the same start #

Run ICP and NDT from an identical perturbed initial guess so the comparison is fair.

python
def run_both(src, dst, init_T, icp_fn, ndt_fn):
    return {
        "icp": icp_fn(src, dst, init_T),
        "ndt": ndt_fn(src, dst, init_T),
    }

Key parameter: init_T is shared — both methods must attempt the same problem from the same start, or the benchmark is meaningless.

3. Score against ground truth #

Compute final RMSE against the known transform, whether the method converged within budget, and its runtime.

python
def score(result, T_true, points, budget=0.05):
    T = result.transformation
    err = np.linalg.norm((points @ T[:3, :3].T + T[:3, 3])
                         - (points @ T_true[:3, :3].T + T_true[:3, 3]), axis=1)
    rmse = float(np.sqrt(np.mean(err ** 2)))
    return {"rmse": rmse, "converged": rmse <= budget}

Key parameter: budget (0.05 m) defines "converged"; a method's convergence rate is the fraction of trials meeting it.

Verification & Acceptance Criteria #

The benchmark must itself be sound before its verdict is trusted.

python
def assert_benchmark_sound(df):
    # both methods win somewhere -> the sweep spans the crossover
    assert (df.groupby("method")["converged"].mean() > 0).all(), "a method never converged"
    # identical starts -> same trial count per method
    counts = df.groupby("method").size()
    assert counts.nunique() == 1, "unequal trials across methods"
    print("benchmark sound: balanced trials, both methods represented")

Acceptance gate: both methods converge on some region of the sweep (the sweep spans the crossover, not one method's home turf); equal trial counts per method; and the reported thresholds are stable across seeds. If one method never converges, widen the easy end of the sweep so the crossover is captured.

Common Errors & Fixes #

NDT looks strictly worse than ICP. The sweep only covers dense, well-initialized cases — ICP's home turf. Extend the sweep into sparse, high-initial-error cells where NDT's advantage appears.

Mean RMSE favours a method that rarely converges. RMSE was averaged only over successes. Report convergence rate alongside RMSE, and never compare mean RMSE without it.

Results vary run to run. Seeds were nondeterministic. Derive each trial's seed from its sweep index so the whole benchmark is reproducible.

Unfair comparison. The methods started from different guesses or ran different iteration budgets. Share init_T and equalize budgets, as in step 2.

FAQ #

How do I get ground truth for a registration benchmark? #

Apply a known transform to a copy of a scan. Take one point cloud, transform it by a chosen rotation and translation, and treat the original and the transformed copy as the pair to register — the true answer is the inverse of the applied transform. This gives exact ground truth without any survey, and lets you sweep the initial error and noise systematically. Real overlapping scans can supplement it, but synthetic pairs are what make the sweep controlled and reproducible.

Which metrics separate ICP from NDT? #

Three, tracked across the sweep. Final RMSE against ground truth measures accuracy where a method converges. Convergence rate — the fraction of trials that reach the budget at all — measures robustness, and is where the two diverge most: ICP's rate falls off steeply as initial error grows or density drops, while NDT holds up longer. Runtime measures cost. A method can win on RMSE where it converges yet lose overall because it converges far less often.

Why report convergence rate separately from RMSE? #

Because averaging RMSE only over converged trials hides the failures. A method that converges on 40 percent of hard cases but with tiny RMSE looks better on mean RMSE than a method that converges on 90 percent with slightly larger RMSE — yet the second is far more useful in production. Reporting convergence rate alongside RMSE exposes that trade-off, so the decision accounts for how often each method actually works, not just how well it does when it happens to succeed.

Up one level: Choosing a Point-Cloud Registration Method: ICP, NDT & Global — the parent decision guide this benchmark quantifies.