Fast Loop Closure Selection Method with Spatiotemporal Consistency for Multi-Robot Map Fusion

Abstract

This paper presents a robust method based on graph topology to find the topologically correct and consistent subset of inter-robot relative pose measurements for multi-robot map fusion. However, the absence of good prior on relative pose gives a severe challenge to distinguish the inliers and outliers, and once the wrong inter-robot loop closures are used to optimize the pose graph, which can seriously corrupt the fused global map. Existing works mainly rely on the consistency of spatial dimension to select inter-robot measurements, while it does not always hold. In this paper, we propose a fast inter-robot loop closure selection method that integrates the consistency and topology relationship of inter-robot measurements, which both conform to the continuity characteristics of similar scenes and spatiotemporal consistency. Firstly, a clustering method integrating topology correctness of inter-robot loop closures is proposed to split the entire measurement set into multiple clusters. Then, our method decomposes the traditional high-dimensional consistency matrix into the sub-matrix blocks corresponding to the overlapping trajectory regions. Finally, we define the weight function to find the topologically correct and consistent subset with the maximum cardinality, then convert the weight function to the maximum clique problem from graph theory and solve it. We evaluate the performance of our method in a simulation and in a real-world experiment. Compared to state-of-the-art methods, the results show that our method can achieve competitive performance in accuracy while reducing computation time by 75%.