Graph SLAM and Loop Closure

Graph SLAM stores every pose the robot visited, along with the laser scans it took there, as nodes in a graph. Odometry links each pose to the next. When the robot recognizes a place it has been before, a loop closure adds a link across the graph, and optimizing the graph pulls the whole map into shape.

Heading drift

Close the loop automatically when back at the start

The pose graph

Nodes are poses $\mathbf{x}_i$. Each edge is a relative measurement $\mathbf{z}_{ij}$ with information matrix $\Omega_{ij}$. The optimizer finds the poses that best satisfy every edge:

$$\mathbf{x}^* = \arg\min_{\mathbf{x}} \sum_{(i,j)} \mathbf{e}_{ij}^\top\,\Omega_{ij}\,\mathbf{e}_{ij}, \qquad \mathbf{e}_{ij} = \mathbf{z}_{ij} - (\mathbf{x}_j \ominus \mathbf{x}_i)$$

The map is not stored separately: each scan is attached to its pose, so moving the poses moves the map.

What to notice

Heading drift makes the map bend, so the corridors no longer line up and the robot thinks the end is far from the start. When the loop closes, the optimizer spreads the correction along the whole path, every scan moves with its pose, and the walls snap together. Here headings are corrected along with positions by spreading the angle error. Real systems such as slam_toolbox and Cartographer do this with a full nonlinear optimization.