ICP

What it computes

ICP (iterative closest point) finds the rigid motion that lays one point cloud on top of another. Match two lidar scans taken from different places, and you learn how the robot moved between them.

Known pairs: the Kabsch fit

If each point $a_i$ has a known partner $b_i$, the $R$ and $t$ that minimise $\sum_i \lVert R a_i + t - b_i \rVert^2$ come from one SVD. Centre both sets on their means $\bar a$ and $\bar b$, then

If $\det R = -1$, that's a reflection, not a rotation: negate the last row of $V^\top$ and recompute $R$.

Unknown pairs: iterate

R, t = identity, zero
repeat iters times:
    moved = source moved by (R, t)
    j = index of the nearest target point to each moved point
    R, t = best fit of source onto target[j]

Nearest neighbours are roughly right once the clouds are close, and each fit brings them closer. ICP settles in the nearest local minimum, so it needs a start within a few tens of degrees.

Tools

((P[:, None] - Q[None]) ** 2).sum(-1) is the (N, M) matrix of squared distances; .argmin(axis=1) picks each row's nearest. np.linalg.svd(H) returns U, S, Vt.

The program

The rover scans, drives 8 cm, turns 20° and scans again. scan.points() gives the hits in the rover's frame, so aligning scan 2 onto scan 1 gives the move: $t$ is where the rover went and $R$ how far it turned. The Plots tab shows the scans before and after.

See it in context

In Seeing with a Camera, hand–eye calibration uses the same SVD step to snap a matrix to the nearest rotation.

Your task

Implement best_fit_transform(A, B) and icp(source, target, iters). Points are rows of (N, 2) arrays, and both return (R, t) (2 × 2 and (2,)) with B ≈ A @ R.T + t. The grader checks the fit on known pairs, ICP on three synthetic rooms, then the rover's motion to within 2 mm and 0.5°.