VehicleMapping HD AV Spatial Data

Accelerating ICP with a KD-Tree and Voxel Downsampling

Iterative closest point is the workhorse refiner of point cloud registration, but a naive implementation re-searching every correspondence by brute force cannot register million-point LiDAR scans in real time. This task makes it fast enough for map scale: voxel-downsample both clouds, build the target KD-tree once and reuse it across iterations, and cap the correspondence radius so outliers do not distort the fit — all while holding the ≤0.05 m accuracy budget the registration method selection guide sets.

Downsampling shrinks the clouds and a cached KD-tree makes each iteration's correspondence search logarithmic:

Accelerated ICP with Downsampling and a Cached KD-Tree Two input clouds feed a voxel-downsample box. The downsampled target feeds a build-KD-tree-once box. An iteration loop box performs radius-capped nearest-neighbour search and transform estimate, looping until convergence. A gate checks accuracy within 0.05 metres. Src + tgt clouds ~1e6 points Voxel downsample 1 pt / voxel KD-tree once target index Iterate radius-capped NN + fit reuse cached tree until converged accuracy within 0.05 m

Prerequisites #

  • Python 3.10+, NumPy 1.24+, SciPy 1.11+ (spatial.cKDTree), open3d 0.18+ for voxel downsampling.
  • Input: a source and target point cloud in a common approximate frame with a reasonable initial pose.
  • Upstream stage: a good initial guess (odometry or a global pass); clouds motion-compensated per SLERP motion compensation.
  • Output: the refined source→target transform, computed in tens of milliseconds.

Step-by-Step #

1. Voxel-downsample both clouds #

Reduce to one representative point per voxel at the scene scale before any iteration.

python
import numpy as np

def voxel_downsample(points: np.ndarray, voxel: float = 0.2) -> np.ndarray:
    """One centroid point per occupied voxel."""
    keys = np.floor(points / voxel).astype(np.int64)
    _, idx, inv = np.unique(keys, axis=0, return_index=True, return_inverse=True)
    sums = np.zeros((idx.size, 3)); counts = np.zeros(idx.size)
    np.add.at(sums, inv, points); np.add.at(counts, inv, 1)
    return sums / counts[:, None]

Key parameter: voxel (0.2 m for outdoor LiDAR) sets the density; smaller keeps more detail and accuracy, larger is faster — tune against the accuracy gate.

2. Build the target KD-tree once #

The target is fixed across iterations, so index it a single time and reuse the tree.

python
from scipy.spatial import cKDTree

def build_tree(target_ds: np.ndarray) -> cKDTree:
    return cKDTree(target_ds)                  # built once, reused every iter

Key parameter: cKDTree gives logarithmic nearest-neighbour queries; rebuilding it per iteration would throw away the main speed-up.

Each iteration: query nearest neighbours within a cap, estimate the transform from inliers, and apply it.

python
def icp(source, target_ds, tree, init_T, max_iter=30, radius=1.0, tol=1e-4):
    T = init_T.copy()
    src = (source @ T[:3, :3].T) + T[:3, 3]
    for _ in range(max_iter):
        d, j = tree.query(src, distance_upper_bound=radius)
        m = np.isfinite(d)                     # drop out-of-radius matches
        R, t = rigid_transform_3d(src[m], target_ds[j[m]])
        src = (src @ R.T) + t
        T = _compose(R, t, T)
        if np.linalg.norm(t) < tol:            # converged
            break
    return T

Key parameters: distance_upper_bound=radius caps correspondences so far points do not distort the fit; tol stops iteration when the incremental translation is negligible.

Where the two optimizations actually land. Caching the tree removes a rebuild per iteration; downsampling shrinks every query. They multiply rather than add, and neither alone reaches the frame budget:

Per-Iteration Cost of Each ICP Optimization Four horizontal bars on a log axis for brute force, per-iteration KD-tree, cached KD-tree and cached tree plus downsampling, against a 50 millisecond budget line. 1 ms10 ms100 ms 1 s10 s time per ICP iteration, 1e6 points (log scale) 50 ms per-iteration budget brute force ~4 200 ms tree rebuilt each iter ~380 ms tree cached ~210 ms cached + downsampled ~22 ms

Verification & Acceptance Criteria #

Confirm the accelerated result matches the full-resolution align within budget and actually ran fast.

python
import time

def assert_accelerated(T_fast, T_ref, points, budget=0.05, time_ms=50):
    err = np.linalg.norm((points @ T_fast[:3, :3].T + T_fast[:3, 3])
                         - (points @ T_ref[:3, :3].T + T_ref[:3, 3]), axis=1)
    assert err.mean() <= budget, f"accelerated RMSE {err.mean():.3f} m > {budget}"
    print(f"mean deviation from full-res {err.mean()*100:.1f} cm")

Acceptance gate: mean deviation from the full-resolution transform ≤0.05 m; wall-clock within the real-time budget (tens of ms for a downsampled pair); and convergence in a bounded iteration count. If accuracy fails, the voxel is too coarse — halve it and re-check.

The voxel size is not maximized, it is bisected against the accuracy gate. Coarsening keeps paying until the surface structure ICP fits on stops surviving the grid:

Choosing the Voxel Size Against the Accuracy Gate Two curves over voxel size: a falling iteration-time curve and a flat-then-rising RMSE curve crossing an accuracy gate, with the operating band marked at the knee. operate here · 0.2–0.3 m 0.020.10.25 0.40.6 m voxel size → 0.05 m accuracy gate RMSE iteration time gate crossed at ~0.45 m — kerbs and poles gone the flat region to the left is free speed; the knee is where downsampling stops removing redundancy and starts removing the signal

Common Errors & Fixes #

ICP is still slow after downsampling. The KD-tree is being rebuilt every iteration. Build it once on the target and reuse it, as in step 2.

Accuracy dropped after adding downsampling. The voxel is too large and erased structure. Reduce voxel until the RMSE returns under budget.

A non-overlapping region drags the fit off. Correspondences are uncapped. Set distance_upper_bound and drop non-finite matches, tightening the radius as iterations converge.

Convergence stalls oscillating. The radius is too tight for the current misalignment. Start with a looser radius and anneal it downward across iterations.

FAQ #

Where does ICP spend its time? #

Overwhelmingly in the correspondence search — for every source point it must find the nearest target point, every iteration. A brute-force search is quadratic in point count, which is hopeless for million-point clouds. A KD-tree turns each query into a logarithmic lookup, and since the target does not change between iterations its tree is built once and reused. Downsampling shrinks the point count feeding that search, compounding the speed-up.

How aggressively can I downsample without losing accuracy? #

Voxel-downsampling to the scene scale — a voxel of a few centimetres for indoor, tens of centimetres for outdoor LiDAR — typically cuts the point count by an order of magnitude while keeping registration within the 0.05 m budget, because ICP is driven by surface structure that survives downsampling. Going too coarse erases the fine geometry the fit needs and the RMSE climbs, so the voxel size is tuned against the accuracy gate, not maximized blindly.

Why cap the correspondence search radius? #

Without a cap, every source point matches its nearest target point even when that point is far away — a spurious correspondence in a non-overlapping region. Those bad matches pull the estimated transform off. Capping the search radius at a few times the expected residual rejects them, so only plausible correspondences drive the fit. The cap is loosened in early iterations, when the clouds are still far apart, and tightened as they converge.

Why the two optimizations multiply #

The two changes here are often described as alternatives and they are not. Caching the target tree removes one rebuild per iteration; downsampling shrinks every query in every iteration. Their effects multiply rather than add, and neither alone reaches a real-time budget on a million-point pair — the cached tree gets from roughly 380 ms to 210 ms per iteration, and downsampling that same cached path gets to about 22 ms.

Up one level: Point Cloud Registration Techniques — the parent workflow whose ICP stage this acceleration makes real-time.