Problem 560995 · medium · Robotics · Level 05 Planning and Mapping

Where Two Walls Meet

corner detection · line fitting · line intersection · angle between lines

Corners are good landmarks: a robot that recognises the corner of a room in two scans can tell how far it has moved. A cleaning robot has split its scan into walls and now looks at two of them, wall_a and wall_b, each a list of points (x, y) in metres.

The wall lines. Each wall's line is the one that minimises the sum of squared perpendicular distances to its points. In normal form x\cos\alpha + y\sin\alpha = d it is

\alpha = \tfrac12\,\mathrm{atan2}(-2S_{xy},\ S_{yy} - S_{xx}), \qquad d = \bar x\cos\alpha + \bar y\sin\alpha,

where (\bar x, \bar y) is the wall's mean point, S_{xx} = \sum (x - \bar x)^2, S_{yy} = \sum (y - \bar y)^2 and S_{xy} = \sum (x - \bar x)(y - \bar y) over that wall's points. (Here d may come out negative; that describes the same line.)

The corner. The angle between the lines is the smaller of the two angles they make where they cross, between 0 and \pi/2 inclusive, in radians. If it is less than min_angle, the walls are too close to parallel for a reliable corner: return None. Otherwise return (x, y, angle): the point where the two lines cross (anywhere, even beyond the ends of the walls) and the angle.

Write wall_corner(wall_a, wall_b, min_angle).

Examples

Input:  wall_a = [(0, 2.02), (1, 1.98), (2, 2.0)], wall_b = [(3.01, 1), (2.99, 0), (3.0, -1)], min_angle = 0.3
Output: (3.009900217332336, 1.9798949669200239, 1.565793993151654)
Explanation: a wall near y = 2 and a wall near x = 3 meet close to (3, 2), almost at a right angle.

Input:  wall_a = [(0, 0), (2, 2), (4, 4)], wall_b = [(1, 4), (2, 4), (3, 4)], min_angle = 0.3
Output: (4.0, 4.000000000000001, 0.7853981633974483)
Explanation: y = x meets y = 4 at (4, 4). The lines make angles of pi/4 and 3pi/4; the smaller is pi/4.

Input:  wall_a = [(0, 1), (1, 1.05), (2, 1.1)], wall_b = [(3, 1.2), (4, 1.3), (5, 1.38)], min_angle = 0.2
Output: None
Explanation: the walls differ in direction by only 0.04 rad.

Constraints

  • each wall has at least 2 points and a single best line (its points do not spread equally in every direction)
  • no test has an angle within 1e-6 of min_angle
  • answers are compared with a tolerance of 1e-6; do not round

Goals

  • Fit a line in normal form to each of two groups of wall points
  • Intersect two lines in normal form by solving a 2x2 system
  • Measure the angle between two lines and reject nearly parallel pairs
Starting Python…