Robust Pose-Graph Loop-Closures with Expectation-Maximization Gim Hee Lee 1 , Friedrich Fraundorfer 2 , and Marc Pollefeys 1 1 Computer Vision and Geometry Lab, Department of Computer Science, ETH Z¨ urich 2 Remote Sensing Technology, Faculty of Civil Engineering and Surveying, Technische Universit¨ at M¨ unchen glee@student.ethz.ch, friedrich.fraundorfer@tum.de, marc.pollefeys@inf.ethz.ch Abstract—In this paper, we model the robust loop-closure pose-graph SLAM problem as a Bayesian network and show that it can be solved with the Classification Expectation- Maximization (EM) algorithm. In particular, we express our robust pose-graph SLAM as a Bayesian network where the robot poses and constraints are latent and observed variables. An additional set of latent variables is introduced as weights for the loop-constraints. We show that the weights can be chosen as the Cauchy function, which are iteratively computed from the errors between the predicted robot poses and observed loop- closure constraints in the Expectation step, and used to weigh the cost functions from the pose-graph loop-closure constraints in the Maximization step. As a result, outlier loop-closure constraints are assigned low weights and exert less influences in the pose-graph optimization within the EM iterations. To prevent the EM algorithm from getting stuck at local minima, we perform the EM algorithm multiple times where the loop constraints with very low weights are removed after each EM process. This is repeated until there are no more changes to the weights. We show proofs of the conceptual similarity between our EM algorithm and the M-Estimator. Specifically, we show that the weight function in our EM algorithm is equivalent to the robust residual function in the M-Estimator. We verify our proposed algorithm with experimental results from multiple simulated and real-world datasets, and comparisons with other existing works. I. I NTRODUCTION The focus of many existing works [1], [2] on the back-end pose-graph SLAM problem is on improving the efficiency of the optimization algorithms. Most of these optimization al- gorithms assumed that the constraints provided by the front- end are free from errors, and would fail if this assumption was violated. For most cases, these errors are from erroneous loop-closure constraints. Erroneous loop-closure constraints are the result of wrong place recognitions by the appearance or vocabulary-tree based approaches and this problem is aggravated in environments with highly repetitive scenes. Despite the numerous efforts [3], [4] to improve the accuracy of the front-end recognition, none of these algorithms is totally free from false positives. The task of identifying erroneous loop-closure constraints is always left to the front- end, and it is only until the recent two years that several works [5]–[9] demonstrated the ability robustly detect and disable erroneous loop-closure constraints within the back- end optimization process. In this paper, we propose a robust pose-graph SLAM opti- mization algorithm based on the Classification EM algorithm [10] to robustly detect and minimize the influences from the (a) (b) (c) (d) (e) (f) Fig. 1. (a)-(c) City1000 dataset randomly corrupted with 1, 10 and 100 outlier loop-closure constraints (red lines) leading to wrong convergence with standard pose-graph SLAM. (d)-(f) Our EM algorithm detects all the outliers and converges to the correct solution. Green and blue lines are the correct loop-closure and odometry constraints. erroneous loop-closure constraints within the optimization process. In particular, we express our robust pose-graph SLAM as a Bayesian network where the robot poses and constraints are latent and observed variables. An additional set of latent variables is introduced as weights for the loop- constraints. We show that the weights can be chosen as the Cauchy function, which are iteratively computed from the errors between the predicted robot poses and observed loop-closure constraints in the Expectation step, and used to weigh the cost functions from the pose-graph loop-closure constraints in the Maximization step. As a result, outlier loop-closure constraints are assigned low weights and exert less influences in the pose-graph optimization within the EM iterations. To prevent the EM algorithm from getting stuck at local minima, we perform the EM process multiple times where the loop constraints with very low weights are removed after each EM process. This is repeated until there are no more changes to the weights. We show proofs of the conceptual similarity between our EM algorithm and the M-Estimator [11]. Specifically, we show that the weight function in our EM algorithm is equivalent to the robust residual function in the M-Estimator. We verify our proposed algorithm with experimental results from multiple simulated and real-world datasets, and comparisons with other existing works. An example of the results from our algorithm is 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) November 3-7, 2013. Tokyo, Japan 978-1-4673-6357-0/13/$31.00 ©2013 IEEE 556