ICP: Sliding the Scan Into Place

A delivery robot has a point map of a building (map_points, world frame, metres) and a fresh laser scan (scan, points in the robot's frame: x forward, y to the left). Its odometry gives a guess pose = (x, y, theta) (theta in radians, counter-clockwise from +x). The robot corrects the guess with ICP (iterative closest point). One iteration, starting from the current pose:

1. Place the scan. Each scan point s goes to the world point w = R(\theta)\,s + (x, y), with R(\theta) = \begin{pmatrix}\cos\theta & -\sin\theta\\ \sin\theta & \cos\theta\end{pmatrix}.
2. Pair. Each placed point w is paired with its nearest map point m (straight-line distance; on a tie, the map point with the smallest index). A pair counts only if that distance is at most max_pair.
3. Correct. If fewer than 2 pairs count, the pose stays as it is. Otherwise find the rotation \Delta\theta and translation \Delta t that minimise \sum |R(\Delta\theta)\,w + \Delta t - m|^2 over the counted pairs, with \Delta\theta in (-\pi, \pi].
4. Update. The new pose is the correction applied after the old pose: (x, y) \leftarrow R(\Delta\theta)(x, y) + \Delta t and \theta \leftarrow \theta + \Delta\theta, wrapped to (-\pi, \pi].

Write icp(map_points, scan, pose, iterations, max_pair) that runs iterations iterations and returns the list of poses (x, y, theta) after each one.

The setup provides two helpers for experiments: rectangle_points(x0, y0, x1, y1, step) (points along the walls of a rectangular room) and seen_from(points, pose, noise=0.0, seed=0) (world points as a robot at pose sees them, with optional noise). Press Run with plot on the placed scan after each iteration to watch it slide onto the walls.

Examples

Input:  map_points = [(0, 0), (1, 0), (2, 0), (0, 1)], scan = [(-0.2, -0.1), (0.8, -0.1), (1.8, -0.1), (-0.2, 0.9)],
        pose = (0, 0, 0), iterations = 2, max_pair = 1.0
Output: [(0.19999999999999996, 0.1, 0.0), (0.19999999999999996, 0.1, 0.0)]
Explanation: every scan point pairs with the map point 0.22 m away; the best correction is a
shift by (0.2, 0.1). The second iteration finds nothing left to correct.

Input:  map_points = [(0.25 * k, 0.0) for k in range(13)] + [(0.0, 0.25 * k) for k in range(1, 13)],
        scan = [(0.18, -0.22), (0.68, -0.27), (1.17, -0.32), (1.67, -0.37), (2.17, -0.42), (-0.27, 0.33),
                (-0.22, 0.83), (-0.17, 1.32), (-0.12, 1.82), (-0.07, 2.32), (1.2, 1.1)],
        pose = (0, 0, 0), iterations = 4, max_pair = 0.5
Output: [(0.1756988922236818, 0.07533054064194789, 0.10013574007937175),
         (0.2506858964610532, 0.15034116366513983, 0.10011209698262091),
         (0.3006944833087269, 0.20033414452888912, 0.10012771902356567),
         (0.3006944833087269, 0.20033414452888934, 0.10012771902356567)]
Explanation: two walls meeting in a corner. The last scan point is a person, more than 1.1 m from
the nearest map point in every iteration, so it never counts. Pairing it anyway would stop at about (0.144, -0.015, 0.049).

Input:  map_points = [(0, 0), (1, 0), (2, 0), (0, 1)], scan = [(0.5, 0), (2, 0), (0, 1)], pose = (0, 0, 0),
        iterations = 1, max_pair = 1.0
Output: [(-0.18387363421201808, 0.0467382319502983, -0.05549850524571642)]
Explanation: (0.5, 0) is exactly as far from map point 0 as from map point 1; the smaller index wins.

Constraints

• map_points is not empty; 0 <= iterations <= 50; max_pair > 0
• in the tests, whenever 2 or more pairs count they use at least two different map points, and no distance is within 1e-7 of max_pair or of another candidate's distance except for exact ties
• answers are compared with a tolerance of 1e-6; do not round

Goals

• Pair each scan point with its nearest map point, rejecting pairs that are too far apart
• Find the rigid motion that best moves the paired scan points onto the map, in closed form
• Compose that correction with the pose and repeat, recording the pose after each round