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

Where the Echoes Land

laser scan · occupancy grid · world to cell · histogram

A robot that cleans a school corridor at night wants a quick picture of where its laser echoes come from. It cuts the floor into square cells res metres wide and counts, for each cell, how many echoes landed in it.

The scan. The robot's pose is (x, y, theta) (x to the right, y up, in metres; theta in radians counter-clockwise from +x). The scan is a list of readings (a, r): the beam left at angle a from the robot's heading (counter-clockwise positive) and its echo came back from r metres away, so the echo is at the world point

\big(x + r\cos(\theta + a),\; y + r\sin(\theta + a)\big).

A reading with r >= max_range means no echo (the beam met nothing in range): skip it.

The grid. The map has rows rows and cols columns. Its bottom-left corner is at (0, 0) and row 0 is the top row, so the point (px, py) lies in the cell

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

and it is off the map (skip it) when px < 0, py < 0, px >= cols * res or py >= rows * res.

Write hit_counts(pose, scan, max_range, rows, cols, res) that returns a list of rows lists of cols integers: counts[row][col] is the number of echoes in that cell.

Examples

Input:  pose = (1.5, 1.5, 0), scan = [(0, 2.0), (math.pi / 2, 1.2), (-math.pi / 2, 1.3), (math.pi, 4.0)],
        max_range = 4.0, rows = 3, cols = 4, res = 1
Output: [[0, 1, 0, 0], [0, 0, 0, 1], [0, 1, 0, 0]]
Explanation: the echo straight ahead is at (3.5, 1.5): column 3, row 2 - 1 = 1.
The one to the left is at (1.5, 2.7), row 0; the one to the right at (1.5, 0.2), row 2.
The reading behind is 4.0 = max_range: no echo.

Input:  pose = (1.1, 0.25, math.pi / 2), max_range = 3.0, rows = 3, cols = 4, res = 0.5,
        scan = [(-0.4, 1.14), (-0.2, 1.07), (0, 1.05), (0.2, 1.07), (0.4, 1.14), (0.9, 3.0), (1.4, 1.2)]
Output: [[0, 2, 2, 1], [0, 0, 0, 0], [0, 0, 0, 0]]
Explanation: the robot faces +y; five echoes trace a wall near y = 1.3, in row 0.
(0.9, 3.0) has no echo, and the echo of (1.4, 1.2) is at x = -0.08: off the map.

Input:  pose = (0.5, 0.5, 0), scan = [(0, 2.0), (0, 1.0)], max_range = 2.0, rows = 1, cols = 4, res = 1
Output: [[0, 1, 0, 0]]
Explanation: a reading of exactly max_range is no echo, even though (2.5, 0.5) is on the map.

Constraints

  • answers are exact integers; no echo in the tests lies within 1e-6 of a cell border

Goals

  • Turn each laser reading (angle, range) into a point in the world frame
  • Drop readings with no echo and points that fall off the map
  • Count how many echoes land in each grid cell
Starting Python…