Problem 592574 · easy · Robotics · Level 05 Planning and Mapping

One Beam Clears a Path

occupancy grid · log-odds · line rasterisation · inverse sensor model

A library robot builds an occupancy map of the reading room. For every cell it keeps the log-odds that the cell is occupied: positive means probably occupied, negative probably free, 0 unknown. One laser beam changes the map like this.

The map. logodds[row][col] is a grid of floats with 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))

The cell is on the map when 0 <= row < rows and 0 <= col < cols; this formula still gives a pair of whole numbers for a point off the map.

The beam. The robot's pose is (x, y, theta) (theta in radians counter-clockwise from +x) and the beam is (a, r): it left at angle a from the heading and reports range r. If r < max_range the beam hit something at distance d = r. If r >= max_range there was no echo and the beam is followed to d = max_range. Its end point is (x + d\cos(\theta + a),\ y + d\sin(\theta + a)).

The cells it crosses. Let A = (r0, c0) be the robot's cell and B = (r1, c1) the end point's cell. With dr = r1 - r0, dc = c1 - c0 and n = max(|dr|, |dc|), the k-th cell for k = 0, 1, ..., n is

\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)

(halves round up; when n = 0 the only cell is A). Cell k = n is B.

The update. Add l_free (a negative number) to cells k = 0, ..., n - 1. Add l_occ (positive) to the end cell B if the beam hit something; leave B unchanged if there was no echo. Cells off the map are skipped.

Write beam_update(logodds, res, pose, beam, max_range, l_occ, l_free) that returns the updated grid.

Examples

Input:  logodds = 3 x 5 zeros, res = 1, pose = (0.5, 1.5, 0), beam = (0, 3.2), max_range = 5, l_occ = 0.9, l_free = -0.4
Output: [[0.0, 0.0, 0.0, 0.0, 0.0], [-0.4, -0.4, -0.4, 0.9, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0]]
Explanation: the robot is in (1, 0); the echo at (3.7, 1.5) is in (1, 3).

Input:  logodds = [[0.0, 0.5, 0.0, 0.0, 0.0], [0.0, -0.4, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0]],
        res = 1, pose = (0.5, 0.5, 0), beam = (0.6, 3.4), max_range = 5, l_occ = 0.9, l_free = -0.4
Output: [[0.0, 0.5, 0.0, 0.9, 0.0], [0.0, -0.8, -0.4, 0.0, 0.0], [-0.4, 0.0, 0.0, 0.0, 0.0]]
Explanation: the echo at (3.306, 2.420) is in (0, 3). From (2, 0): dr = -2, dc = 3, n = 3;
k = 1 gives row 2 + floor(-2/3 + 1/2) = 1 and column 1; k = 2 gives (1, 2).

Input:  logodds = 2 x 4 zeros, res = 0.5, pose = (0.25, 0.75, 0), beam = (0, 1.5), max_range = 1.5, l_occ = 0.85, l_free = -0.4
Output: [[-0.4, -0.4, -0.4, 0.0], [0.0, 0.0, 0.0, 0.0]]
Explanation: no echo: the beam is followed 1.5 m to (1.75, 0.75), cell (0, 3), which stays unchanged.

Constraints

  • the robot is on the map; no end point in the tests lies within 1e-6 of a cell border
  • answers are compared with a tolerance of 1e-6; returning a changed logodds or a new grid are both fine

Goals

  • Find the robot's cell and the cell where a beam ends
  • List the cells between them by an exact rounding rule
  • Mark the crossed cells free and the end cell occupied, except for a reading with no echo
Starting Python…