Which Pose Explains the Scan?

A robot has been switched on somewhere in a known building and must work out where it is. It takes one laser scan and compares it with what it would see from each of a few candidates poses: the candidate whose predicted scan matches best is its guess.

The map. walls is a list of straight segments ((ax, ay), (bx, by)) in metres (x to the right, y up).

Predicting one beam. A candidate is a pose (x, y, theta), theta in radians counter-clockwise from +x. The scan has one beam for each angle a in angles, measured from the heading, so the beam leaves (x, y) in the world direction theta + a. Its predicted range is the distance to the first wall it meets, with these rules:

• only forwards (distances of 0 or more); a wall is hit anywhere between its end points, end points included;
• a wall parallel to the beam is never hit (the beam grazes it);
• if no wall is hit within max_range, the predicted range is max_range.

Scoring. ranges[i] is the measured range of beam i (a reading of max_range means no echo). A candidate's score is the number of beams whose predicted range is within tol of the measured one: abs(predicted - measured) <= tol. Counting agreements, rather than adding up errors, means one beam blocked by a passing person costs a single point, not the whole match.

Write best_pose(walls, candidates, angles, ranges, max_range, tol) that returns a tuple (index, score): the position of the candidate with the highest score in candidates, and that score. If several candidates share the highest score, return the one that comes first.

The setup provides hall_walls(), a 10 m by 6 m hall with a 1 m crate whose corners are (6, 2) and (7, 3), and warehouse(seed), which returns all six arguments for a larger random warehouse (best_pose(*warehouse(3))).

Examples

Input:  walls = hall_walls(),
        candidates = [(7, 4, -2.7416), (3, 2, 0.4), (3.5, 2, 0.4), (3, 2, 0.6), (2, 4, -0.4)],
        angles = [-2.3562, -1.5708, -0.7854, 0.0, 0.7854, 1.5708, 2.3562, 3.1416],
        ranges = [2.13, 2.13, 1.2, 7.57, 4.27, 4.33, 3.28, 3.29], max_range = 8, tol = 0.1
Output: (1, 7)
Explanation: from (3, 2, 0.4) the predicted scan is 2.158, 2.171, 5.32, 7.6, 4.317, 4.343,
3.237, 3.257: every beam agrees except the third (someone stood 1.2 m away). Candidate 0 is
the same spot turned half-way round the hall; the room alone looks the same from there, but
its fifth beam would meet the crate at 1.079 m, so it scores 6.

Input:  the same, but ranges = [2.13, 2.13, 1.2, 7.57, 8.0, 4.33, 3.28, 3.29]
Output: (0, 6)
Explanation: with the fifth beam lost too, candidates 0 and 1 both score 6; the first wins.

Constraints

• candidates is not empty; ranges has one reading per angle
• each call should finish well under a second: at most 100 candidates, 360 beams and 200 walls

Goals

• Predict the scan a robot would see from a candidate pose on a known map
• Score a candidate by how many beams agree with the measured scan, within a tolerance
• Pick the best candidate and see how one landmark breaks a room's symmetry