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

The Night Shift Draws the Map

occupancy grid · log-odds · clamping · line rasterisation · mapping

A security robot patrols an office at night and builds a map from its laser scans. Each cell keeps an integer log-odds score (as on many small robots, where whole numbers are cheaper than floats): positive means occupied, negative free, 0 unknown. Every cell starts at 0.

The map. rows by cols square cells, res metres wide. The map's bottom-left corner is at (0, 0) (x to the right, y up) and row 0 is the top row, so the point (px, py) lies in the cell

(row, col) = ((rows - 1) - floor(py / res), floor(px / res))

which is on the map when 0 <= row < rows and 0 <= col < cols (the formula gives whole numbers for points off the map too).

The scans. scans is a list of (pose, readings). The pose is (x, y, theta) (theta in radians counter-clockwise from +x) and each reading (a, r) is a beam at angle a from the heading with range r. If r < max_range the beam hit something at distance r; otherwise there was no echo and the beam is followed to max_range. Its end point is (x + d\cos(\theta + a),\ y + d\sin(\theta + a)) with d that distance.

One beam. Let A = (r0, c0) be the robot's cell and B = (r1, c1) the end point's cell, dr = r1 - r0, dc = c1 - c0, n = max(|dr|, |dc|). The beam crosses the cells

\left(r_0 + \left\lfloor \frac{k \cdot dr}{n} + \frac12 \right\rfloor,\; c_0 + \left\lfloor \frac{k \cdot dc}{n} + \frac12 \right\rfloor\right), \qquad k = 0, 1, \ldots, n - 1

(halves round up): each gets l_free (negative). Then the end cell B gets l_occ (positive), only if the beam hit something. Cells off the map are skipped. After every single change the cell's score is clamped into [l_lo, l_hi].

Process the scans in order and each scan's readings in order. Write build_map(rows, cols, res, scans, max_range, l_occ, l_free, l_lo, l_hi) that returns the map as a list of rows strings: character col of string row is '#' if that cell's score is positive, '.' if it is negative, and '?' if it is 0.

The setup provides box_room(x0, y0, x1, y1, poses, beams, max_range), which simulates scans in an empty rectangular room with corners (x0, y0) and (x1, y1): beams readings per pose, evenly spread over a full turn. Print "\n".join(build_map(...)) to see a map.

Examples

Input:  rows = 3, cols = 5, res = 1, max_range = 5.0, l_occ = 2, l_free = -1, l_lo = -3, l_hi = 3,
        scans = [((0.5, 1.5, 0), [(0, 3.3), (math.pi / 2, 1.2), (-math.pi / 2, 5.0)]),
                 ((2.5, 0.5, math.pi / 2), [(0, 2.3)])]
Output: ['#?#??', '...#?', '.?.??']
Explanation: from cell (1, 0) the first beam crosses (1, 0), (1, 1), (1, 2) and hits (1, 3);
the second hits (0, 0); the third has no echo and frees (1, 0) and (2, 0) before leaving the map.
From (2, 2) the last beam crosses (2, 2), (1, 2) and hits (0, 2).

Input:  rows = 1, cols = 5, res = 1, max_range = 5.0, l_occ = 2, l_free = -1, l_lo = -3, l_hi = 3,
        scans = [((0.5, 0.5, 0), [(0, 5.0)] * 5), ((0.5, 0.5, 0), [(0, 2.4)] * 2)]
Output: ['..#..']
Explanation: an empty aisle, then a trolley. Five beams with no echo push every cell to -5, clamped at -3.
Two echoes from cell 2 then give -3 + 2 + 2 = 1. Without the clamp it would be -1.

Constraints

  • every pose is on the map; no end point in the tests lies within 1e-6 of a cell border
  • the map is compared exactly

Goals

  • Fuse many beams from several poses into one occupancy grid
  • Clamp each cell's evidence after every update, so old evidence cannot outvote new readings forever
  • Turn the evidence into a map of occupied, free and unknown cells
Starting Python…