Testing Robust Noise Models
Robust Noise Models
Traditional M-Estimators
Traditional M-Estimators have been research for many years. The first comprehensive book on the subject was published by Huber in 1981. The field of research related to M-estimation can be boiled down to reducing the influence of outliers by replacing the $L^2$ cost function with a modified cost function.
So, M-Estimators replace the traditional $L^2$ cost,
$$ min \sum_i e_i^2 $$
with a modified cost function
$$ min \sum_i \rho(e_i) $$
One of the most important qualities of an M-Estimator is that the weight approaches 0 as the residual approachs $\infty$. This function property is call redescending and be defined more formally as
$$ \lim_{e_i \to \infty} \rho'(e_i) = 0 $$
A table of m-estimator cost functions is provided below. This table was taken from this thesis.
Graph Based Methods
Switch Constraints (S.C.)
Switchable constraints was introduced by Sunderhauf in 2012 in this paper
The main idea behind the switchable constraints is that the topology of the factor graph that represents the pose graph SLAM problem, should be partially variable and subject to the optimization instead of being kept fixed. This way, edges representing outlier constraints can be removed from the graph during the optimization. This is achieved by augmenting the original problem and introducing an additional type of hidden variable: A switch variable is associated with each factor that could potentially represent an outlier. This additional variable acts as a multiplicative scaling factor on the information matrix associated with that constraint. Depending on the state of the switch variable (a value between 0 and 1), the resulting information matrix is either the original matrix (when the switch is equal to 1) or 0 (when the switch is 0) or something between both ends. Notice that if the switch variable is equal to 0, the associated constraint is completely removed and has no influence on the overall solution
Dynamic Covariance Scaling (D.C.S.)
Dynamic Covariance Scaling was introduced in 2013 in this paper as a closed-form solution to switch factors.
The DCS approach reduces the confidence of an observable by de-weighting the corresponding element in the information matrix by the scale factor
$$ s_{ij} = min(1,\frac{2\Phi}{\Phi + \mathcal{X}}) $$
Ultimitely this is just the Geman-McClure weighting.
Max-Mixtures (M.M)
Max-Mixtures was introduced in 2012 in this paper .
The Max-Mixture model consists of a front-end loop-closure and null hypotheses. The front-end loop-closure hypothesis represents the distribution of the inlier loop-closure constraints and the null hypothesis represents the distribution of the outlier loopconstraints. Each loop-closure constraint is verified against the hypotheses iteratively within the optimization loops and the weight associated with the most likely hypothesis is used to scale the Jacobian, residual and information matrix from that loop-closure constraint. In other words, the hypothesis testing acts as an “selector” to the weighting of the loop closure constraint.
REF $\rightarrow$ this paper
Realizing, Reversing, Recovering (R.R.R)
RRR was introduced in 2012 in this paper . This was then extended to an incremental estimator in this paper
RRR divides loop closures into clusters based on topological similarity and then tries to find the largest subset of clusters than are consistent among themselves as well as with the underlying odometry. Consistency is considered in the chi-squared ($\mathcal{X}^2)$ sense. The algorithm first carries out consistency checks for each cluster in order to weed out incorrect links within it, followed by an intra-cluster consistency check. RRR is different from the previous algorithms as it explicitly requires convergence of the graph in order to verify the validity of loop closures. In contrast with SC and DCS, in RRR and MM loop closure decisions are not modeled as continuous variables but as discrete yes/no decisions that need to be made
REF $\rightarrow$ this paper
Comparison
A good paper comparing SC to DCS to RRR is provided by Sunderhauf
The robust back-end formulation known as Switchable constraints was introduced by Sunderhauf in 2012.
Noise Model Testing
Data-set
Clean Data
The Manhattan world with 3500 nodes is a common graph optimization test data-set. The truth ground trace is shown in the figure below.
True Manhattan wold 3500 ground truth
Adding faults
Gaussian noise with standard deviation 0.3rad is added to the relative orientation measurements.
Testing
Using the erronous data, each of the noise models above will be tested. For all weighting functions, Levenberg Marquardt is utilized with a kernal width of 10.
$L^2$ Cost Function
First we will test the traditional $L^2$ minimization. A video of the optimizer going through 100 iterations is shown below.
M-Estimators
Cauchy Cost Function
Next, we will test the Cauchy weighting function. A video of the optimizer going through 100 iterations is shown below.
Geman-McClure Cost Function
Now, a video of the optimization process with the Geman-McClure cost function.
Saturaded Cost Function
Applying the saturated cost function to the optimization process
Tukey Cost Function
Applying the tukey cost function to the optimization process
Tukey Cost Function
The last M-Estimator that will be tested is the Welcsh weighting. A video of the optimization process is shown below.