The inverse sensor model

One reading is two statements: free all the way along the ray, occupied at the end of it.

U8.2MappingUniversity30 min

Do this lesson in the simulator

Print it: worksheet (PDF) · answers (PDF) · as a web page

Guide: SLAM explained: simultaneous localisation and mapping

A range reading looks like one number. It is two statements, and the second one is the one beginners throw away.

The beam went out 100 cm and came back.

  1. Something is at 100 cm. The cell at the end is occupied.
  2. Nothing is in the first 100 cm. Every cell the beam passed through is free.

The second statement covers twenty cells rather than one, and it is what makes a grid fill in at any useful rate. A mapper that only marked the hits would produce an outline of the walls and know nothing about the floor, which is precisely backwards for a planner.

Why it is called inverse

The forward sensor model answers: given the world, what will the sensor read? That is U7's measurement model, and it is a simulation.

The inverse sensor model answers: given the reading, what is the world like? That is this lesson. The name is standard and it is worth getting straight, because the two appear in the same system and do opposite jobs. U8.5 builds the forward one out of the map this one produces.

The forward model turns a map and a pose into a reading; the inverse model turns a reading into cellsforward: map and pose in,a reading outinverse: a reading in,cells outthe wall, from the mappose (100, 50)predicts 100 cmreads 100.2 cm1 occupied20 freeunknownoppositejobs
Left, the forward model: from a map with the wall in it and the pose (100, 50), the reading should be 100 cm. Right, the inverse model: from a reading of 100.2 cm, drawn to the same scale, 20 cells become a little more free and one a little more occupied. Nothing is said about the cells beside the beam or past the wall.

The model, in full

for each beam (angle, d) in the scan:
    direction = robot heading + angle
    free = the set of cells at (x + r*sin, y + r*cos), for r stepping from 0 to d - CELL
    mark each cell in free FREE, once
    if d is a real return, not a timeout:
        mark the cell at (x + d*sin, y + d*cos) OCCUPIED

Three details carry the weight.

Step smaller than a cell, and count each cell once. Stepping in whole cells along a diagonal ray skips cells, and the free space comes out speckled. Half a cell is the usual choice, but it lands in most cells twice, so collect the cells in a set and mark each one once. One beam passing through a cell is one piece of evidence, and U8.3 depends on it counting once. The proper answer is a line-drawing algorithm, and the one everybody uses is Bresenham's, which visits exactly the cells a line passes through, once each, with integer arithmetic only.

Stop short of the hit. If the free marking runs all the way to d, it fights the occupied marking in the same cell, and the wall never becomes confident. Stop one cell short. That has a cost, which U8.4 shows on a real map: the floor cell just in front of a wall is hardly ever marked free.

A timeout is not a hit. When the beam finds nothing within range it returns its maximum. That is a statement about free space only, and marking an obstacle at maximum range puts a ring of imaginary wall around the robot. This bug is easy to write and obvious once you see the map.

Marking a timeout as a hit draws a ring of wall at the sensor's maximum range; leaving it as free space does nottimeout marked as a hitred: 25 cells of wall that is not theretimeout: free space onlyno wall where nothing was hitdashed: the real wall; dark cells: its hits; the robot turns a full circle in the middle
A robot turning on the spot with one real wall in range (dashed). Every other beam times out at the sensor's maximum. Mark those timeouts as hits and a ring of imaginary wall appears round the robot; treat them as free space only and just the real wall is drawn.

Where the beam actually points

The robot's heading is clockwise positive with 0 facing along +y, and scan() gives angles from straight ahead, negative to the left. So a beam at angle a from a pose (x, y, h) reaches, at range r:

th = math.radians(h + a)
hit_x = x + r * math.sin(th)
hit_y = y + r * math.cos(th)

Get the sine and the cosine the wrong way round and the map comes out mirrored about the diagonal, which looks plausible enough to waste an afternoon.

A beam's direction is the heading plus its angle in the fan; its x is r times the sine and its y r times the cosine+y, heading 0+xheadinghitr sin(th) = 41.8r cos(th) = 43.1r = 60roboth = 30°, the headinga = 14.1°, the beam in the fanth = h + a = 44.1°the fan's other beams
A pose turned h = 30° clockwise from +y, and the seventh beam of the fan, a = 14.1° right of ahead, so th = 44.1°. A 60 cm reading lands 41.8 cm along x (the sine) and 43.1 cm along y (the cosine). Beams left of ahead have negative a.

One ray, drawn

from bugbot import *
connect()

CELL = 5.0
X0, Y0 = 100.0, 50.0
d = distance()

free = set()
r = 0.0
while r < d - CELL:
    free.add((int(X0 / CELL), int((Y0 + r) / CELL)))
    r += CELL / 2

hit = (int(X0 / CELL), int((Y0 + d) / CELL))
print("reading", round(d, 1), "marks", len(free), "cells free and cell", hit, "occupied")
for row in range(hit[1] + 2, 8, -1):
    print(str(row).rjust(3), "#" if row == hit[1] else ("." if (hit[0], row) in free else " "))

Run this in the simulator

Twenty free cells and one occupied, from a single number. That ratio is why grids fill so fast. The loop took 39 half-cell steps to find those 20 cells, landing in nearly every one twice, which is why they go into a set.

The beam is not a line

A real depth sensor has a cone, not a ray, and treating it as a ray is a lie that mostly works. Where it stops working:

  • Glancing angles. A beam meeting a wall at a shallow angle returns a range from somewhere across the footprint, and the wall comes out smeared and thick.
  • Corners. Part of the cone sees the near surface and part sees past it, and the reported range is somewhere in between, which puts occupied cells in mid-air.
  • Wide fields. The 45 degree fan here is eight beams, so at 100 cm neighbouring beams are 10 cm apart, wider than a cell. Distant free space comes out striped until the robot turns.
One scan of eight beams, put through the model, fills the floor near the robot but leaves stripes of unknown near the wallthe wall: 8 hitsfar out: stripes,unknown betweenthe beamsclose in: the beamsoverlap, solid freerobot at (100, 50)
One real scan from (100, 50) on this mat, every beam run through the model. Near the robot the beams share cells and the floor comes out solid. At the wall the hits are 10 to 11 cm apart, two cells, so the far floor is striped until the robot turns.

The usual fixes are to trust near readings more than far ones, and to keep moving, because two viewpoints disagree about an artefact and agree about a wall.

Task: one ray, two statements

Take a reading, run the inverse sensor model along it, and print free:, the number of different cells the ray passes through, and wall y:, the mat y of the cell it ended in.

from bugbot import *
connect()

CELL = 5.0
X0, Y0 = 100.0, 50.0

Challenges

  1. Step in whole cells instead of half and count the cells again, then do the same for a 100 cm beam 30 degrees to the right. Why does only the slanting one lose cells?
  2. Mark the free cells all the way to d instead of stopping short. What happens to the hit cell?
  3. Work out how far apart two neighbouring beams of scan() are at 50 cm and at 150 cm.