Thrun et al., Chapter 14

Fast Incremental Mapping

Real-time mapping via gradient descent, cycle detection, backwards correction, multi-robot, and 3D mapping.

Prerequisites: Chapters 9-10 (occupancy grids, SLAM fundamentals).

Chapters

Simulations

Quizzes

Chapter 0: Real-Time Mapping

EKF SLAM is online but scales poorly. EIF SLAM scales well but is batch. EM is powerful but requires multiple passes. Can we build maps in real-time, processing each sensor reading once as it arrives?

Incremental maximum likelihood mapping does exactly this. At each time step, it finds the pose that maximizes the local likelihood:

s^(t), m^(t) = argmax p(o^(t) | s, m^(t-1)) p(s | a^(t-1), s^(t-1))

and updates the map by incorporating the new observation at the estimated pose.

The key simplification: Instead of maintaining a full probability distribution over poses and maps (as in EKF SLAM), incremental ML mapping keeps only the single best estimate. This makes the algorithm fast (O(1) per step for pose estimation) but brittle — a single wrong pose estimate permanently corrupts the map.

Check: What does incremental ML mapping sacrifice for real-time speed?

It maintains only a single pose/map estimate instead of a full posterior distribution It ignores sensor data It uses a coarser map

Chapter 1: Incremental ML

The incremental ML algorithm maximizes the conditional belief — the probability of the current pose and map given the previous pose and map:

Bel(s^(t), m^(t) | s^(t-1), m^(t-1)) = η p(o^(t) | s^(t), m^(t)) p(s^(t) | a^(t-1), s^(t-1))

This is the product of two factors: (1) How well does the current observation match the current map at the proposed pose? (2) How likely is this pose given the last pose and the action?

The incremental trick: If we use occupancy grid mapping (Chapter 9) to build the map, the map at time t is deterministically constructed from the map at t-1 plus the new observation at pose s^(t). This eliminates the map from the optimization — we only need to search over 3D poses, not over the enormous space of maps.

Starting from the odometry prediction, gradient descent adjusts the pose to maximize the likelihood. A few iterations (5-20) typically suffice for convergence.

Check: Why can incremental ML search over poses only (not maps)?

The map is built deterministically from the pose and observation, so the pose fully determines the map The map doesn't change The map is given a priori

Chapter 2: Gradient Descent

To find the pose that maximizes the likelihood, we use gradient ascent in log-likelihood space:

s̄^(t) ← s̄^(t) + κ ∇_s [log p(o^(t) | s̄, m^(t-1)) + log p(s̄ | a^(t-1), s^(t-1))]

where κ > 0 is a small step size. The gradient has two terms: one from the sensor model (which scan positions match the map?) and one from the motion model (which poses are consistent with odometry?).

Starting point: the odometry prediction (noise-free motion model applied to the previous pose). Gradient descent then adjusts x, y, and θ to better align the current scan with the existing map.

This is scan matching! The gradient descent in incremental ML mapping is essentially a probabilistic form of scan matching. The algorithm finds the rigid transformation (translation + rotation) that best aligns the current laser scan with the existing map, while regularizing toward the odometry prediction.

Check: What does the gradient of the sensor model log-likelihood measure?

How the scan-to-map alignment quality changes as the pose is perturbed in each direction The sensor noise level The number of beams that hit obstacles

Chapter 3: Gradient Calculation

The gradient for the likelihood field sensor model (Chapter 6) is computed per beam:

For each beam k with endpoint (x_ok, y_ok) and nearest obstacle (x̄, ȳ):

∂dist²/∂x = 2(x_ok - x̄)
∂dist²/∂y = 2(y_ok - ȳ)
∂dist²/∂θ = 2[(x_ok - x̄)∂x_ok/∂θ + (y_ok - ȳ)∂y_ok/∂θ]

The gradient of the log-likelihood is the sum of these per-beam contributions, weighted by the Gaussian likelihood.

Implementation detail: The nearest-obstacle distances come from the distance transform of the current map (already computed for the likelihood field sensor model). Computing the gradient per beam is O(1), so the total gradient is O(K) where K is the number of beams. With 180 beams and 10 gradient steps, each pose optimization takes ~1,800 distance lookups — very fast.

Check: What precomputed data structure makes the gradient calculation efficient?

The distance transform of the current map — nearest-obstacle lookups are O(1) A k-d tree of landmarks A precomputed path plan

Chapter 4: Scan Matching

Incremental ML mapping is extremely effective for straight corridors and small environments where odometry drift is modest. But it has a critical weakness: it can't handle loops.

When the robot traverses a large loop (e.g., around a building), odometry error accumulates. By the time it returns to the starting point, the accumulated error might be several meters. Incremental ML mapping doesn't know it's returned — it just keeps building the map based on local scan matching, creating a map where the loop doesn't close.

The loop closure problem: Local scan matching gives excellent results locally (each consecutive scan is well-aligned with its predecessor). But small errors accumulate over long paths. After traveling 100 meters in a loop, even 1% drift means the map is 1 meter off at the closure point. Incremental ML can't correct this because it never looks back at the global picture.

Check: Why does incremental ML mapping fail at loop closure?

It only optimizes locally, so accumulated drift over long paths is never corrected It doesn't use sensor data The map resolution is too low

Chapter 5: Cycle Detection

To handle loops, incremental mapping algorithms must first detect when the robot has returned to a previously visited area. Two approaches:

Sensor-based detection: Compare the current sensor reading to readings taken earlier. If the current scan matches an old scan well, the robot has probably returned. This is essentially a global localization problem within the existing map.

Pose-based detection: Monitor whether the current pose estimate is close to any previously visited pose. If so, and the sensor readings match, a loop has been detected.

False positives are dangerous: A false loop closure (thinking you've returned when you haven't) can destroy the map by warping it to satisfy a non-existent constraint. Cycle detection must be conservative — better to miss a genuine loop closure than to apply a false one.

Check: Why must cycle detection be conservative?

A false loop closure warps the entire map to satisfy a non-existent constraint, causing permanent corruption Conservative detection is faster Loop closures are never correct

Chapter 6: Backwards Correction

Once a loop is detected, the accumulated error must be distributed across all poses in the loop. Backwards correction adjusts past pose estimates to make the loop close:

1. Detect the loop: current pose matches a past pose (say, from time t₀).

2. Compute the closure error: the difference between the current pose and the matched past pose.

3. Distribute the error across all poses from t₀ to t, proportionally to the amount of motion. Poses near the closure point get large corrections; poses far from it get small ones.

After correction: The corrected poses are used to rebuild the map from scratch. Since the poses are now globally consistent (the loop closes), the resulting map is much better. This rebuild is fast: it's just occupancy grid mapping (Chapter 9) applied to the corrected poses.

Check: How does backwards correction distribute the loop closure error?

Proportionally across all poses in the loop — more correction near the closure, less far from it Only the current pose is corrected All poses get the same correction

Chapter 7: Multi-Robot Mapping

Multiple robots can map faster by dividing the environment. The challenge: each robot builds its own local map in its own coordinate frame. To merge them into a global map, the robots must establish a common reference frame.

Rendezvous approach: Two robots meet and observe each other. This fixes the relative transformation between their coordinate frames. Their maps can then be merged by applying this transformation.

Overlap approach: Two robots independently map overlapping areas. Scan matching between the overlapping regions determines the relative transformation.

Practical considerations: Multi-robot mapping requires communication (sharing maps or observations), coordination (who maps where), and robustness (what if a robot fails). The overlapping approach is simpler because robots don't need to meet, but it requires sufficiently distinctive overlap regions for reliable matching.

Check: What must be established before two robots' maps can be merged?

The relative transformation between their coordinate frames (through meeting or map overlap) Identical sensor types Identical map resolutions

Chapter 8: 3D Mapping

All mapping algorithms in this book extend to 3D, but at significantly higher computational cost. The robot pose becomes 6D (x, y, z, roll, pitch, yaw). Occupancy grids become voxel grids. Scan matching operates in 6D pose space.

Aspect	2D	3D
Pose dimensions	3 (x, y, θ)	6 (x, y, z, roll, pitch, yaw)
Map representation	2D grid (~10⁴ cells)	Voxel grid (~10⁷ cells)
Scan matching	3D optimization	6D optimization
Sensors	2D laser, sonar	3D laser, stereo, depth cameras

3D mapping is active research: Techniques from this book (occupancy grids, scan matching, loop closure) all apply, but the increased dimensionality makes them much more expensive. Modern 3D SLAM systems use efficient data structures (octrees, surfels, voxel hashing) to manage the enormous state spaces.

Check: What is the main challenge of extending SLAM from 2D to 3D?

The dimensionality increase: 6D poses and 3D voxel grids make computation much more expensive 3D sensors don't exist The algorithms are fundamentally different

Chapter 9: Summary

Concept	Key Idea
Incremental ML	Real-time mapping via local gradient descent; O(1) per step
Scan matching	Align current scan to existing map using likelihood gradient
Loop closure	Detect revisited places; distribute error backwards
Multi-robot	Merge maps via coordinate frame alignment
3D mapping	Same principles, much higher computational cost

What comes next: We've covered all the major mapping and localization techniques. The final two chapters shift to a completely different problem: planning and control under uncertainty. Chapter 15 introduces Markov Decision Processes (MDPs) for fully observable planning, and Chapter 16 extends to Partially Observable MDPs (POMDPs).

"Map as you go. Fix mistakes when you find them.
The perfect is the enemy of the real-time."

Check: What makes incremental ML mapping suitable for real-time operation?

It processes each scan once using local gradient descent, with no need to revisit past data It ignores loop closure It uses a very coarse map