Distributed Pose Graph Optimization via Contractive Belief Sharing

Abstract

Following the relative maturity of single-robot Simultaneous Localization And Mapping (SLAM) techniques, works addressing collaborative SLAM have started emerging lately. Driven by the need for robust and scalable multi-robot systems, the community has been targeting Distributed Pose Graph Optimization (DPGO), with current DPGO methods falling into two categories: optimization-based methods providing favorable convergence properties at the expense of excessive communication rounds among participants, and belief-propagation methods that exhibit better scalability and faster computation, albeit risking divergence on loopy and noisy graphs. Inspired by the need for more effective DPGO techniques, this work introduces Contractive Belief Sharing (CBS), a two-stage message-passing algorithm that combines Maximum-A-Posteriori (MAP) optimization with belief propagation with a Hellinger-distance-based damping rule. In this way, CBS ensures fast and reliable convergence while maintaining fully distributed computation and communication with neighbors only. Experiments on benchmarks show that CBS reaches convergence substantially faster and more efficient and scalable than the state-of-the-art methods while maintaining high trajectory accuracy, opening up new capabilities for collaborative SLAM.