Occupancy Grid Mapping

Occupancy grid mapping builds a map from range measurements taken at known poses. Localization estimates the pose given the map; occupancy grid mapping estimates the map given the poses (Thrun, Burgard and Fox, 2005, Chapter 9). It does not estimate the poses, and it does not model how an error in a pose spreads into the map, so on its own it is not SLAM. Many SLAM systems run it as a last step: they estimate the robot’s path first and then build a grid along that path for the planner. This section builds a map from a simulated LIDAR at the true poses. It then maps the same scans along a wheel odometry path, which drifts.

The grid as a map

An occupancy grid divides the plane into square cells

m_i

. A cell is occupied (

m_i = 1

) or free (

m_i = 0

). The map is assumed static: nothing in the room moves while the robot maps it. Given the measurements

z_{1:t}

and the poses

s_{1:t}

, we want the posterior

p(m \mid z_{1:t}, s_{1:t}).

The actions

a_{1:t}

do not appear, because the poses are given. A grid of

N

cells has

2^N

possible maps, far too many to store a probability for each. The standard algorithm therefore keeps only one marginal per cell and treats the cells as independent:

p(m \mid z_{1:t}, s_{1:t}) \approx \prod_i p(m_i \mid z_{1:t}, s_{1:t}).

This is a modeling choice, and it loses information. A LIDAR return at range

z

means that the first occupied cell along the beam is at

z

and that every cell before it is free. That is one statement about a group of cells, and a product of marginals cannot represent it. Neighboring beams of one scan, and scans taken from nearby poses, see the same surfaces, so their evidence is correlated. The update below counts each of them as independent. The factorized grid is an approximation even for a LIDAR with very thin beams, and the last part of this section returns to the errors this approximation causes. After the factorization, each cell is a binary state with its own Bayes filter. Because the map is static, the motion model of each filter is the identity. The prediction step leaves the belief unchanged and only the measurement update remains.

Log-odds update

Apply Bayes rule to one cell and separate the newest measurement

z_t

from the older ones:

\begin{aligned} p(m_i \mid z_{1:t}, s_{1:t}) &= \eta\; p(z_t \mid m_i, z_{1:t-1}, s_{1:t}) \\ &\quad \times p(m_i \mid z_{1:t-1}, s_{1:t}), \end{aligned}

where

\eta = 1 / p(z_t \mid z_{1:t-1}, s_{1:t})

normalizes. Two assumptions simplify the right side. The new pose alone carries no information about the map, so

p(m_i \mid z_{1:t-1}, s_{1:t}) = p(m_i \mid z_{1:t-1}, s_{1:t-1})

, the previous posterior. Given

m_i

and

s_t

, the measurement is treated as independent of the earlier measurements:

p(z_t \mid m_i, z_{1:t-1}, s_{1:t}) \approx p(z_t \mid m_i, s_t).

The factorization loses accuracy in the second assumption. The scan

z_t

depends on the whole map, not on

m_i

alone, and the earlier scans carry information about the other cells. Treating the approximation as exact is what makes correlated scans count as independent evidence. Bayes rule then turns the measurement model into the inverse sensor model

p(m_i \mid z_t, s_t)

, the occupancy probability of the cell given only the current scan:

p(z_t \mid m_i, s_t) = \frac{p(m_i \mid z_t, s_t)\; p(z_t \mid s_t)}{p(m_i)},

using

p(m_i \mid s_t) = p(m_i)

once more. After substitution, the posterior still contains

p(z_t \mid s_t)

and

\eta

, which are hard to compute. Write the same expression for

m_i = 0

and divide one by the other. The normalizers cancel and the odds multiply:

\begin{aligned} \frac{p(m_i = 1 \mid z_{1:t}, s_{1:t})}{p(m_i = 0 \mid z_{1:t}, s_{1:t})} &= \frac{p(m_i = 1 \mid z_t, s_t)}{p(m_i = 0 \mid z_t, s_t)} \cdot \frac{p(m_i = 0)}{p(m_i = 1)} \\ &\quad \cdot \frac{p(m_i = 1 \mid z_{1:t-1}, s_{1:t-1})}{p(m_i = 0 \mid z_{1:t-1}, s_{1:t-1})}. \end{aligned}

The filter therefore stores the log-odds of occupancy,

l_{t,i} = \log \frac{p(m_i = 1 \mid z_{1:t}, s_{1:t})}{1 - p(m_i = 1 \mid z_{1:t}, s_{1:t})},

and the product becomes a sum:

\begin{aligned} l_{t,i} &= l_{t-1,i} + \log \frac{p(m_i = 1 \mid z_t, s_t)}{1 - p(m_i = 1 \mid z_t, s_t)} - l_0, \\ l_0 &= \log \frac{p(m_i = 1)}{1 - p(m_i = 1)}. \end{aligned}

Every cell starts at

l_0

. The middle term is the inverse sensor model in log-odds form. It is a posterior, so it already contains the prior

p(m_i)

, and subtracting

l_0

removes that second copy. The subtraction is correct only if the inverse model was built with the same prior that the grid starts from. For a cell about which the scan carries no information, the inverse model equals the prior, so its update is zero. The probability is recovered as

p = 1 - 1/(1 + e^{l_{t,i}})

. Thrun, Burgard and Fox (2005) derive this update for a general binary state in Section 4.1.4 and apply it to grids in Section 9.2. Adding log-odds also avoids the round-off of multiplying probabilities near 0 or 1.

Inverse sensor model for a range beam

A LIDAR scan is a list of beams,

z_t = (z_t^1, \ldots, z_t^K)

, where

z_t^k

is the range measured by beam

k

at bearing

\theta_k

in the sensor frame. A beam that hits nothing reports the maximum range

z_{\max}

. The inverse model below follows Table 9.2 of Thrun, Burgard and Fox (2005). For each cell, compute the range

r_i

and bearing

\phi_i

of its center from the robot at

s_t

, and let

k

be the beam whose bearing is closest to

\phi_i

. Then:

If $z_t^k < z_{\max}$ and $|r_i - z_t^k| \le \alpha/2$ , the beam ended in the cell, and $p(m_i = 1 \mid z_t, s_t) = 0.7$ .
Otherwise, if $r_i \le z_t^k$ , the beam passed through the cell, and $p(m_i = 1 \mid z_t, s_t) = 0.4$ .
Otherwise the cell lies behind the return or beyond $z_{\max}$ . The scan carries no information about it, and the model returns the prior.

\alpha

is the thickness of a surface, 10 cm here. A beam at maximum range marks every cell out to

z_{\max}

as free and none as occupied. The book’s model also has a beam width

\beta

and skips cells more than

\beta/2

away from the nearest beam. Here the beams are 1° apart and

\beta

equals that spacing, so every cell within range belongs to exactly one beam, and a scan updates a cell at most once. The values 0.7 and 0.4 are set by hand. With a prior of 0.5 they add

\log(0.7/0.3) = 0.85

\log(0.4/0.6) = -0.41

to the log-odds, and so they set how fast the map converges. In principle the inverse model follows from the forward model

p(z_t \mid s_t, m)

, such as the beam model of the sensor, together with the map prior: marginalize the full posterior over all maps that agree on cell

i

. That sum over maps cannot be computed. Section 9.3 of the book approximates it by simulating scans in random maps and fitting a function that predicts each cell’s occupancy from the scan. Hand-set values like the ones here are the common shortcut. The code also clips the log-odds to

\pm 5

, which keeps every probability between 0.007 and 0.993. Clipping is a heuristic added to the Bayes update, for two reasons. Because correlated scans count as independent evidence, the exact sum grows without bound, and the map becomes far more certain than the data justify. A cell with a very large log-odds would also take many scans to change if something in the room did move, a case the static-map assumption ignores. The code uses a 10 m by 6 m room, the same one as in the correspondence estimation section, a 2D LIDAR with 360 beams and an 8 m range, and 5 cm cells. The first cell simulates the LIDAR by casting rays against the walls. The second implements the grid, the inverse model and the update.

walls = np.array(
    [
        [[0, 0], [10, 0]], [[10, 0], [10, 6]], [[10, 6], [0, 6]], [[0, 6], [0, 0]],
        [[6, 2], [7.5, 2]], [[7.5, 2], [7.5, 3.5]], [[7.5, 3.5], [6, 3.5]], [[6, 3.5], [6, 2]],
        [[3, 6], [3, 4.2]],
    ],
    dtype=float,
)
MAX_RANGE = 8.0


def wrap(angle):
    return (angle + np.pi) % (2 * np.pi) - np.pi


def lidar_ranges(pose, n_beams=360, sigma=0.01):
    """Ray-cast the walls from pose; return beam angles (sensor frame) and ranges z_t^1..z_t^K."""
    x, y, th = pose
    angles = np.linspace(-np.pi, np.pi, n_beams, endpoint=False)
    ranges = np.full(n_beams, MAX_RANGE)
    for b, angle in enumerate(angles):
        d = np.array([np.cos(th + angle), np.sin(th + angle)])
        for p, q in walls:
            A = np.array([d, p - q]).T
            if abs(np.linalg.det(A)) < 1e-12:
                continue
            t, u = np.linalg.solve(A, p - np.array([x, y]))
            if t > 0 and 0 <= u <= 1 and t < ranges[b]:
                ranges[b] = t
    hit = ranges < MAX_RANGE
    ranges[hit] += np.random.normal(0, sigma, hit.sum())
    return angles, np.minimum(ranges, MAX_RANGE)  # a return beyond the range reads as no return

RES = 0.05  # cell size in meters
ORIGIN = np.array([-2.0, -2.0])  # world coordinates of the grid corner
NX, NY = int(14 / RES), int(10 / RES)
# world coordinates of every cell center
CELL_X, CELL_Y = np.meshgrid(ORIGIN[0] + (np.arange(NX) + 0.5) * RES,
                             ORIGIN[1] + (np.arange(NY) + 0.5) * RES, indexing="ij")


def logodds(p):
    return np.log(p / (1 - p))


def probability(L):
    return 1.0 - 1.0 / (1.0 + np.exp(L))


P_0, P_OCC, P_FREE = 0.5, 0.7, 0.4  # prior, and the inverse model's two outputs
L_0, L_OCC, L_FREE = logodds(P_0), logodds(P_OCC), logodds(P_FREE)
ALPHA = 0.1  # thickness of a surface in meters
L_MAX = 5.0  # clipping bound on the log-odds


def new_grid():
    return np.full((NX, NY), L_0)


def inverse_range_model(pose, angles, ranges):
    """Log-odds of p(m_i | z_t, s_t) for every cell; L_0 where the scan says nothing."""
    x, y, th = pose
    r = np.hypot(CELL_X - x, CELL_Y - y)  # range of each cell center
    phi = wrap(np.arctan2(CELL_Y - y, CELL_X - x) - th)  # bearing in the sensor frame
    k = np.round((phi - angles[0]) / (angles[1] - angles[0])).astype(int) % len(angles)
    z = ranges[k]  # the range of the beam nearest in bearing
    l = np.full(r.shape, L_0)
    l[r <= z] = L_FREE  # in front of the return (all of the beam if it hit nothing)
    l[(z < MAX_RANGE) & (np.abs(r - z) <= ALPHA / 2)] = L_OCC  # at the return
    return l


def update_grid(L, pose, angles, ranges):
    """One log-odds update with the scan z_t taken at s_t; return the cells it changed."""
    l = inverse_range_model(pose, angles, ranges)
    L += l - L_0
    np.clip(L, -L_MAX, L_MAX, out=L)
    return l != L_0

Start with one scan from the middle of the room, at

(4, 2.75)

, facing along

+x

pose_one = np.array([4.0, 2.75, 0.0])
angles_one, ranges_one = lidar_ranges(pose_one)
L_one = new_grid()
touched = update_grid(L_one, pose_one, angles_one, ranges_one)
in_room = (CELL_X > 0) & (CELL_X < 10) & (CELL_Y > 0) & (CELL_Y < 6)
print(f"cells inside the room: {in_room.sum()}, updated by one scan: {(touched & in_room).sum()}")
print(f"occupied cells after one scan: probability {probability(L_one.max()):.2f}")

cells inside the room: 24000, updated by one scan: 18819
occupied cells after one scan: probability 0.70

Occupancy grid after one LIDAR scan from the middle of the room, facing along x. The free space in front of the walls is light gray, the walls hit by the scan are dark gray, and the regions behind the box and the partition stay at the prior. Every 30th beam and the true walls are overlaid.

The scan updates 18,819 of the 24,000 cells inside the room. A cell at a return gets one occupied update, which takes it from 0.5 to 0.7, so after one scan the walls are only dark gray. Every cell more than 5 cm in front of a return gets one free update, from 0.5 to 0.4, so the free space is only slightly lighter than the prior. The cells that stay at the prior are the shadows behind the box and the partition, the inside of the box, and the area beyond the walls. The blue lines are every 30th beam, drawn to its return, and the dashed lines are the true walls.

A map from the true path

The robot drives a loop around the room and scans every 0.4 m. It turns in place at each corner, and at the end it turns back to its starting heading, so the last pose equals the first. Each scan is applied at its true pose.

waypoints = np.array([[1.5, 1], [8.5, 1], [8.5, 5], [4.5, 5], [4.5, 3], [1.5, 3], [1.5, 1]])
positions = []
for a, b in zip(waypoints[:-1], waypoints[1:]):
    n = int(np.ceil(np.linalg.norm(b - a) / 0.4))
    positions += [a + (b - a) * k / n for k in range(n)]
positions = np.array(positions + [waypoints[-1]])
step = np.diff(positions, axis=0)
headings = np.arctan2(step[:, 1], step[:, 0])  # each pose faces the next one
true_path = np.c_[positions, np.append(headings, headings[-1])]
# turn in place at the end so that the loop closes in heading as well as position
true_path = np.vstack([true_path, [*waypoints[-1], 0.0]])

scans = [lidar_ranges(pose) for pose in true_path]
L_true = new_grid()
for pose, (angles, ranges) in zip(true_path, scans):
    update_grid(L_true, pose, angles, ranges)
print(f"{len(true_path)} scans along a path of {np.sum(np.linalg.norm(step, axis=1)):.1f} m")
box = (CELL_X > 6) & (CELL_X < 7.5) & (CELL_Y > 2) & (CELL_Y < 3.5)
never_seen = (L_true == L_0) & in_room
print(f"cells inside the room still at the prior: {never_seen.sum()}, of them inside the box: {(never_seen & box).sum()}")
free = (L_true < L_0) & in_room
print(f"free cells: {free.sum()}, of them at the clipping bound: {np.sum(free & (L_true <= -L_MAX))}")

58 scans along a path of 22.0 m
cells inside the room still at the prior: 782, of them inside the box: 782
free cells: 22280, of them at the clipping bound: 22247

Occupancy grid from 58 scans at the true poses along a loop. Walls, box and partition are thin black lines, the free space is white, and the inside of the box and the area beyond the outer walls stay gray at the prior.

The walls, the box and the partition are thin black lines, and the free space is white. Of the 22,280 free cells inside the room, 22,247 have reached the clipping bound of 0.007. A cell needs 13 free updates to get there. The 782 cells inside the room that are still at the prior of 0.5 are all inside the box, where no beam can reach. Beyond the thin occupied band of each outer wall, the area outside the room also stays gray. Every scan sits at its exact pose, so each wall is drawn once.

A map from wheel odometry

Odometry estimates the poses by integrating wheel encoder readings. The robot here has two wheels 0.16 m apart. Each motion between two true poses is split into a turn in place, a straight drive and a second turn, as in the odometry motion model (Thrun, Burgard and Fox, 2005, Section 5.4). For each part, the code computes how far each wheel rolled. The right encoder reports 0.2 percent more distance than the wheel rolls, as it would if the right wheel were slightly smaller than the radius the software assumes. Both readings also get a little noise. Integrating these readings gives the odometry path, and the same scans are then mapped at the odometry poses.

WHEELBASE = 0.16  # distance between the two wheels in meters
RIGHT_SCALE = 1.002  # the right encoder reports 0.2 percent more distance than the wheel rolls
ENCODER_NOISE = 0.0005  # standard deviation of each encoder reading per motion segment, in meters


def encoder_readings(d_left, d_right):
    """What the two encoders report for the true distances rolled by the wheels."""
    return (d_left + np.random.normal(0, ENCODER_NOISE),
            RIGHT_SCALE * d_right + np.random.normal(0, ENCODER_NOISE))


def dead_reckon(pose, d_left, d_right):
    """Integrate one segment of differential-drive motion from the wheel distances."""
    ds, dth = (d_left + d_right) / 2, (d_right - d_left) / WHEELBASE
    mid = pose[2] + dth / 2
    return np.array([pose[0] + ds * np.cos(mid), pose[1] + ds * np.sin(mid), wrap(pose[2] + dth)])


odometry_path = [true_path[0].copy()]
for prev, curr in zip(true_path[:-1], true_path[1:]):
    # the true motion as rotate, translate, rotate (the odometry motion model)
    dx, dy = curr[:2] - prev[:2]
    trans = np.hypot(dx, dy)
    rot1 = wrap(np.arctan2(dy, dx) - prev[2]) if trans > 0 else 0.0
    rot2 = wrap(curr[2] - prev[2] - rot1)
    pose = odometry_path[-1]
    for d_left, d_right in ((-rot1 * WHEELBASE / 2, rot1 * WHEELBASE / 2), (trans, trans),
                            (-rot2 * WHEELBASE / 2, rot2 * WHEELBASE / 2)):
        if d_left or d_right:
            pose = dead_reckon(pose, *encoder_readings(d_left, d_right))
    odometry_path.append(pose)
odometry_path = np.array(odometry_path)

L_odom = new_grid()
for pose, (angles, ranges) in zip(odometry_path, scans):
    update_grid(L_odom, pose, angles, ranges)

drift = np.linalg.norm(odometry_path[-1, :2] - true_path[-1, :2])
heading_drift = np.rad2deg(wrap(odometry_path[-1, 2] - true_path[-1, 2]))
print(f"odometry error at the end of the loop: {drift:.2f} m, {heading_drift:.1f} degrees")

odometry error at the end of the loop: 1.36 m, 19.3 degrees

Occupancy grid from the same scans placed at the drifting odometry poses. The odometry path bends away from the dashed true path, the room is tilted, the outer walls appear twice and the box is smeared.

With odometry, the pose error at the end of the loop is 1.36 m and 19.3 degrees. On a straight 0.4 m step, the scale error turns the robot by

0.4 \times 0.002 / 0.16 = 0.005

rad, about 0.3°, and these small turns add up over the loop. The odometry path (solid) moves away from the true path (dashed) as the heading error grows. Each scan is placed at a wrong pose, so the same wall is drawn several times at different positions and angles. The room is tilted, the outer walls are doubled and the box is smeared. The update uses the poses as given and cannot detect the error. To correct it, the poses have to be estimated from the scans, for example by aligning each scan with the map. The alignment needs correspondences between scan and map, covered in the next section. Estimating the poses and the map together is SLAM.

Limits of independent cells

The map from the true poses looks right, but the independent-cell approximation behind it has two shortcomings. Each cell is updated as if alone, so the map cannot express that a return means “the first occupied cell along this beam is near

z

”. The update also counts every scan as new evidence, although consecutive scans see the same walls from almost the same pose. That is why the free space reached the clipping bound: 58 scans of the same room are worth far less than 58 independent measurements. The first problem is visible with wide beams. A sonar cone returns one range for a whole sector, and the inverse model marks every cell at that range as occupied, although only one of them produced the echo. Two overlapping cones can then give conflicting evidence about the same cells. The update resolves the conflict by counting how often each kind of evidence arrived, even when one map would explain both readings. For example, when a sonar passes an open door, the door tends to appear closed in the map (Thrun, Burgard and Fox, 2005, Section 9.4). Section 9.4 of the book replaces the inverse model with the forward model

p(z_t \mid s_t, m)

and searches for the single most probable map:

m^* = \arg\max_m \Big[ \sum_t \log p(z_t \mid s_t, m) + l_0 \sum_i m_i \Big].

The search starts from an empty map and flips one cell at a time whenever the flip raises this score. Each likelihood term scores the whole map, so a sonar return is explained by any occupied cell in its cone, and the search can settle on one map that explains overlapping cones together. The result is a single map, with no occupancy probability per cell, so the remaining uncertainty is lost. The search is a batch computation that keeps all the data in memory, it can stop at a local maximum, and it cannot run incrementally while the robot drives.

Occupancy grids in practice

Occupancy grids remain a basic map representation in mobile robot navigation. ROS 2 Nav2 plans and controls on costmaps, which are grids built from layers (Nav2 documentation). The static layer holds the map, received from a SLAM package or from the map server. Nav2 recommends SLAM Toolbox for building that map, and its localizer, AMCL, estimates the robot’s pose in the same map. The obstacle layer updates the grid from a 2D LIDAR or a depth camera while the robot drives. It marks the cells where returns land and, when clearing is turned on, clears the cells along each ray. Those are the occupied and free cases of the inverse sensor model above. The documentation describes the obstacle layer only as marking and clearing, with no probabilities, so the costmap holds costs rather than log-odds. An inflation layer adds cost around obstacles so that paths keep a margin from them, and the planner and the controller each read their own costmap, a global one and a local one. OctoMap applies the same filter in 3D (Hornung et al., 2013). It stores the log-odds of each voxel in an octree, which merges uniform regions into larger nodes and keeps the map compact, and it keeps free, occupied and unknown space apart. Its default sensor model is the one used in this section: a voxel at a return gets an inverse model of 0.7, and a voxel the ray passes through gets 0.4. It also clamps the log-odds, by default between probabilities 0.12 and 0.97, for the same reasons as the clipping above. Autonomous cars use the same kind of grid. A bird’s-eye view (BEV) is a top-down grid on the ground plane around the vehicle, laid out like a 2D occupancy grid. A driving stack projects LIDAR returns into it, or uses a network that transforms the images of several cameras into it, and then labels each cell, for example as drivable road or as an obstacle. Tesla extended this to 3D. At the CVPR 2022 Workshop on Autonomous Driving it presented occupancy networks, which take the images of all eight cameras and predict whether each voxel around the car is occupied (Elluswamy, 2022). In the terms of this section, the network is a learned inverse sensor model, and the grid it fills is the map that collision avoidance runs on.

References

Elluswamy, A. (2022). Keynote, Workshop on Autonomous Driving, CVPR 2022. Video.
Hornung, A., Wurm, K. M., Bennewitz, M., Stachniss, C., and Burgard, W. (2013). OctoMap: An efficient probabilistic 3D mapping framework based on octrees. Autonomous Robots. Project page.
Nav2 documentation. Costmap 2D, obstacle layer, mapping and localization.
Thrun, S., Burgard, W., and Fox, D. (2005). Probabilistic Robotics. MIT Press.

Found an error on this page? Report an issue.

State Estimation & Mapping

State Estimation

SLAM

Occupancy Grid Mapping

The grid as a map

Log-odds update

Inverse sensor model for a range beam

A map from the true path

A map from wheel odometry

Limits of independent cells

Occupancy grids in practice

References

​The grid as a map

​Log-odds update

​Inverse sensor model for a range beam

​A map from the true path

​A map from wheel odometry

​Limits of independent cells

​Occupancy grids in practice

​References

The grid as a map

Log-odds update

Inverse sensor model for a range beam

A map from the true path

A map from wheel odometry

Limits of independent cells

Occupancy grids in practice

References