Occupancy grid mapping

Lesson 3 assumed the map was given. Where does it come from? The simplest version of the mapping problem assumes the opposite of localization: the robot's poses are known exactly, and the task is to build the map from the sensor readings. That assumption is never quite true, but it is a useful building block. It is exactly the situation inside many SLAM systems, which estimate the poses first and then build the grid from them, and it is how a map is rebuilt after a SLAM system has optimized its trajectory. This lesson derives the occupancy grid mapping algorithm, works through its log-odds update by hand, and discusses the practical choices that determine map quality: the inverse sensor model, resolution, thresholds and handling of dynamic objects.

The mapping problem with known poses

The full mapping problem is to estimate the posterior over maps given all measurements \( z_{1:t} \) and poses \( x_{1:t} \), written \( p(m \mid z_{1:t}, x_{1:t}) \). A map with a million cells, each occupied or free, has \( 2^{1{,}000{,}000} \) possible configurations, so this cannot be computed directly. Occupancy grid mapping makes a strong simplifying assumption: each cell is estimated independently of every other cell. The posterior factorizes into a product over cells,

and each cell becomes a binary estimation problem, occupied or free, with a static state. This independence assumption is wrong, because neighboring cells on a wall are strongly correlated, and it causes some artifacts discussed below, but it makes mapping fast and simple, and the results are good enough for most navigation.

The log-odds update

For a single binary static state, the Bayes filter becomes especially simple in log-odds form. The log-odds of a probability p is

Log-odds run from minus infinity (certainly free) through 0 (probability 0.5, unknown) to plus infinity (certainly occupied). In this form, the Bayes update for cell i after measurement \( z_t \) from pose \( x_t \) is a single addition:

Here \( p(m_i \mid z_t, x_t) \) is the inverse sensor model, the probability that the cell is occupied given only this one measurement, and \( \ell_0 \) is the prior log-odds, which is zero for a prior of 0.5. Addition avoids numerical trouble near 0 and 1, is cheap enough to run for every cell of every beam, and makes the result independent of the order of measurements. Implementations clamp the log-odds between limits (for example \( \pm 3.5 \), probabilities of about 0.03 and 0.97) so that cells can still change when the environment does.

The inverse sensor model for a laser

For a range sensor, the inverse sensor model follows from the geometry of a beam. Cells along the beam between the sensor and the measured range are probably free, because the beam passed through them. The cell (or a thin band of cells) at the measured range is probably occupied, because something reflected the beam. Cells beyond the measured range are unknown, because the beam never reached them, and receive no update. Cells outside the beam's angular width are also untouched. A beam at the sensor's maximum range with no return is either discarded or used only to clear free space up to some distance, since a missing return gives no evidence of an obstacle.

Finding the cells a beam passes through is a line-drawing problem. Bresenham's algorithm, invented in 1962 for pen plotters, steps along the line from the sensor cell to the endpoint cell using only integer additions, visiting each crossed cell once. For a 2-D lidar with 1,000 beams per scan at 10 Hz and beams up to 10 m long on a 5 cm grid, that is up to about 200 cells per beam and 2 million cell updates per second, well within a modern processor's reach. 3-D lidars produce far more beams, which is why 3-D mapping uses octrees, voxel hashing and GPU ray casting.

import numpy as np
res, n = 0.1, 60                                  # 6 m x 6 m grid, 10 cm cells
L = np.zeros((n, n))                              # log-odds, 0 = unknown
l_hit, l_miss, lmax = np.log(0.7 / 0.3), np.log(0.4 / 0.6), 3.5
def bresenham(x0, y0, x1, y1):
    cells, dx, dy = [], abs(x1 - x0), -abs(y1 - y0)
    sx, sy, e = (1 if x0 < x1 else -1), (1 if y0 < y1 else -1), dx + dy
    while True:
        cells.append((x0, y0))
        if x0 == x1 and y0 == y1: return cells
        e2 = 2 * e
        if e2 >= dy: e += dy; x0 += sx
        if e2 <= dx: e += dx; y0 += sy
# square room walls at 1 m and 5 m; robot at (3, 3) takes 4 scans of 180 beams
def true_range(x, y, a):
    t = 0.0
    while 1.0 < x + t * np.cos(a) < 5.0 and 1.0 < y + t * np.sin(a) < 5.0: t += 0.01
    return t
rng = np.random.default_rng(0)
for _ in range(4):
    for a in np.linspace(-np.pi, np.pi, 180, endpoint=False):
        r = true_range(3.0, 3.0, a) + rng.normal(0, 0.02)
        ex, ey = 3.0 + r * np.cos(a), 3.0 + r * np.sin(a)
        ray = bresenham(30, 30, int(ex / res), int(ey / res))
        for (i, j) in ray[:-1]: L[j, i] = max(-lmax, L[j, i] + l_miss)
        i, j = ray[-1]; L[j, i] = min(lmax, L[j, i] + l_hit)
P = 1 - 1 / (1 + np.exp(L))
print("occupied cells (p > 0.65):", int((P > 0.65).sum()))
print("free cells (p < 0.35):    ", int((P < 0.35).sum()))
print("unknown cells:            ", int(((P >= 0.35) & (P <= 0.65)).sum()))
print("highest p near the wall x = 1 m on row y = 3 m: %.2f; p at the robot cell: %.2f" % (P[30, 8:12].max(), P[30, 30]))

occupied cells (p > 0.65): 222
free cells (p < 0.35):     1448
unknown cells:             1930
highest p near the wall x = 1 m on row y = 3 m: 0.93; p at the robot cell: 0.03

Resolution, thresholds and map quality

Grid resolution trades detail against memory, computation and noise. A cell much smaller than the lidar's range noise or the pose uncertainty just spreads the same evidence over more cells, producing thick, fuzzy walls; a cell much larger than the robot's clearance hides doorways. For a 2-D lidar with a few centimeters of noise and good SLAM, 5 cm is the common default, with 2.5 cm for precise docking areas and 10 cm for large outdoor maps.

When a grid is saved for navigation, the probabilities are usually thresholded into three classes. ROS map_server's YAML file stores occupied_thresh (often 0.65) and free_thresh (often 0.196 or 0.25); cells above the first are obstacles, cells below the second are free, and those in between remain unknown. The navigation cost map then inflates obstacles by the robot's radius plus a safety margin.

Artifacts and dynamic objects

The independence assumption and the simple inverse model produce characteristic artifacts. Walls seen at a glancing angle get thickened or partly erased, because beams that nearly graze a wall mark wall cells as free on the way past. Thin obstacles such as chair legs may be hit by some beams and passed by others, so they flicker between occupied and free. Glass and mirrors produce no return or a return from the wrong place. Black or very shiny surfaces return weakly. More sophisticated inverse models account for beam width and incidence angle, and forward-model mapping methods estimate the most likely map jointly rather than cell by cell, at higher cost.

Dynamic objects leave trails. A person walking past the robot is marked occupied wherever they were seen, until later beams pass through those cells and clear them. Clamping log-odds keeps cells changeable, and using a slightly larger miss increment than hit increment lets the map forget transient objects faster. Some systems detect and remove moving objects before mapping, or build the map only from scans taken when the area is empty. Long-term map maintenance, where the robot updates a deployed map as the environment changes, is covered in lesson 11.

From mapping to SLAM

Occupancy grid mapping with known poses is easy; the difficulty is that the poses are not known. If odometry is used as the poses, the map warps as odometry drifts: corridors bend, rooms overlap, and a loop around a building fails to close. The rest of this course is about estimating the poses well enough that the map comes out right. Some SLAM methods (Rao-Blackwellized particle filters, lesson 7) carry a grid per particle; others (graph SLAM, lesson 8) optimize the trajectory and then build the grid once from the corrected poses. Either way, the grid-building step is the one from this lesson.