Robust map optimization using dynamic covariance scaling

Developing the perfect SLAM front-end that produces graphs which are free of outliers is generally impossible due to perceptual aliasing. Therefore, optimization back-ends need to be able to deal with outliers resulting from an imperfect front-end. In this paper, we introduce dynamic covariance scaling, a novel approach for effective optimization of constraint networks under the presence of outliers. The key idea is to use a robust function that generalizes classical gating and dynamically rejects outliers without compromising convergence speed. We implemented and thoroughly evaluated our method on publicly available datasets. Compared to recently published state-of-the-art methods, we obtain a substantial speed up without increasing the number of variables in the optimization process. Our method can be easily integrated in almost any SLAM back-end.