Current state of the art solutions of the SLAM problem are based on efficient 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.

Our solution

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.

Results

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

Vertigo (Versatile Extensions for Robust Inference using Graphical Models) provides a C++ implementation of the switchable constraints approach I developed during my thesis. It is an extension library for g2o and gtsam 2.0 and enables g2o or gtsam to solve pose graph SLAM problems in 2D and 3D despite a large number of false positive loop closure constraints. In addition, a remplementation of the max-mixture model described in (Olson and Agrawal, 2012) for g2o and gtsam is also contained.

Furthermore, Vertigo contains a number of standard pose graph SLAM datasets and a script to spoil them with false positive loop closure constraints. These datasets have been used in our evaluations. They can serve as a set of benchmark datasets for future developments in robust pose graph SLAM.

Download: Vertigo is available at openslam.org.

You can use SVN to check it out: svn co https://svn.openslam.org/data/svn/vertigo

Publications

Please see the publications below for further details.

  1. Switchable Constraints for Robust Pose Graph SLAM Niko Sünderhauf, Peter Protzel. In Proc. of IEEE International Conference on Intelligent Robots and Systems (IROS), 2012. Current SLAM back-ends are based on least squares optimization and thus are not robust against outliers like data association errors and false positive loop closure detections. Our paper presents and evaluates a robust back-end formulation for SLAM using switchable constraints. Instead of proposing yet another appearance-based data association technique, our system is able to recognize and reject outliers during the optimization. This is achieved by making the topology of the underlying factor graph representation subject to the optimization instead of keeping it fixed. The evaluation shows that the approach can deal with up to 1000 false positive loop closure constraints on various datasets. This largely increases the robustness of the overall SLAM system and closes a gap between the sensor-driven front-end and the back-end optimizers.
  1. Switchable Constraints vs. Max-Mixture Models vs. RRR – A Comparison of three Approaches to Robust Pose Graph SLAM Niko Sünderhauf, Peter Protzel. In Proc. of Intl. Conf. on Robotics and Automation (ICRA), 2013. SLAM algorithms that can infer a correct map despite the presence of outliers have recently attracted increasing attention. In the context of SLAM, outlier constraints are typically caused by a failed place recognition due to perceptional aliasing. If not handled correctly, they can have catastrophic effects on the inferred map. Since robust robotic mapping and SLAM are among the key requirements for autonomous longterm operation, inference methods that can cope with such data association failures are a hot topic in current research. Our paper compares three very recently published approaches to robust pose graph SLAM, namely switchable constraints, maxmixture models and the RRR algorithm. All three methods were developed as extensions to existing factor graph-based SLAM back-ends and aim at improving the overall system’s robustness to false positive loop closure constraints. Due to the novelty of the three proposed algorithms, no direct comparison has been conducted so far.
  1. Towards a Robust Back-End for Pose Graph SLAM Niko Sünderhauf, Peter Protzel. In Proc. of IEEE Intl. Conf. on Robotics and Automation (ICRA), 2012. We propose a novel 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 demonstrate the approach and present results both on large scale synthetic and real-world dataset
  1. Robust Optimization for Simultaneous Localization and Mapping Niko Sünderhauf. PhD Thesis. Chemnitz University of Technology 2012.

We successfully applied our approach to robust optimization to the problem of multipath and NLOS mitigation in satellite-based localization:

  1. Switchable Constraints and Incremental Smoothing for Online Mitigation of Non-Line-of-Sight and Multipath Effects Niko Sünderhauf, Marcus Obst, Sven Lange, Gerd Wanielik, Peter Protzel. In Proc. of IEEE Intelligent Vehicles Symposium (IV), 2013. Reliable vehicle positioning is a crucial requirement for many applications of advanced driver assistance systems. While satellite navigation provides a reasonable performance in general, it often suffers from multipath and non-line-of-sight errors when it is applied in urban areas and therefore does not guarantee consistent results anymore. Our paper proposes a novel online method that identifies and excludes the affected pseudorange measurements. Our approach does not depend on additional sensors, maps, or environmental models. We rather formulate the positioning problem as a Bayesian inference problem in a factor graph and combine the recently developed concept of switchable constraints with an algorithm for efficient incremental inference in such graphs. We furthermore introduce the concepts of auxiliary updates and factor graph pruning in order to accelerate convergence while keeping the graph size and required runtime bounded. A realworld experiment demonstrates that the resulting algorithm is able to successfully localize despite a large number of satellite observations are influenced by NLOS or multipath effects.
  1. Multipath Mitigation in GNSS-Based Localization using Robust Optimization Niko Sünderhauf, Marcus Obst, Gerd Wanielik, Peter Protzel. In Proc. of IEEE Intelligent Vehicles Symposium (IV), 2012. Our paper adapts recent advances in the SLAM (Simultaneous Localization and Mapping) literature to the problem of multipath mitigation and proposes a novel approach to successfully localize a vehicle despite a significant number of multipath observations. We show that GNSS-based localization problems can be modelled as factor graphs and solved using efficient nonlinear least squares methods that exploit the sparsity inherent in the problem formulation. Using a recently developed novel approach for robust optimization, satellite observations that are subject to multipath errors can be successfully identified and rejected during the optimization process. We demonstrate the feasibility of the proposed approach on a real-world urban dataset and compare it to an existing method of multipath detection.