Occupancy Grid Mapping

Occupancy grid mapping builds a map from range measurements taken at known poses. It is the mapping half of SLAM. Localization estimates the pose given the map; occupancy grid mapping estimates the map given the poses (Thrun, Burgard and Fox, 2005, Chapter 9). Many SLAM systems also use it as their last step: they estimate the robot’s path first and then build a grid along that path for the planner. The last part of this section maps the same scans along an odometry path, which drifts.

%config InlineBackend.figure_formats = ["svg"]

import matplotlib.pyplot as plt
import numpy as np
from cycler import cycler

plt.rcParams.update(
    {
        "figure.figsize": (6.5, 3.5),  # matches the 6.5 in text column
        "font.size": 9,
        "axes.titlesize": 10,
        "axes.labelsize": 9,
        "xtick.labelsize": 8,
        "ytick.labelsize": 8,
        "legend.fontsize": 8,
        "axes.prop_cycle": cycler(
            color=[
                "#0072B2",
                "#D55E00",
                "#009E73",
                "#cb77a6",
                "#c28600",
                "#1d99e0",
                "#7f7f7f",
            ]
        ),
        "axes.grid": True,
        "grid.alpha": 0.3,
        "svg.fonttype": "path",
    }
)

# named colors from the palette above
BLUE, VERMILLION, GREEN, PINK, GOLD, SKY, GRAY = (
    "#0072B2",
    "#D55E00",
    "#009E73",
    "#cb77a6",
    "#c28600",
    "#1d99e0",
    "#7f7f7f",
)

np.random.seed(5)

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

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

A grid of

N

cells has

2^N

possible maps. The posterior is therefore approximated by treating 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}).

Each cell is then a binary state that does not change over time, with its own Bayes filter.

Log-odds update

The filter 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})},

so that the Bayes update is a sum. Each cell covered by the measurement

z_t

is updated as

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,

and the other cells keep their value.

l_0

is the log-odds of the prior. The middle term is the inverse sensor model, the occupancy probability of the cell given only

z_t

and

s_t

. The probability is recovered as

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

. Summing log-odds also avoids the round-off of multiplying probabilities near 0 or 1 (Thrun, Burgard and Fox, 2005, Section 9.2).

Inverse sensor model for a range beam

Take a beam with measured range

z

and a cell at distance

d

along it:

$d < z - \alpha/2$ : the beam passed through the cell. Add $l_{\text{free}} - l_0$ , with $l_{\text{free}} < l_0$ .
$|d - z| \le \alpha/2$ : the beam ended in the cell. Add $l_{\text{occ}} - l_0$ , with $l_{\text{occ}} > l_0$ .
$d > z + \alpha/2$ : the cell is behind the hit point. No update.

\alpha

is the thickness of the surface region. A beam at maximum range hit nothing, so it frees its cells and occupies none. This is the inverse range sensor model of Section 9.2 for a lidar beam of negligible width. 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. It finds the cells a beam crosses by sampling the beam every half cell. Bresenham’s line algorithm walks the same cells exactly.

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 lidar_ranges(pose, n_beams=360, sigma=0.01):
    """Ray-cast the walls from pose; return beam angles (sensor frame) and ranges."""
    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, ranges


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)
L_OCC, L_FREE, L_0, ALPHA = 0.85, -0.4, 0.0, 0.1  # L_0 = 0 is a prior of 0.5
L_MAX = 5.0  # clip the log-odds so that the map can still change


def update_grid(L, pose, angles, ranges):
    """Apply the log-odds update for one scan taken at pose."""
    x, y, th = pose
    for angle, z in zip(angles, ranges):
        hit = z < MAX_RANGE
        d = np.arange(0.0, min(z + ALPHA / 2, MAX_RANGE), RES / 2)
        points = np.c_[x + d * np.cos(th + angle), y + d * np.sin(th + angle)]
        cells = np.floor((points - ORIGIN) / RES).astype(int)
        inside = (cells[:, 0] >= 0) & (cells[:, 0] < NX) & (cells[:, 1] >= 0) & (cells[:, 1] < NY)
        cells, d = cells[inside], d[inside]
        _, first = np.unique(cells, axis=0, return_index=True)  # each cell once per beam
        cells, d = cells[first], d[first]
        free = d < z - ALPHA / 2
        occupied = hit & (np.abs(d - z) <= ALPHA / 2)
        L[cells[free, 0], cells[free, 1]] += L_FREE - L_0
        L[cells[occupied, 0], cells[occupied, 1]] += L_OCC - L_0
    np.clip(L, -L_MAX, L_MAX, out=L)


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

One scan from the middle of the room. A hit cell gets one occupied update, which gives it a probability of 0.70. Near the robot many beams cross each cell, and the cells become clearly free. Farther out a cell is crossed by one beam or none, so the free space is only slightly lighter than the prior and has gaps between the beams. Cells that no beam reached stay at the prior of 0.5. These include the shadows behind the box and the partition.

L_one = np.zeros((NX, NY))
pose_one = np.array([4.0, 2.75, 0.0])
update_grid(L_one, pose_one, *lidar_ranges(pose_one))
print(f"cells updated by one scan: {np.count_nonzero(L_one)} of {NX * NY}")

cells updated by one scan: 17606 of 56000

A map from the true path

The robot drives a loop around the room and scans every 0.4 m. Each scan is applied at its true pose.

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


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])
true_path = np.c_[positions, np.append(headings, headings[-1])]

scans = [lidar_ranges(pose) for pose in true_path]
L_true = np.zeros((NX, NY))
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")

57 scans along a path of 22.0 m

A map from odometry

The odometry poses integrate the motion between consecutive true poses with small random errors and a heading bias of 0.3 degrees per step, as a slightly miscalibrated wheel would produce. The same scans are then mapped at the odometry poses.

odometry_path = [true_path[0].copy()]
for prev, curr in zip(true_path[:-1], true_path[1:]):
    c, s = np.cos(prev[2]), np.sin(prev[2])
    dx, dy = curr[:2] - prev[:2]
    forward, sideways = c * dx + s * dy, -s * dx + c * dy  # motion in the robot frame
    dth = wrap(curr[2] - prev[2])
    forward += np.random.normal(0, 0.01)
    sideways += np.random.normal(0, 0.01)
    dth += np.random.normal(0, np.deg2rad(0.5)) + np.deg2rad(0.3)  # noise and bias
    o = odometry_path[-1]
    c, s = np.cos(o[2]), np.sin(o[2])
    odometry_path.append(
        np.array([o[0] + c * forward - s * sideways, o[1] + s * forward + c * sideways, wrap(o[2] + dth)])
    )
odometry_path = np.array(odometry_path)

L_odom = np.zeros((NX, NY))
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.06 m, 14.4 degrees

With the true poses, the walls, the box and the partition are thin dark lines and the free space is uniformly light. Each cell was observed from many poses, and its probability is close to 0 or 1. With odometry, the pose error at the end of the loop is 1.06 m and 14.4 degrees. The odometry path (solid) moves away from the true path (dotted) 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 independence assumption fails for wide beams. A sonar cone returns one range for a whole sector, and the update marks every cell at that range as occupied, although only one of them produced the echo. Two overlapping cones can then disagree about the same cells. Section 9.4 of Probabilistic Robotics describes mapping with the forward sensor model, which keeps the dependencies between cells at a higher computational cost.

References

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 odometry

Limits of independent cells

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 odometry

​Limits of independent cells

​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 odometry

Limits of independent cells

References