Detecting the correct graph structure in pose graph SLAM

Abstract

While graph-based representations allow an efficient solution to the SLAM problem posing it as a non-linear least squares optimization, they require additional methods to detect and eliminate outliers. It is necessary to obtain the correct structure of the graph representing the SLAM problem which is topologically correct and will lead to a metric correct solution once optimized. In the graph-SLAM context, the edges represent constraints relating two poses whereas the vertices represent the robot poses. While all the edges between consecutive poses actually exist (odometry), the same may not be true for edges coming from a place recognition system. Place recognition algorithms always have some degree of failure in the presence of perceptual aliasing in real environments, creating edges between otherwise unrelated poses. We argue that rather than mitigating the effect of incorrect loop closures, these edges must be detected and removed as they represent non-existent topological connections in the graph. In this paper we describe a method that is able to detect and remove such false edges, leading to a solution of the SLAM problem based on the resulting topologically correct graph. Our method is robust both to outliers in place recognition as well as errors in odometry systems. We focus our experiments on real world and synthetic datasets and provide comparisons against other robust SLAM methods.