> ## Documentation Index
> Fetch the complete documentation index at: https://aegean.ai/llms.txt
> Use this file to discover all available pages before exploring further.

# Occupancy Grid Mapping

> Log-odds occupancy grids from lidar scans at known poses.

[![Open In Colab](https://colab.research.google.com/assets/colab-badge.svg)](https://colab.research.google.com/github/pantelis/eng-ai-agents/blob/main/notebooks/rse/occupancy-mapping/index.ipynb)

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.

```python theme={null}
%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](https://aegean.ai/aiml-common/lectures/rse/correspondence-estimation/index.md) 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.

```python theme={null}
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.

```python theme={null}
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}")
```

```output theme={null}
cells updated by one scan: 17606 of 56000
```

![Output from cell 4](https://mintcdn.com/aegeanaiinc/izJXkwFvXGX6Sn6q/aiml-common/lectures/rse/occupancy-mapping/images/cell_4_output_1.svg?fit=max\&auto=format\&n=izJXkwFvXGX6Sn6q\&q=85\&s=dd1d1abe307bc76033d54156730a2801)

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

```python theme={null}
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")
```

```output theme={null}
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.

```python theme={null}
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")
```

```output theme={null}
odometry error at the end of the loop: 1.06 m, 14.4 degrees
```

![Output from cell 7](https://mintcdn.com/aegeanaiinc/izJXkwFvXGX6Sn6q/aiml-common/lectures/rse/occupancy-mapping/images/cell_7_output_1.svg?fit=max\&auto=format\&n=izJXkwFvXGX6Sn6q\&q=85\&s=657137a54a2406ab4fa42db8e635b6f3)

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](https://aegean.ai/aiml-common/lectures/rse/correspondence-estimation/index.md). Estimating the poses and the map together is [SLAM](https://aegean.ai/aiml-common/lectures/rse/slam/index.md).

## 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](https://github.com/aegean-ai/eaia-feedback/issues/new?template=page-issue.yml\&page=/aiml-common/lectures/rse/occupancy-mapping).


This documentation is built and hosted on [Mintlify](https://mintlify.com), a developer documentation platform.