Factor Graphs and Smoothing

A filter only updates the latest pose and then forgets. A factor graph keeps every pose as a variable and every measurement as a factor tying variables together, then solves for all poses at once. Later evidence, such as returning to a known spot, can then fix earlier mistakes.

Loop closure factor (start and end are the same place) A few GPS factors Odometry noise

Least squares over all poses

Each factor says how two variables should relate, with a covariance. For poses $\mathbf{x}_i$, odometry readings $\mathbf{u}_i$, and a loop closure between $\mathbf{x}_0$ and $\mathbf{x}_N$:

$$\min_{\mathbf{x}_{0:N}}\; \sum_i \lVert \mathbf{x}_{i+1} - \mathbf{x}_i - \mathbf{u}_i \rVert^2_{\Sigma_u} + \lVert \mathbf{x}_N - \mathbf{x}_0 \rVert^2_{\Sigma_\ell} + \sum_{k} \lVert \mathbf{x}_k - \mathbf{g}_k \rVert^2_{\Sigma_g}$$

Here poses are positions only, so the problem is linear and one solve gives the answer. With headings it becomes nonlinear and is solved with Gauss-Newton, as libraries like GTSAM and g2o do.

What to notice

Before optimizing, the dead-reckoned path, amber, does not close the loop. The loop closure factor says the end is the same place as the start. Optimizing spreads the error back along the whole chain, so every pose moves a little and the red path closes neatly near the truth. GPS factors pin individual poses. This is smoothing: the past is re-estimated using future evidence, which a filter cannot do.