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

RANSAC Picks the Wall From the Clutter

RANSAC · robust fitting · inliers · line fitting · laser scan

A warehouse robot sees a long shelf front as a row of laser points, but the scan also holds points from a pallet, a person's legs and a stray reflection. Fitting one line to all the points would tilt it towards the clutter. Instead, the robot lets pairs of points propose lines and keeps the line that most points agree with. The points are in the robot's frame (x forward, y to the left, in metres).

Proposals. Go through the pairs (i, j) with i < j in order: (0, 1), (0, 2), ..., (0, n-1), (1, 2), .... Skip a pair whose two points are equal. The pair proposes the line through points[i] and points[j]; its inliers are the indices k of all points whose perpendicular distance to that line is at most tol (so i and j themselves are inliers). The winning proposal is the one with the most inliers; on a tie, the earliest pair in the order above wins.

Refit. The answer's line is the one that best fits the winner's inliers: it minimises the sum of squared perpendicular distances to them. Give it in normal form x\cos\alpha + y\sin\alpha = d with d > 0 and \alpha in radians wrapped to (-\pi, \pi]. With the inliers' mean point (\bar x, \bar y) and the sums S_{xx} = \sum (x - \bar x)^2, S_{yy} = \sum (y - \bar y)^2, S_{xy} = \sum (x - \bar x)(y - \bar y) over the inliers, that line is

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

and if this d is negative, use -d and \alpha + \pi (wrapped) instead.

Write ransac_wall(points, tol) that returns (inliers, alpha, d), with inliers the winner's indices in increasing order.

Examples

Input:  points = [(0, 2.0), (1, 2.05), (2, 1.95), (3, 2.0), (1.5, 0.5), (4, 2.02), (2.5, 3.1)], tol = 0.1
Output: ([0, 1, 2, 3, 5], 1.5697957958471824, 2.006000058497346)
Explanation: the pair (0, 3) proposes y = 2; five points lie within 0.1 of it. No pair does better.
The refit through those five is a wall about 2 m to the left.

Input:  points = [(1.0, -1.0), (2.5, 3.0), (1.02, 0.0), (0.0, 3.0), (0.98, 1.0), (1.5, 3.02), (3.0, 0.5)], tol = 0.05
Output: ([0, 2, 4], 0.01000266698657519, 0.9999499737436878)
Explanation: two walls, three points each. The pair (0, 4) finds [0, 2, 4] before
the pair (1, 3) finds [1, 3, 5], so the wall ahead wins the tie.

Input:  points = [(0, 0.5), (4, 0.5), (2, 1.0), (1, 2.0)], tol = 0.5
Output: ([0, 1, 2], 1.5707963267948966, 0.6666666666666665)
Explanation: point 2 is exactly 0.5 from the line through points 0 and 1, and at most means it counts.

Constraints

  • 2 <= len(points) <= 80, with at least two different points; tol >= 0
  • the refit line passes more than 0.01 m from the robot, and its alpha is not within 1e-6 of \pm\pi
  • alpha and d are compared with a tolerance of 1e-6

Goals

  • Measure the perpendicular distance from a point to the line through two other points
  • Let every pair of points propose a line and keep the one most points agree with
  • Refit the winning line to its inliers by perpendicular least squares
Starting Python…