M4-T6 — Robust Optimization and Outlier Handling

Learning objectives

After this lesson, you should be able to:

• Explain why least squares is not robust to outliers and write the weighted form of an M-estimator.
• Implement iteratively reweighted least squares for Huber and Cauchy losses and read the weights.
• Describe switchable constraints, dynamic covariance scaling, max-mixtures and consensus methods.
• State when robust methods help and when they cannot (many outliers, bad initialisation).

The problem and the intuition

Five friends estimate the length of a table: 1.00, 1.10, 0.90, 1.05, and a fifth who misread the tape: 10.0. The mean is 2.81, which is wrong by any standard. The median, 1.05, ignores the outlier but throws away the other information. A sensible compromise listens to everyone, but with less patience for the one who is far from the rest. That is robust estimation: a loss function that stops caring when the error is large.

Robust estimation does not detect outliers first. It lets the fit and the outlier detection happen together: whatever is far from the fit gets a small weight.

The algorithm: robust losses and reweighting

Problem. Minimise Σ ρ(‖e_k‖_Ω) where ρ grows more slowly than the square, so that the constraints with large residuals count for less. ‖e‖_Ω = sqrt(eᵀΩe) is the error in units of standard deviations.

Assumptions. Most constraints are correct (the outliers are a minority); an initial guess good enough that correct constraints have small residuals, so that outliers stand out.

M-estimators and reweighting

Write the derivative of the robust cost as a weight: w(r) = ρ′(r) / r. Setting the gradient to zero gives the same equations as weighted least squares, with weights that depend on the current residuals. This suggests iteratively reweighted least squares (IRLS): solve a weighted problem, update the weights from the new residuals, repeat.

r

a residual in standard deviations: sqrt(eᵀΩe) for a vector error.

δ, c

tuning constants. δ ≈ 1.345 gives 95% efficiency for Gaussian data with Huber; c ≈ 2.385 for Cauchy.

w(r)

the weight applied to that constraint’s information in the next iteration: Ω ← w Ω.

Other approaches in SLAM

• Switchable constraints (Sünderhauf and Protzel, 2012): each loop closure carries a switch variable s ∈ [0, 1] that scales its information, with a prior pulling s toward 1. The optimiser can turn a constraint off if it is too inconsistent.
• Dynamic covariance scaling (Agarwal et al., 2013): closed-form switch value s = min(1, 2Φ/(Φ + χ²)) that scales a loop closure’s information by its own error; no extra variables.
• Max-mixtures (Olson and Agarwal, 2013): the likelihood of each edge is a mixture of a good component and a broad “null” component; the optimiser uses the most likely component.
• Consensus / pairwise consistency (PCM, Mangelson et al., 2018): keep the largest set of loop closures that are mutually consistent; discard the rest before optimising.
• Graduated non-convexity (Yang et al., 2020): start from a convex (smoothed) version of the robust cost and gradually make it non-convex, avoiding bad local minima without a good initial guess.

Limits

Robust losses assume the outliers are a minority and not systematic. A group of false closures that agree with each other (the same aliased corridor visited repeatedly) looks like a consensus. The non-convex losses help only if the initial guess is reasonable. And a robust cost with a too-small threshold also discards correct, if surprising, constraints, so the threshold should come from the expected noise.

A worked example: one bad measurement among five

Estimate a length from five measurements: 1.00, 1.10, 0.90, 1.05, 10.0 (the last is a gross error). Assume σ = 1 for simplicity.

1. Least squares gives the mean: 2.81. The outlier pulls the estimate by 1.8 and the four good values are all 1.8 below it.
2. Median (a robust statistic, for comparison): 1.05.
3. Huber with δ = 0.5, IRLS. Start from the mean 2.81. The residuals are −1.81, −1.71, −1.91, −1.76, +7.19; weights δ/|r| = 0.276, 0.292, 0.262, 0.284, 0.070. The weighted mean is 1.54. Iterate: the residuals shrink for the good points, whose weights rise to 0.92–1.0, and the outlier’s weight falls to 0.059. After four iterations the estimate settles at 1.14 (weights 1, 1, 1, 1, 0.056).
4. Cauchy with c = 0.5 converges to 1.02, with weights (0.998, 0.975, 0.945, 0.997, 0.003): the outlier is discounted almost to nothing, so the answer is the mean of the four good values (1.0125) up to small corrections.
5. Reading the weights. The weights are the diagnosis: the measurement whose final weight is far below 1 is the suspect. In a pose graph, the loop-closure edges with the lowest weights after optimisation are the ones to examine.
6. A caution. Had the outlier been two of the five (say 10.0 and 9.8), the initial mean 4.57 would be far from both groups and Huber could settle on the wrong side. Robust methods need the outliers to be fewer than the inliers and a start closer to the inliers than to the outliers.

The lab’s third stage uses exactly this idea, plus the chi-square test, to find and neutralise a false loop closure in a trajectory.

Engineering implications and limitations

Engineering implications

• Apply the robust kernel to loop-closure edges (the risky ones), not necessarily to odometry. g2o and Ceres let you set a kernel per edge or residual block.
• Set the threshold from the expected noise. With information from a chi-square distribution, an edge with χ² above the 99% threshold (11.34 for 3 degrees of freedom) is an outlier candidate; δ ≈ 1.345 σ for Huber.
• Log the final weights. After optimisation, list the edges by weight or switch value. A low weight means the optimiser disagreed with the constraint: that is the first thing to inspect, and a signal to improve the front end.
• Use a two-pass strategy. Optimise robustly, remove edges whose weight fell below, say, 0.1, and re-optimise with plain least squares on the remainder: cleaner final estimate.
• Do not rely on the back end alone. Verification and consistency checks in the front end (M4-T5) are cheaper and stronger; robust optimisation is the safety net.
• Test with injected outliers. Add synthetic false closures to a known graph and measure the error with and without the robust method.

Common misconceptions

Engineering takeaways

1. Least squares gives an outlier influence proportional to its error; a robust loss bounds or removes it.
2. IRLS solves a robust problem by repeating weighted least squares with weights w(r) = ρ′(r)/r.
3. Huber is convex and safe; Cauchy and Geman-McClure are stronger but need a good start.
4. Switchable constraints, DCS, max-mixtures, consensus and GNC are SLAM-specific ways to survive false loop closures.
5. The final weights are a diagnostic: inspect the constraints that were down-weighted.

References and further reading

Classical robust statistics and established SLAM methods; certifiably robust back ends (Module 8) are active research.

• Huber, P. J. (1964). Robust estimation of a location parameter. The Annals of Mathematical Statistics 35(1).
• Sünderhauf, N. and Protzel, P. (2012). Switchable constraints for robust pose graph SLAM. IEEE/RSJ IROS.
• Olson, E. and Agarwal, P. (2013). Inference on networks of mixtures for robust robot mapping. International Journal of Robotics Research 32(7).
• Agarwal, P., Tipaldi, G. D., Spinello, L., Stachniss, C. and Burgard, W. (2013). Robust map optimization using dynamic covariance scaling. IEEE International Conference on Robotics and Automation.
• Mangelson, J. G., Dominic, D., Eustice, R. M. and Vasudevan, R. (2018). Pairwise consistent measurement set maximization for robust multi-robot map merging. IEEE International Conference on Robotics and Automation.
• Yang, H., Antonante, P., Tzoumas, V. and Carlone, L. (2020). Graduated non-convexity for robust spatial perception: from non-minimal solvers to global outlier rejection. IEEE Robotics and Automation Letters 5(2).

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Graphs are not only for poses. The next lesson extends the same machinery to cameras and 3D points: bundle adjustment.

Related content

Nothing below is required to finish this lesson.

• Before
  M4-T5: Loop Closure

  Explain loop-closure detection and verification, and the cost of accepting a false closure.

• Lab
  M4-L3: Diagnose a Broken Loop Closure

  One loop closure is wrong and bends the map: find it with residual tests and make the optimisation robust.