mobile robots from scratch, part 14: SLAM

Building a map, step by step
The pose graph
On a real robot
What’s still missing
Going deeper
Further reading

Part 13 found the robot on a map it was given. But someone had to make that map. Making it needs to know where the robot was for each scan, and knowing where the robot was needs the map. SLAM, simultaneous localisation and mapping, solves both at once.

This part builds a map of the room from one lap, four ways: with perfect poses, with odometry, with scan matching, and with scan matching plus a loop closure.

Building a map, step by step

Cut the room into 5 cm cells, 24,000 of them, and give each a probability of being occupied. At first every cell is unknown: 50%.

Each lidar beam (blue) says two things. The cells it passes through are probably free; the cell where it ends is probably occupied. The first scan makes 1,012 cells fairly sure to be free (tinted) and 89 occupied (dark).

Rather than store probabilities, a map stores log-odds, \(l = \ln \frac{p}{1 - p}\), which turns each update into an addition. Here a hit adds 0.85 and a pass subtracts 0.4:

\[p = 1 - \frac{1}{1 + e^{l}}.\]

One hit makes a cell 70% likely occupied, two make it 85%. A person walking through leaves a few hits, and the beams that pass through later wipe them out.

Scroll to drive the lap. Here, for now, the poses are perfect. After 161 scans, 21,647 cells are free and 997 occupied: a clean map of the room.

A real robot doesn’t know its poses. Use the wheels instead: odometry, with part 12’s 5% wheel noise (orange, dashed). By the end of the 20.8 m lap, it’s 60 cm off.

Every scan is drawn where odometry says it was taken, so the walls come out doubled and smeared. Of the cells this map calls occupied, only 39% lie on a real wall.

Part 9’s scan matching does far better. Match each scan against the one before it with point-to-line ICP, every half second, and chain the motions (green). This robot has a cheaper lidar than part 9’s, with 3 cm of noise per beam. Scan matching ends 38.3 cm off, and the map is sharper: 61% of its walls are real.

But it still drifts, because each match has a small error and 160 of them add up. Nothing in the chain can notice: each match only knows the scan before it.

At the end of the lap, the robot is back where it started. Its last scan sees the same walls as its first. Matching the two (purple, dashed) gives the true motion between the first pose and the last, to within 1 cm, however much drift has built up between them. That’s a loop closure.

Now the robot has a pose graph: a node for each keyframe (green dots), an edge for each scan match between consecutive ones, and one more edge across the loop. The chain says the last pose is here; the loop edge says it’s there. They can’t both be exactly right, so spread the disagreement over every edge, finding the poses that fit all the edges best.

Press the button. One round of Gauss–Newton brings the total squared error from 15.6 to 0.02. The last pose is now 1.0 cm off; the worst, part way round, 10.0 cm, down from 40.9. And 89% of the map’s walls are real.

A loop closure is a strong claim: these two places are the same. If it’s wrong, the optimiser obeys it anyway. Here, one extra loop edge claims that a pose at the far end of the room is at the start (red, dashed: the two places it joins).

The graph tears itself apart to agree: the poses end 3.44 m off on average, and only 21% of the map’s walls are real. One bad edge has ruined a map that 161 good ones built.

So real SLAM is careful about loops. It only tries matches near where the robot thinks it is, accepts one only if the scans fit very well, and uses robust costs that stop trusting an edge that disagrees with all the others.

The pose graph

Each node is a pose \(x_i = (x, y, \theta)\). Each edge says how pose \(j\) looks from pose \(i\), \(z_{ij}\), measured by scan matching. If the poses were right, \(x_j\) seen from \(x_i\) would match \(z_{ij}\) exactly. The optimiser finds the poses that make the total disagreement smallest:

\[\min_{x} \sum_{\text{edges}} e_{ij}^\top \Omega_{ij}\, e_{ij},\] \[e_{ij} = z_{ij} \ominus (x_j \text{ seen from } x_i),\]

where \(\Omega_{ij}\) says how much each edge is trusted (its inverse covariance, part 12). Gauss–Newton linearises every edge around the current poses, solves one big sparse linear system, and repeats. The first node is held fixed, since moving the whole map changes nothing.

This is the same least squares as part 12’s Kalman filter, solved for the whole trajectory at once rather than one step at a time. That’s why it can go back and fix poses from a minute ago, which a filter can’t.

On a real robot

	slam_toolbox	Cartographer
from	Steve Macenski, 2018	Google, 2016
matching	scan to scan and map	scans into submaps
in ROS 2	Nav2’s default	maintained by community
also does	lifelong mapping	3-D lidar

In ROS 2, slam_toolbox is the usual choice. It takes /scan and the odom → base_link transform from part 12’s EKF, matches scans, finds loop closures, and publishes the map as an occupancy grid on /map, along with the map → odom transform (part 10). Drive the robot round once, save the map, and from then on part 13’s AMCL can localise in it.

What’s still missing

Going somewhere. The robot has a map and knows where it is on it. Planning a path through it, and following it without hitting anything, is part 15.

Going deeper

The front end and the back end

SLAM systems split into two halves. The front end turns sensor data into edges: scan matching, and finding loop closure candidates, often by matching the new scan against a stored map of the area rather than one old scan. The back end optimises the graph. Here the front end was ICP and the back end Gauss–Newton; real systems use faster matchers (correlative scan matching, branch and bound) and solvers that exploit the graph’s sparsity (g2o, Ceres, GTSAM).

Before graphs: EKF SLAM and FastSLAM

The first SLAM systems put the robot and every landmark in one big EKF state. The covariance grows as the square of the number of landmarks, so it only works for small maps. FastSLAM used a particle filter over paths, part 13’s idea, with a small map for each particle; gmapping, long ROS 1’s default, does this with occupancy grids. Graph-based SLAM, which keeps every pose and solves for them all, now dominates.

Visual SLAM

A camera can do the same: match features between frames instead of scans, and recognise places by their appearance to close loops. ORB-SLAM and RTAB-Map do this, and part 11’s depth cameras give them depth for each feature. They work where lidar can’t afford to go, but suffer in the dark and on blank walls.