You need loop closure to correct for the error when you do SLAM.
In practice, when a loop is detected, what this is doing under the hood is merely adding an edge to the Pose Graph.
There are multiple solutions:
- Almost identical positions (e.g. ) merge them
- Scan matching (similarity)
- Key Frame matching
- Each state corresponds to a local 2D occupancy grid (laser scan representation).
- Features — not practical in real systems because it works in
In the cost function , a loop closure is integrated by adding a new term that replaces the standard landmark measurement model with a pose-match function . This allows the algorithm to match any two arbitrary poses and by defining the relationship , where represents the associated Gaussian noise. This effectively creates a new spatial constraint between non-consecutive nodes in the graph
From
to
Instead of seeing a landmark, we say pose sees pose through scan matching or feature comparison. This is the explanation for the right part of the equation.
To ensure a robust back-end, switchable constraints are introduced as an “on/off switch” for loop closure edges. This is mathematically implemented via a sigmoid function which acts as a weight for the constraint. This allows the optimizer to effectively discard incorrect associations during the global optimization process.
The Switch Prior Constraints term penalizes the switch for moving away from its initial guess : . It creates a “cost” for switching and handles the poor measurement geometry.
