A greenhouse robot measures the positions of the same posts twice, before and after a short drive, each time in its own frame (x forward, y to the left, in metres). old[i] and new[i] are the same post. If the robot moved by (t_x, t_y) and turned by \theta between the scans, every post satisfies
so (t_x, t_y, \theta) is the robot's new pose seen from its old pose. The measurements are a little noisy, so the robot takes the \theta and t that make the sum of squared errors
as small as possible.
Write align_scans(old, new) that returns (tx, ty, theta, rms): the best translation, the best rotation in radians wrapped to (-\pi, \pi], and the root mean square error of the alignment, \sqrt{\frac1n \sum_i |R(\theta)\,\text{new}_i + t - \text{old}_i|^2}.
Examples
Input: old = [(1, 0), (2, 0), (2, 1)], new = [(0, 0), (1, 0), (1, 1)]
Output: (1.0, 0.0, 0.0, 0.0)
Explanation: the posts look 1 m closer: the robot drove 1 m straight ahead.
Input: old = [(1, 3), (0, 2), (-1, 4)], new = [(1, 0), (0, 1), (2, 2)]
Output: (1.0, 2.0, 1.5707963267948966, 0.0)
Explanation: turning new by a quarter turn, (x, y) -> (-y, x), and adding (1, 2) gives old exactly.
Input: old = [(2.0, 1.0), (3.0, 1.0), (3.0, 2.5), (1.0, 3.0)], new = [(1.79, 0.7), (2.74, 0.41), (3.19, 1.84), (1.42, 2.91)]
Output: (0.4987403757065425, -0.1987742713033014, 0.29939601427930507, 0.0036631291659926036)
Explanation: about 0.5 m ahead and 0.2 m to the right, turned 0.3 rad to the left; the posts
then agree to within a few millimetres.
Constraints
len(old) == len(new) >= 2; the best rotation is unique (the points are not all in one place)- answers are compared with a tolerance of
1e-6; do not round
Goals
- Find the rotation and translation that best lay one point set over another, in closed form
- Separate the translation from the rotation by working with centred points
- Report how well the two scans agree after the alignment