Scan matching with ICP

Two scans, one move

Scan the walls, drive a few centimetres, scan again. The same walls appear in both, shifted and turned by exactly the rover's move. Find the rotation and translation that lay the new scan over the old one and you have measured the move with the lidar, not the wheels. That is scan matching; ICP (iterative closest point) is the classic way to do it.

Known pairs: the best fit

If each point $a_i$ has a known partner $b_i$, the rotation $R$ and translation $t$ that minimise $\sum_i \lVert R a_i + t - b_i \rVert^2$ come from the Kabsch method. Subtract each set's mean ($\bar a$, $\bar b$), then take the singular value decomposition of their cross-covariance:

If $\det R = -1$ that's a mirror image, not a rotation: flip the sign of the last row of $V^T$ and recompute $R$. Foundations: Least squares covers fitting by minimising squared error.

Unknown pairs: iterate

Scan points have no labels, so guess the partners. Move the source scan by your current estimate of the pose, and find each moved point $m$'s nearest target point $q$. That $q$ is only where the other scan's beam happened to land, a centimetre or so along the wall from the true partner, so slide it along its wall: with $d$ the wall's direction at $q$ (from wall_directions), the partner is $q + ((m - q) \cdot d)\,d$. Fit the source to the partners, move, re-pair, fit again. Pairs further apart than REJECT are dropped: walls only one scan saw have no partner.

ICP finds the nearest good fit, so it needs a decent start. The wheels give one: their estimate of the move since the last scan.

The program

The rover drives a loop through the maze on worn wheels, scanning every half second. Each scan is matched to the previous one and the moves are chained into a track (orange; the wheels are grey). Both tracks build a map from the scans: compare them in the Images tab.

Your task

Write nearest(points, cloud) → (idx, dist), best_fit(A, B) → (R, t), and icp(source, target, guess), which returns the pose $[dx, dy, d\theta]$ of the source scan in the target's frame. The grader tests them on fixed data, then checks your track against the rover's real path.