VehicleMapping HD AV Spatial Data

Choosing a Registration Method for Sparse Scans

The comparison in registration method selection sets out what ICP, NDT and a global pass each cost and each buy. What it leaves open is the decision, and a decision made once — in a design document, for a scene that no longer resembles what the fleet drives — is how a stack ends up running ICP through tunnels.

This task replaces that with three measurements taken from the scan pair in front of you. Each costs microseconds, each is a useful diagnostic in its own right, and together they select the method.

The three measurements and what each one rules out:

Three Scan Measurements and What Each Rules Out Three panels, each naming a measurement, its threshold, and the method or trust level it eliminates. overlap source points with a target point inside the radius below 0.30 no refiner — global pass first a refiner with too little overlap converges confidently and wrongly density points per square metre in the sparser cloud below 20 /m² not ICP — use NDT too few correspondences make the objective noisy structure rank effective rank of the surface-normal scatter below 3 inflate the covariance no method recovers a direction the geometry does not constrain the third is not a method choice — it is a statement about how far to trust whichever method runs and it is the one most stacks omit, which is why tunnel poses look confident

Prerequisites #

  • Python 3.10+, NumPy 1.24+, SciPy 1.11+ (spatial.cKDTree).
  • Input: a source and target cloud with an initial pose estimate.
  • Upstream stage: motion compensation and downsampling.
  • Output: a method name, the three measurements, and a covariance-inflation flag.

Step-by-Step #

1. Measure overlap #

python
import numpy as np
from scipy.spatial import cKDTree

def overlap(src: np.ndarray, tgt: np.ndarray, radius: float = 0.5) -> float:
    """Fraction of source points with a target point inside the radius."""
    d, _ = cKDTree(tgt).query(src, distance_upper_bound=radius)
    return float(np.isfinite(d).mean())

Measured at the initial pose, not the final one, because that is the information available when the decision has to be made. Expected output: a fraction; urban scan pairs one frame apart typically return 0.75–0.9, and anything under 0.3 means the two clouds are barely looking at the same place.

2. Measure density #

python
def ground_density(points: np.ndarray, cell_m: float = 1.0) -> float:
    """Median points per square metre, over occupied cells only."""
    keys = np.floor(points[:, :2] / cell_m).astype(np.int64)
    _, counts = np.unique(keys, axis=0, return_counts=True)
    return float(np.median(counts) / (cell_m ** 2))

Median over occupied cells rather than mean over the bounding box: a scan with one dense patch and a lot of empty space has a high mean density and few correspondences anywhere useful.

3. Measure structure rank #

python
def structure_rank(normals: np.ndarray, tol: float = 0.05) -> int:
    """Effective rank of the surface-normal scatter — how many directions are constrained."""
    w = np.linalg.eigvalsh(normals.T @ normals / len(normals))
    return int((w / w.max() > tol).sum())

A ground plane alone gives rank 1; a ground plane plus a wall gives 2; a corner gives 3. The tolerance decides how weak a direction still counts as constrained, and 0.05 is deliberately generous — the point is to catch the outright degenerate cases, not to grade good scenes.

4. Apply the rule and record it #

python
from dataclasses import dataclass

@dataclass(frozen=True)
class Choice:
    method: str
    inflate_covariance: bool
    overlap: float
    density: float
    rank: int

def choose(src, tgt, normals, prior_error_m: float) -> Choice:
    ov, den, rk = overlap(src, tgt), ground_density(src), structure_rank(normals)
    if ov < 0.30 or prior_error_m > 1.5:
        method = "global+ndt"
    elif den < 20.0:
        method = "ndt"
    else:
        method = "icp_point_to_plane"
    return Choice(method, rk < 3, ov, den, rk)

prior_error_m enters separately because it is a property of the seed rather than of the scan: after a GNSS outage or a stop of unknown duration, no scan measurement can rescue a prior outside the basin.

The method mix across three environments, and why one fixed choice fails somewhere:

Selected Method and Inflation Flag in Three Environments Three rows with the three measurements, the method selected and whether the covariance is inflated. sceneoverlapdensity rankmethodinflate? dense urban 0.86 140 3 point-to-plane ICP no suburban, sparse returns 0.71 18 3 NDT no tunnel 0.79 46 2 point-to-plane ICP yes the tunnel row is the one a fixed choice cannot express at all — the method is fine and the trust is not

Verification & Acceptance Criteria #

python
def assert_selection(choices, results) -> None:
    for c, r in zip(choices, results):
        assert r.recorded_choice == c, "the recorded choice does not match the run"
        if c.method == "icp_point_to_plane":
            assert c.density >= 20.0 and c.overlap >= 0.30, "ICP chosen outside its band"
        if c.inflate_covariance:
            assert r.sigma_along > 3.0 * r.sigma_lat, "rank deficiency not reflected in σ"

    diverged = [r for r in results if r.final_rmse > 0.20]
    assert len(diverged) / len(results) <= 0.02, "divergence rate over 2%"

Acceptance gate: the recorded choice matching the run; ICP selected only inside its density and overlap band; a rank-deficient scene producing a visibly anisotropic covariance; and a divergence rate ≤2%, which is the outcome measure the whole procedure exists to move.

What each measurement costs, against the divergence it prevents:

Measurement Cost Against Divergence Prevented Three rows pairing a scan measurement with its cost and the divergence it prevents. three measurements against a 15 ms registration overlap one KD-tree query per point <1 ms on a downsampled cloud prevents a wrong minimum density unique-count over ground cells microseconds prevents ICP on sparse returns structure rank one 3×3 eigendecomposition microseconds prevents a confident wrong axis together they cost a few per cent of one registration and remove the two failure modes that produce no error at all

Common Errors & Fixes #

Registration diverges in suburbs and works downtown. ICP is being used at densities where it has too few correspondences. Measure density and switch to NDT below the threshold.

Overlap is high and the fit is still wrong. Overlap was measured at the final pose rather than the initial one, so it is measuring the fit rather than informing it.

Rank is 3 everywhere, including tunnels. The tolerance is too generous, or normals are being computed over too large a neighbourhood so the tunnel walls and ground blur together. Tighten the neighbourhood before tightening the tolerance.

The global pass runs on every frame. prior_error_m is being estimated from the previous residual rather than from the odometry gap. Use the propagated prior's own uncertainty.

Choices are not reproducible. The measurements are being taken after downsampling with a random seed. Pin the downsampling, or measure before it.

FAQ #

Why does sparsity favour NDT over ICP? #

Because ICP's cost is a sum over point-to-point correspondences, and a sparse cloud has few of them — so the objective is noisy and the fit is driven by whichever handful of points happened to land near a surface. NDT scores against a local distribution instead, which pools the map's structure regardless of how many live points fall in a cell, so its objective degrades gracefully as the live cloud thins. Below about 20 points per square metre the difference is large enough to change which method converges.

What is structure rank and why measure it? #

It is the effective rank of the scatter matrix of surface normals in the overlapping region, and it says how many directions the geometry actually constrains. A scene with walls in two directions plus a ground plane has rank three and constrains all six degrees of freedom; a tunnel has rank two and leaves along-track free. Measuring it before registering tells you which directions the result will be unreliable in, whichever method you use.

When is a global pass mandatory rather than optional? #

Whenever the initial pose error is outside the refiner's basin, which for ICP on urban scans is around 0.6 metres and for NDT around 1.5. That is a property of the seed rather than of the scan, so it is checked separately: after a GNSS outage, after a stop of unknown duration, or on the first frame of a session, the prior is simply not good enough and a global pass is required regardless of what the three scan measurements say.

Up one level: Registration Method Selection — the decision stage this procedure implements.