M10-T3 — Mapping with Uncertain Poses

Learning objectives

After this lesson, you should be able to:

• Explain how a pose error becomes a map error, and why the map errors accumulate with the path length.
• Quantify the smearing of a grid map as a function of the drift.
• Describe scan-to-map matching and what it can and cannot fix.
• State why mapping and localisation are coupled, and what the joint problem is.

The intuition

The map update of the last two lessons takes a scan and a pose, and draws the scan into the grid from that pose. If the pose is wrong by a heading error of $\delta\theta$, the walls the robot sees at distance $r$ are drawn displaced sideways by about $r\,\delta\theta$. A drift of 0.1 rad turns a wall 5 m away into one 0.5 m off. Each scan has its own error, so the same wall is drawn several times in several places: the wall doubles and thickens.

The left panel shows it for one run in which the robot drives round the divider and the box and returns, with commands that the real robot follows imperfectly (2 cm and 0.03 rad of error per step). With the true poses the 160 wall cells are drawn once (352 occupied cells, none far from a wall). With dead-reckoned poses the outline is rotated and doubled, and the box is smeared; in the average of four runs 94 of the 318 occupied cells lie farther than one cell from any wall.

The damage grows with the drift. Doubling the command error, from the scale of 1 to 2, takes the end position error from 1.5 m to 2.9 m and the number of false wall cells from 94 to 110; halving it, from 1 to 0.5, gives 0.74 m and 56. At a quarter of the error the position is still 0.37 m off, and 30 cells are off the walls. Since the pose errors accumulate (they are an integral of the command errors), the map gets worse the longer the robot drives without a correction.

The right panel tests the cheapest repair. Before drawing a scan, try small shifts of the dead-reckoned pose (±20 cm, ±0.06 rad) and keep the one where the scan best fits the map built so far. In all six runs the end error is smaller, between 0.18 and 0.96 m against 0.48 to 2.72 m, and the occupied cells far from walls fall from 91 to 41 on average. The error is not gone, because the matching only ever corrects against a map that already contains the earlier errors.

Map and pose estimate are one problem. A good map needs good poses, and good poses need a good map to localise against. Solving them together is SLAM.

Definitions

Dead reckoning

Computing the pose by integrating the commands or the odometry, without any correction from the world. Its error grows without limit.

Smearing

Walls drawn in several slightly different places, which thickens them and doubles them.

Scan-to-map matching

Choosing the pose, near the odometry estimate, that makes the new scan fit the current map best: $\hat x_t=\arg\max_{x}\sum_k \ell_{\mathrm{map}}(\text{endpoint}_k(x))-\lambda\lVert x-x_\mathrm{odo}\rVert$.

Loop closure

Recognising that the robot is back at a place it has mapped, which gives a constraint that links the current pose to a pose long ago and lets the accumulated drift be corrected.

SLAM

Simultaneous localisation and mapping: estimating the map and the whole path together from the controls and the scans.

Derivation: how pose errors enter the map, and why local matching is not enough

A pose error is a rigid shift of the whole scan

The endpoint of beam $k$ in the world is $\mathbf{e}_k=\mathbf{x}+r_k\,\mathbf{u}(\theta+\beta_k)$ with $\mathbf{u}(\phi)=(\cos\phi,\sin\phi)$. If the estimated pose is $\hat{\mathbf{x}}=\mathbf{x}+\delta\mathbf{x}$ and $\hat\theta=\theta+\delta\theta$:

1. $$\hat{\mathbf{e}}_k-\mathbf{e}_k=\delta\mathbf{x}+r_k\bigl(\mathbf{u}(\theta+\delta\theta+\beta_k)-\mathbf{u}(\theta+\beta_k)\bigr)$$

   The difference between where the scan is drawn and where it should be.

2. $$\approx\delta\mathbf{x}+r_k\,\delta\theta;\mathbf{u}^\perp(\theta+\beta_k)$$

   To first order in the small angle: a position shift plus a rotation about the robot, whose effect is proportional to the range.

3. $$\lVert\hat{\mathbf{e}}_k-\mathbf{e}_k\rVert\le\lVert\delta\mathbf{x}\rVert+r_{\max}\lvert\delta\theta\rvert$$

   The map error is bounded by the position error plus the heading error times the longest range: heading errors dominate for long-range scanners.

Accumulation

Each step adds independent errors to the pose, and the pose error is the sum. With a position error per step of standard deviation $s$ and a heading error $h$, after $n$ steps the heading error has standard deviation $h\sqrt n$, and the position error grows with $n$ through the heading: a path of length $L$ with heading error $\delta\theta$ is displaced sideways by about $L\,\delta\theta$. The example (0.03 rad of command error per 0.3 m step, on a path of about 30 m) gives an end error of 1.5 m: of the order $0.03\sqrt{115}\times 5$, which matches.

Why matching helps and why it is not enough

Scan-to-map matching replaces the odometry's guess with the pose that best aligns the new scan with the map. It removes the new error added since the last scan, which is why every run improved. But the map already contains the pose errors of the past scans; each match aligns the new scan with a slightly wrong map, and a small residual error is absorbed into the next step. The error becomes a slow drift rather than an integral of independent errors, but it does not vanish, and it is not corrected retroactively when the robot returns to the start. That needs a constraint between the two ends of the loop and a way to distribute the correction over the whole path: the pose graph.

1. $$\text{odometry}:;x_{t+1}\ominus x_t\approx u_t,\qquad \text{scan matching}:;x_{t+1}\ominus x_t\approx\Delta_t$$

   Each gives a relative constraint between consecutive poses, with its own uncertainty.

2. $$\text{loop closure}:;x_j\ominus x_i\approx\Delta_{ij}\quad(i\ \text{far before}\ j)$$

   A scan taken from a place seen before gives a relative constraint across the loop.

3. $$\hat x_{1:t}=\arg\min\sum_{\text{constraints}}\lVert\text{residual}\rVert^2_{\Sigma^{-1}}$$

   The maximum a posteriori path given all constraints at once: a nonlinear least-squares problem (Module 11).

A map built from a drifting path is wrong everywhere the path was wrong. Local matching slows the drift; only a global fit that uses loop closures removes it.

A worked example

The room has a divider and a box (9 segments); the robot follows a route of 115 steps around them and back. Its commands are followed with errors scaled by a factor (2 cm and 0.03 rad per step at scale 1). The scanner has 90 beams, 5 cm noise, range 6 m; the grid has 25 cm cells and probabilities 0.65 and 0.45, clamped at log odds ±4.

1. No drift. With true poses the map has 352 occupied cells and none farther than one cell from a wall.
2. Drift scale. Dead reckoning, mean of four runs. Scale 0.25: end error 0.37 m, 30 occupied cells far from walls. Scale 0.5: 0.74 m and 56. Scale 1: 1.48 m and 94. Scale 2: 2.92 m and 110. The end error grows linearly with the scale; the number of false cells saturates, since the false walls begin to overlap each other.
3. Scan matching at scale 1. The end error falls in every run: 1.36 to 0.89, 1.37 to 0.18, 2.72 to 0.96, 0.48 to 0.20, 1.37 to 0.88 and 0.53 to 0.18 m. The occupied cells far from walls fall from 91 to 41 on average. They do not fall to zero (the true-pose map has none).

import math
from robotprob import Rng, OccupancyGrid, sample_motion_velocity, wrap_angle, mean

SEGS = [((0, 0), (10, 0)), ((10, 0), (10, 8)), ((10, 8), (0, 8)), ((0, 8), (0, 0)), ((5, 0), (5, 4)),
        ((7, 5), (8.5, 5)), ((8.5, 5), (8.5, 6.5)), ((8.5, 6.5), (7, 6.5)), ((7, 6.5), (7, 5))]
ZMAX, RES, NB = 6.0, 0.25, 90
def cast(x, y, th):
    dx, dy, best = math.cos(th), math.sin(th), ZMAX
    for (x1, y1), (x2, y2) in SEGS:
        ex, ey = x2 - x1, y2 - y1
        den = dx * ey - dy * ex
        if abs(den) < 1e-12: continue
        t = ((x1 - x) * ey - (y1 - y) * ex) / den
        u = ((x1 - x) * dy - (y1 - y) * dx) / den
        if t > 0.0 and 0.0 <= u <= 1.0 and t < best: best = t
    return best
def new_grid(): return OccupancyGrid(44, 36, RES, origin=(-0.5, -0.5), l_min=-4.0, l_max=4.0)
truth = set(); g0 = new_grid()
for (x1, y1), (x2, y2) in SEGS:
    n = int(max(abs(x2 - x1), abs(y2 - y1)) / 0.05)
    for k in range(n + 1): truth.add(g0.cell(x1 + (x2 - x1) * k / n, y1 + (y2 - y1) * k / n))
near_wall = lambda i, j: any((i + a, j + b) in truth for a in (-1, 0, 1) for b in (-1, 0, 1))

def route():                                                # the commands that drive the robot round the divider and the box
    pose = [1.5, 1.5, math.pi / 2]; cmds = []
    for wp in [(1.5, 7.0), (9.0, 7.4), (9.0, 1.5), (6.5, 1.5), (6.5, 3.2), (6.5, 1.5), (1.5, 1.5), (1.5, 3.0)]:
        for _ in range(80):
            ang = wrap_angle(math.atan2(wp[1] - pose[1], wp[0] - pose[0]) - pose[2])
            if math.hypot(wp[0] - pose[0], wp[1] - pose[1]) < 0.35: break
            w = max(-0.8, min(0.8, 2.5 * ang)); v = 0.3 if abs(ang) < 0.5 else 0.05
            cmds.append((v, w)); pose = sample_motion_velocity(Rng(1), pose, v, w, 1.0, 0.0, 0.0)
    return cmds
CMDS = route()

def match(grid, pose, scan, search=(0.2, 0.2, 0.06)):    # scan-to-map matching: try small shifts of the dead-reckoned pose, keep the best-fitting one
    best, best_pose = None, pose
    for ix in range(-4, 5):
        for iy in range(-4, 5):
            for ia in range(-3, 4):
                cand = (pose[0] + ix * search[0] / 4, pose[1] + iy * search[1] / 4, pose[2] + ia * search[2] / 3)
                s = 0.0
                for k, r in enumerate(scan):
                    if r >= ZMAX: continue
                    a = cand[2] + 2 * math.pi * k / NB
                    i, j = grid.cell(cand[0] + r * math.cos(a), cand[1] + r * math.sin(a))
                    if grid.inside(i, j): s += max(grid.l[j][i], -1.0)
                s -= 0.1 * (abs(ix) + abs(iy)) + 0.1 * abs(ia)           # a small penalty for moving away from the odometry
                if best is None or s > best: best, best_pose = s, cand
    return list(best_pose)

def run(seed, mode, scale=1.0):
    rng = Rng(seed)
    truth_pose = [1.5, 1.5, math.pi / 2]; est = list(truth_pose)
    g = new_grid(); err = []
    for t, (v, w) in enumerate(CMDS):
        # the real robot does not do exactly what it is told: speed and turn rate errors of 2 cm and 0.03 rad per step, times the scale
        truth_pose = sample_motion_velocity(rng, truth_pose, v, w, 1.0, 0.02 * scale, 0.03 * scale)
        scan = [min(ZMAX, max(0.1, cast(truth_pose[0], truth_pose[1], truth_pose[2] + 2 * math.pi * k / NB) + rng.gauss(0.0, 0.05))) for k in range(NB)]
        if mode == 'true':
            est = list(truth_pose)
        else:
            est = sample_motion_velocity(rng, est, v, w, 1.0, 0.0, 0.0)                           # dead reckoning: believe the command
            if mode == 'matched' and t > 3:
                est = match(g, est, scan)
        for k, r in enumerate(scan):
            g.update_ray(est[0], est[1], est[2] + 2 * math.pi * k / NB, r, ZMAX, 0.65, 0.45)
        err.append(math.hypot(est[0] - truth_pose[0], est[1] - truth_pose[1]))
    return g, err

def sharpness(g):
    P = g.probabilities(); occ = [(i, j) for j in range(g.h) for i in range(g.w) if P[j][i] > 0.5]
    at_wall = sum(1 for (i, j) in occ if near_wall(i, j))
    return len(occ), len(occ) - at_wall

print('steps in the route:', len(CMDS))
print('how pose drift smears the map (dead reckoning, mean of 4 runs; scale 1 = 2 cm and 0.03 rad of command error per step)')
print('drift scale   position error at the end   occupied cells   of them far from any wall')
for scale in (0.0, 0.25, 0.5, 1.0, 2.0):
    rows = []
    for seed in range(1, 5):
        g, err = run(seed, 'dead', scale)
        occ, far = sharpness(g)
        rows.append((err[-1], occ, far))
    print(str(scale).rjust(10), str(round(mean([r[0] for r in rows]), 2)).rjust(18), str(round(mean([r[1] for r in rows]))).rjust(20), str(round(mean([r[2] for r in rows]))).rjust(22))
g, err = run(1, 'true')
print('true poses: occupied cells', sharpness(g)[0], ' far from any wall', sharpness(g)[1])

print('scan-to-map matching at drift scale 1: end position error, dead reckoning against matched (6 runs)')
better = 0; far_d = []; far_m = []
for seed in range(1, 7):
    gd, ed = run(seed, 'dead'); gm, em = run(seed, 'matched')
    better += em[-1] < ed[-1]
    far_d.append(sharpness(gd)[1]); far_m.append(sharpness(gm)[1])
    print(' run', seed, ' dead reckoning', str(round(ed[-1], 2)).rjust(5), ' matched', str(round(em[-1], 2)).rjust(5))
print('matching helped in', better, 'of 6 runs; occupied cells far from any wall, mean:', round(mean(far_d)), 'with dead reckoning,', round(mean(far_m)), 'with matching')

steps in the route: 115
how pose drift smears the map (dead reckoning, mean of 4 runs; scale 1 = 2 cm and 0.03 rad of command error per step)
drift scale   position error at the end   occupied cells   of them far from any wall
       0.0                0.0                  353                      0
      0.25               0.37                  297                     30
       0.5               0.74                  308                     56
       1.0               1.48                  318                     94
       2.0               2.92                  274                    110
true poses: occupied cells 352  far from any wall 0
scan-to-map matching at drift scale 1: end position error, dead reckoning against matched (6 runs)
 run 1  dead reckoning  1.36  matched  0.89
 run 2  dead reckoning  1.37  matched  0.18
 run 3  dead reckoning  2.72  matched  0.96
 run 4  dead reckoning  0.48  matched   0.2
 run 5  dead reckoning  1.37  matched  0.88
 run 6  dead reckoning  0.53  matched  0.18
matching helped in 6 of 6 runs; occupied cells far from any wall, mean: 91 with dead reckoning, 41 with matching

In robotics

Where the pose comes from in practice

Mapping robots take their poses from a localisation filter (Module 8) or from scan matching: correlative scan matching as in the brute-force search above (usually with a multi-resolution grid for speed), or ICP, which aligns the scan to the previous scan or to a submap. Wheel odometry alone is the baseline that, as shown, is not good enough beyond a few metres.

Submaps and loop closure

Scan-to-map matching works well against a local map built from a few recent scans (a submap): the errors inside it are small. The mapper builds a chain of submaps, and detects loop closures by matching the current scan against old submaps. The SLAM course walks through this pipeline in its mapping lessons and a pose-graph back end in its optimisation lessons.

Uncertainty-aware mapping

One can also blur the update by the pose uncertainty: widen the hit and free updates in proportion to the covariance of the pose, so that a poorly localised robot draws thick faint walls instead of thin confident ones. This is honest, but it blurs the final map; the preferred remedy is to reduce the pose error.

Diagnosing the problem from the map

• Doubled walls and rotated rooms are the classical signature of heading drift.
• Thick, uniformly blurred walls suggest random position noise rather than drift.
• A corridor that appears shorter than it is points to a scale error in the wheel odometry.

Common misconceptions

Key takeaways

1. A pose error draws every scan in the wrong place: displacement ≈ position error + range × heading error. Errors accumulate along the path.
2. In the example, the end error grows from 0.37 m to 2.9 m as the command error is raised from a quarter of the nominal to twice it, and 94 of 318 occupied cells were far from any wall at the nominal scale.
3. Scan-to-map matching reduces the drift (smaller end error in all six runs, false cells from 91 to 41) but cannot correct what is already in the map.
4. Mapping needs poses and localisation needs a map: the joint posterior of path and map is the SLAM problem.
5. Loop closures add constraints across the path, and a global least-squares fit (the pose graph of Module 11) distributes the correction.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• S. Thrun, W. Burgard and D. Fox, Probabilistic Robotics, MIT Press, 2005, chapters 9 and 10 (occupancy grids with known poses, then SLAM). Paid book.
• C. Stachniss, “Graph-based SLAM using Pose Graphs” (video lecture) , the continuation of this lesson. Free to watch.
• T. D. Barfoot, State Estimation for Robotics, 2nd edition, 2024 , the SLAM material of chapter 9 (or the nonlinear estimation chapters). Free to read as the author’s PDF; the book is copyrighted.
• C. Cadena et al., “Past, Present, and Future of Simultaneous Localization and Mapping: Toward the Robust-Perception Age”, IEEE Transactions on Robotics 32 (6), 2016. Paid journal article.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

This closes the mapping module. Module 11 treats estimation as optimisation: maximum likelihood and MAP, weighted least squares, factor graphs, and pose-graph optimisation, which is the global fit that this lesson called for.

Related content

Nothing below is required to finish this lesson.

• Before
  M10-T2: Inverse Sensor Models

  Build an inverse sensor model for a range beam, choose its free and occupied probabilities, and explain clamping.

Other courses on this site treat neighbouring topics from their own angle:

• SLAM M3-T1: Occupancy Grid Mapping
• SLAM M3-L1: Build a 2D Occupancy Grid
• Navigation M2-T1: Occupancy Grids for Planning