Ray casting the map

The forward model: given a map and a pose, what would the sensor read?

U8.5MappingUniversity35 min

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

Guide: SLAM explained: simultaneous localisation and mapping

U8.2 went from a reading to a map. This lesson goes the other way: given a map and a pose, what would the sensor read? That is the forward sensor model, and it is the piece U7 quietly assumed.

def cast(x, y, h):
    step out from (x, y) along heading h
    the first occupied cell you enter is the return
    if you leave the map first, it is a timeout

Why it matters

It closes the loop with localisation. U7 weighted each particle by comparing a measured range with a predicted one, and the prediction was hard-coded as 200 - y because the map was one wall. With a grid and a ray caster, the same filter works in any room, and the pair of them is the standard architecture: a grid map, and MCL localising against it by ray casting per particle per beam.

It is how you check a map. Cast a ray from where the robot is, compare with what the sensor says, and the residual tells you whether the map is right. Large residuals in one direction mean the map is stale or the pose is wrong. This is the map's own innovation, in the U6 sense.

It is what a planner asks. "Would I be able to see that from over there?" is a ray cast, and it is how a robot decides where to go to learn the most.

The cost, and what everybody does about it

Marching a ray in 1 cm steps over 150 cm is 150 array lookups. One beam, one pose: nothing. A particle filter with 500 particles and 8 beams at 10 Hz is six million lookups a second, and that is on the edge.

The standard answers, in order of how often they are reached for:

1. Subsample the beams. Use two of the eight. The readings are correlated anyway, and a filter fed 8 nearly-identical beams is overconfident, so throwing some away often improves the estimate as well as the speed.
2. A likelihood field. Precompute, once, the distance from every cell to the nearest occupied cell. Then scoring a beam is one lookup of the hit point instead of a march. This is what most production MCL uses, and it is smoother to optimise against because it has no discontinuities where a ray just clips a corner.
3. Amanatides and Woo's algorithm, the standard grid traversal from voxel ray tracing: step cell to cell rather than in fixed increments, so no cell is visited twice and none is skipped.

Doing it

from bugbot import *
import math
connect()

CELL, W = 5.0, 40
L_OCC, L_FREE, L_MAX, FAR = 0.85, -0.4, 8.0, 170.0
START_X, START_Y = 100.0, 50.0
grid = [0.0] * (W * W)

def bump(x, y, amount):
    c, r = int(x / CELL), int(y / CELL)
    if 0 <= c < W and 0 <= r < W:
        grid[r * W + c] = max(-L_MAX, min(L_MAX, grid[r * W + c] + amount))

for i in range(20):
    px, py = position()
    x, y, h = START_X + px, START_Y + py, heading()
    for a, d in scan():
        th = math.radians(h + a)
        sx, sy = math.sin(th), math.cos(th)
        r = 0.0
        while r < min(d, FAR) - CELL:
            bump(x + r * sx, y + r * sy, L_FREE)
            r += CELL / 2
        if d < FAR:
            bump(x + d * sx, y + d * sy, L_OCC)
    wait(0.1)

def cast(x, y, h, limit=250.0):
    th = math.radians(h)
    sx, sy = math.sin(th), math.cos(th)
    r = 0.0
    while r < limit:
        c, row = int((x + r * sx) / CELL), int((y + r * sy) / CELL)
        if not (0 <= c < W and 0 <= row < W):
            return limit
        if grid[row * W + c] > 1.0:
            return r
        r += 1.0
    return limit

print("from here the map predicts", round(cast(START_X, START_Y, 0.0), 1))
print("the sensor says            ", round(distance(), 1))
print("from 20 cm back it predicts", round(cast(START_X, START_Y - 20.0, 0.0), 1))

Run this in the simulator

The prediction comes out a few centimetres short of the reading, every time, and that is not a bug. A caster stops when it enters an occupied cell, so it reports the range to the near edge of a 5 cm cell rather than to the surface inside it. Here the map predicts 95.0 cm and the sensor reads 99.1 cm, most of a cell short, because the wall's face at y = 150 sits right on a cell boundary and readings a little short of it mark the cell below. Over surfaces that fall anywhere in a cell the bias averages about half a cell, and it is always systematic, which matters: a localiser scored on these predictions will be pulled forward unless the bias is either modelled or absorbed into a measurement noise large enough to cover it. Half a cell is another argument for a cell size smaller than the sensor's noise.

Unknown is not occupied

A ray that crosses cells the robot has never seen has to decide what they are, and the choice is a policy, not a fact.

• Treat unknown as free, and the caster reports the range to the nearest thing you actually know about. This is right for checking a map and for localisation, where you only want to be scored on evidence you have.
• Treat unknown as blocked, and the caster reports the edge of knowledge. This is right for a planner that must not drive into a place it has not looked at.

Both are used, in the same system, for different questions. Write the caster so the caller says which.

Task: predict a reading from the map

Standing at (100, 50), build a map from the scans. Then ray cast from a pose 20 cm behind the robot and print predicted:. Drive back 20 cm, measure, and print measured: and error:.

from bugbot import *
import math
connect()

CELL, W = 5.0, 40
L_OCC, L_FREE, L_MAX, FAR = 0.85, -0.4, 8.0, 170.0
START_X, START_Y = 100.0, 50.0
grid = [0.0] * (W * W)

Challenges

1. Cast rays at the eight angles scan() uses and compare the whole predicted fan with the real one.
2. Add a mode that treats unknown cells as blocked and see how much shorter the predictions get.
3. Time 1,000 casts. How many particles could you afford at 10 Hz?