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

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.

Something is at 100 cm. The cell at the end is occupied.

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 model, in full

for each beam (angle, d) in the scan:
    direction = robot heading + angle
    for r stepping from 0 to d - CELL:
        mark the cell at (x + r*sin, y + r*cos) FREE
    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. Stepping in whole cells along a diagonal ray skips cells, and the free space comes out speckled. Half a cell is the usual choice. 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 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.

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.

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.

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 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.

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

Step in whole cells instead of half and count how many cells you now mark. Where did the others go?
Mark the free cells all the way to d instead of stopping short. What happens to the hit cell?
Work out how far apart two neighbouring beams of scan() are at 50 cm and at 150 cm.