A hospital delivery robot sees a wall as a row of laser points, given in the robot's own frame (x forward, y to the robot's left, in metres). It stores the wall as a line in normal form
where (\cos\alpha, \sin\alpha) is the unit vector from the robot (the origin) straight towards the line, and d > 0 is the line's distance from the robot. For example the wall x = 2 straight ahead is \alpha = 0, d = 2; the wall y = -1 on the right is \alpha = -\pi/2, d = 1.
The best line through the points is the one that makes the sum of the squared perpendicular distances from the points to the line as small as possible. (The distance of (x_i, y_i) from the line is |x_i\cos\alpha + y_i\sin\alpha - d|.) Walls can run in any direction, including straight ahead of the robot, so a fit of y against x will not do.
Write fit_wall(points) that returns the best line as a tuple (alpha, d), with d > 0 and alpha in radians wrapped to (-\pi, \pi].
Examples
Input: points = [(2.02, -1), (1.98, 0), (2.01, 1), (1.99, 2)]
Output: (0.00600091209208443, 2.002964427199993)
Explanation: a wall just under 2 m ahead, turned very slightly.
Input: points = [(0, 3), (1, 2), (2, 1), (3, 0)]
Output: (0.7853981633974483, 2.121320343559643)
Explanation: the points lie exactly on x + y = 3; the nearest point of that line is (1.5, 1.5),
at angle pi/4 and distance 3 / sqrt(2).
Input: points = [(-3, 1), (-3.1, 2), (-2.9, 3)]
Output: (3.0912583275108787, 3.0968266351747005)
Explanation: a wall behind the robot, so alpha is close to pi.
Constraints
2 <= len(points); the points have a single best line, and it passes more than0.01m from the robot- no test has a best line with
alphawithin1e-6of \pm\pi - answers are compared with a tolerance of
1e-6; do not round
Goals
- Describe a line by its normal angle and its distance from the origin
- Fit the line that minimises the squared perpendicular distances, which works for walls in every direction
- Pick the one representation with a positive distance and a wrapped angle