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.

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.

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.

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