Current state of the art solutions of the SLAM problem are based on efﬁcient sparse optimization techniques and represent the problem as probabilistic constraint graphs.
For example in pose graphs the nodes represent poses and the edes between them express spatial information (e.g. obtained from odometry) and information on loop closures. The task of constructing the graph is delegated to a front-end that has access to the available sensor information.
The optimizer, the so called back-end of the system, relies heavily on the topological correctness of the graph structure and is not robust against misplaced constraint edges. Especially edges representing false positive loop closures will lead to the divergence of current solvers.
We developed a novel problem formulation that allows the back-end to change parts of the topological structure of the graph during the optimization process. The back-end can thereby discard loop closures and converge towards correct solutions even in the presence of false positive loop closures. This largely increases the overall robustness of the SLAM system and closes a gap between the sensor-driven front-end and the back-end optimizers.
We propose to augment the original optimization problem by a new set of hidden variables. These switch variables allow the optimizer to estimate the optimal graph topology and the optimal covariance for each potential outlier constraint. Despite the increased number of variables, the sparse structure of the problem is maintained, which allows efficient calculations.
For the evaluation we added false positive loop closures to several standard datasets that are commonly used in the SLAM community. These false positive loop closures were added following different policies to achieve a realistic distribution. Despite a vast number of false loop closure constraints, our proposed robust method is able to converge to a correct solution, discarding all outlier constraints.
This works for a large real-world dataset as well. We tested it on the St. Lucia Dataset which covers 66 km of suburban roads. The only sensor information are the images of a webcam mounted on top of a car. The vehicle’s ego-motion and the place recognition were performed using very simple methods (like BRIEF-Gist) on these images. Despite the large number of false positive loop closure detections caused by the simplistic place recognition module, the SLAM system is able to perform well and create a semi-metric map that is almost correct. (Thanks to Michael Milford for providing the dataset.
Download the Source Code and Datasets
Please see the publications below for further details.
- Sünderhauf, N., Protzel, P. (2012). Switchable Constraints for Robust Pose Graph SLAM. Proc. of IEEE International Conference on Intelligent Robots and Systems (IROS), Vilamoura, Portugal.
- Sünderhauf, N., Protzel, P. (2013). Switchable Constraints vs. Max-Mixture Models vs. RRR – A Comparison of Three Approaches to Robust Pose Graph SLAM. Proc. of IEEE International Conference on Robotics and Automation (ICRA), Karlsruhe, Germany.
- Sünderhauf, N., Protzel, P. (2012). Towards a Robust Back-End for Pose Graph SLAM. Proc. of IEEE International Conference on Robotics and Automation (ICRA), St. Paul, USA.
- Sünderhauf, N. (2012). Robust Optimization for Simultaneous Localization and Mapping. PhD Thesis, Chemnitz University of Technology.
We successfully applied our approach to robust optimization to the problem of multipath and NLOS mitigation in satellite-based localization:
- Sünderhauf, N., Obst, M., Lange, S., Wanielik, G., Protzel, P. (2013). Switchable Constraints and Incremental Smoothing for Online Mitigation of Non-Line-of-Sight and Multipath Effects. Proc. of IEEE Intelligent Vehicles Symposium (IV), Gold Coast, Australia
- Sünderhauf, N., Obst, M., Wanielik, G., Protzel, P. (2012). Multipath Mitigation in GNSS-Based Localization using Robust Optimization. Proc. of IEEE Intelligent Vehicles Symposium (IV), Madrid, Spain.