Photogrammetry & Robotics Lab Least Squares Minimization

§ Minimizes sum of squared errors

Robust Least Squares § ML estimation for the Gaussian case
for SLAM

Cyrill Stachniss

Partial slide courtesy: Nived Chebrolu, Pratik Agarwal

1 2

Least Squares Minimization Data Association Is Ambiguous

And Not Always Perfect
§ Minimizes sum of squared errors
§ ML estimation for the Gaussian case § Places that look identical
§ Key assumption: No outliers! § Similar rooms in the same building
§ Cluttered scenes
Problems: § GPS multi path (signal reflections)
§ Outliers and ambiguities always occur § …
in the real world
§ Optimization is sensitive to outliers
§ Gaussian distributions (one mode)
3 4
Example Ambiguities

3D world belief about the

robot’s pose
Courtesy: E. Olson 5 Courtesy: E. Olson, P. Agarwal 6

Committing To The Wrong Mode Data Association Is Ambiguous

Can Lead to Mapping Failures And Not Always Perfect
§ Places that look identical
§ Similar rooms in the same building
§ Cluttered scenes
§ GPS multi path (signal reflections)
§ …

How to deal with this problem

in graph-based SLAM?
Courtesy: E. Olson, P. Agarwal 7 8
 Mathematical Model
§ Can we formulate constraints
modeling Gaussian noise differently?
MaxMixtures
or Dealing with Multiple Modes

9 10

Mathematical Model Problem

§ We can express a multi-modal belief § During error minimization, we consider
by a sum of Gaussians the negative log likelihood

Sum of Gaussians with k modes The log cannot be moved inside the sum!
11 12
Max-Mixture Approximation Max-Mixture Approximation
§ Instead of computing the sum of
Gaussians at , compute the
maximum of the Gaussians

13
approximation error 14

Log Likelihood Of The Max- Integration

Mixture Formulation
§ With the max-mixture formulation, the
§ The log can be moved inside the max log likelihood again results in local
operator quadratic forms
§ Easy to integrate in the optimizer:
1. Evaluate all k components
2. Select the component with the
maximum log likelihood
3. Perform the optimization as before
using only the max components
or:
(as a single Gaussian)
15 16
MM For Outlier Rejection MM For Outlier Rejection

Courtesy: E. Olson, P. Agarwal 17 Courtesy: E. Olson, P. Agarwal 18

Performance (Gauss vs. MM) Runtime

Courtesy: E. Olson, P. Agarwal 19 Courtesy: E. Olson, P. Agarwal 20

MM For Outlier Rejection and Max-Mixture and Outliers
Data Association Ambiguities
§ MM formulation is useful for multi-
model constraints (D.A. ambiguities)
§ MM is also a handy tool for dealing
with outliers
§ Outliers: one mode represents the
main constraint and a second model
uses a flat Gaussian for the outlier
hypothesis

Courtesy: E. Olson, P. Agarwal 21 22

Performance (1 outlier) Performance (10 outliers)

Gauss-Newton MM Gauss-Newton Gauss-Newton MM Gauss-Newton

23 24
Performance (100 outliers) MaxMixtues for Dealing with
Outliers
§ Supports multi-model constraints
§ Approximate the sum of Gaussians
using the max operator
§ Idea: “Select the best mode of a sum
of Gaussians and use it as if it would
be a single Gaussian”
§ Easy to use, quite effective
Gauss-Newton MM Gauss-Newton

25 26

Standard Least Squares

Dynamic Covariance Scaling

27 28
Dynamic Covariance Scaling Scaling Parameter

29 30

Dynamic Covariance Scaling Dynamic Covariance Scaling

Both have
squared error

31 32
Dynamic Covariance Scaling Dynamic Covariance Scaling

Original
error
Linearization
Scaled
error

33 34

Dynamic Covariance Scaling DCS for Dealing with Outliers

§ Add an additional weighting term to
the error function
§ The weight depends on the error value
§ Idea: “Weight down constraints that
are far away from the mean estimate”
§ A special case of robust least squares
estimation (Geman-McClure kernel)

35 36
 Optimizing With Outliers
§ Assuming a Gaussian error in the
constraints is not always realistic
Least Squares § Large errors are problematic
with Robust Kernels

37 38

Optimizing With Outliers Robust M-Estimators

§ Assuming a Gaussian error in the § Assume non-normally-distributed
constraints is not always realistic noise
§ Large errors are problematic § Intuitively: PDF with “heavy tails”
We need more § function used to define the PDF
probability mass
in the tails
of the distribution
§ Minimizing the neg. log likelihood

39 40
Robust M-Estimators: Different Rho Functions
Gaussian Case
§ Gaussian:
§ Kernel function used to define the § Absolute values (L1 norm):
PDF § Huber M-estimator

§ For the Gaussian case, we set

to be a quadratic function
§ Several others (Tukey, Cauchy, Blake-
Zisserman, Corrupted Gaussian, …)

41 42

Huber Loss Different Robust Loss Functions

§ Mixture of a quadratic and a linear
function
§ Quadratic around the solution (noise)
§ Linear for outliers (error > threshold)
L1 norm Huber Tukey

43 Cauchy Blake-Zisserman Corrupted G. 44

Robust Estimation as Robust Estimation
Weighted Least Squares as Weighted Least Squares
§ Weighted Least Squares § Gradient at optimum goes to zero
§ For weighted least squares:

§ Robust Estimation

45 46

Robust Estimation Robust Estimation

as Weighted Least Squares as Weighted Least Squares
§ Gradient at optimum goes to zero § Compare both equations
§ For the robust estimation:

47 48
Robust Estimation Robust Least Squares
as Weighted Least Squares
§ We can use the weighted least squares
§ Compare both equations approach to realize robust L.S.
§ The kernel will impact the Jacobians
§ The rest stays the same
§ The choice of the kernel must align
with the outlier distribution
§ We can use weighted least squares if
we set the weight using the kernel as:

49 50

Which Function to Chose?

§ Which loss/kernel function to chose?

Generalized Robust Kernels

51 52
Which Function to Chose? Which Function to Chose?
§ Which loss/kernel function to chose? § Which loss/kernel function to chose?
§ It depends on the type of outliers! § It depends on the type of outliers!
Some approaches combine them:
1. Start with a strong tails
for N1 iterations
2. N2 iteration with weaker tails
3. Remove all outliers larger c
4. Gaussian/Huber for the rest

53 54

General Robust Loss Function Adaptive Robust Loss Function

Source: A General and Adaptive Robust Loss Function, Barron [CVPR 2019] 55 56
Recommended Video to Watch Adaptive Robust Loss Function

https://www.youtube.com/watch?v=BmNKbnF69eY
57 58

Adaptive Robust Loss Function Adaptive Robust Loss Function

§ Jointly optimize over and § Adaptive loss function defined as

§ Define a probability distribution for the § with

general loss function

59 60
Adaptive Robust Loss Function Adaptive Robust Loss Function
§ Adaptive loss function defined as § Adaptive loss function defined as

§ with § with

approaches We can limit

the range of
infinity for outliers to
negative maintain a pdf
61 62

Adaptive Robust Loss Function Adaptive Robust Loss Function

§ Adaptive loss function defined as § Adaptive loss function defined as

§ with

We can limit
the range of
outliers to
maintain a pdf
63 64
Joint Optimization with the Joint Optimization with the
Adaptive Robust Kernel Adaptive Robust Kernel
§ In theory, we can now solve a joint § Joint optimization of :
optimization problem

Problems in practice:
§ New Jacobians need to be computed
§ in the weighted least squares sense § can dominate the parameter
using as our robust kernel estimation for complex problems
§ Sensitive to initial guess
65 66

EM-Based Optimization with the Iterative Closest Point Example

Adaptive Robust Kernel
§ Solve via Expectation-Maximization
§ E-Step: 1D line search problem

§ M-Step: Minimize as weighted least

squares

Outlier rejection in presence of dynamic objects

67 68
Iterative Closest Point Example EM-Based Optimization with the
Adaptive Robust Kernel
§ Kernel and unknown are estimated in
a iterative fashion
§ EM-based estimation provides better
results than the joint optimization
§ Kernel function adapts to the current
situation by adapting
§ No need to change the least squares
problem (Jacobians stay the same)
69 70

Dealing with Outliers Dealing with Outliers Summary

§ There are different ways for dealing § Max-Mixtures supports multi-model
with outliers during optimization constraints
§ Key question: how does the outlier § It approximates the sum of Gaussians
distribution look like? using the max operator
§ Dynamic Covariance Scaling is a good
choice for a fixed kernel in SLAM
§ DCS is a form of Geman-McClure
§ Choice of the robust loss function
depends on the problem at hand
71 72
Dealing with Outliers Summary Literature
§ Robust least squares using kernels Max-Mixtures:
§ Olson, Agarwal: “Inference on Networks of Mixtures
§ Choice of the rho function (kernel) for Robust Robot Mapping”
depends on the problem at hand Dynamic Covariance Scaling:
§ Agarwal, Tipaldi, Spinello, Stachniss, Burgard:
§ Popular: Huber, L1, DCS/Geman “Robust Map Optimization Using Dynamic Covariance
McClure Scaling”
§ Better: Don’t commit on one kernel General Robust Loss Function:
§ Barron: “A General and Adaptive Robust Loss
§ Adaptive robust kernel is the most Function”
flexible way EM-based Estimation of Kernel and LS Problem:
§ Chebrolu, Läbe, Vysotska, Behley, Stachniss:
§ We obtained best results with adaptive “Adaptive Robust Kernels for Non-Linear Least
kernels in an EM-style estimation 73
Squares Problems” 74

Slide Information
§ These slides have been created by Cyrill Stachniss as part of
the robot mapping course taught in 2012/13 and 2013/14. I
created this set of slides partially extending existing material
of Edwin Olson, Pratik Agarwal, and myself.
§ I tried to acknowledge all people that contributed image or
video material. In case I missed something, please let me
know. If you adapt this course material, please make sure
you keep the acknowledgements.
§ Feel free to use and change the slides. If you use them, I
would appreciate an acknowledgement as well. To satisfy my
own curiosity, I appreciate a short email notice in case you
use the material in your course.
§ My video recordings are available through YouTube:
http://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_&feature=g-list

Cyrill Stachniss, 2014

[email protected]
75