MR-iSAM2: Incremental Smoothing and Mapping with Multi-Root Bayes Tree for Multi-Robot SLAM

We present multi-robot iSAM2 (MR-iSAM2), an efficient incremental smoothing and mapping (iSAM) algorithm to solve multi-robot simultaneous localization and mapping (SLAM) inference problems. MR-iSAM2 is based on a novel data structure multi-root Bayes tree (MRBT), which packs multiple Bayes trees with the same undirected clique structure. In multi-robot scenarios, the MRBT enables new measurements from different robots to be updated in different root branches, while all updates are performed around the single root of the Bayes tree in the original iSAM2 algorithm. As a result, the MRBT better reveals the underlying sparsity and information flow in multi-robot SLAM inference problems than the Bayes tree. Based on this insight, we further develop MR-iSAM2 to incrementally update and maintain the sparsity structure of the MRBT and enable efficient information propagation among the roots for inter-robot inference. We analyze the properties of the MR-iSAM2 algorithm, and show with both synthetic and real world datasets that it significantly outperforms iSAM2 in efficiency when solving multi-robot SLAM problems.