iMESA: Incremental Distributed Optimization for Collaborative Simultaneous Localization and Mapping

TL;DR

iMESA introduces an incremental, distributed back-end for Collaborative SLAM that operates under sparse ad-hoc communications. Building on MESA, it amortizes constraint tightening via biased priors and dual updates, enabling real-time, online state estimation with limited inter-robot communication. Comprehensive experiments on synthetic and real data show iMESA outperforms state-of-the-art incremental back-ends and approaches centralized accuracy, while maintaining scalable runtimes. The work advances practical multi-robot mapping by delivering consistent, high-quality solutions under realistic communication constraints and provides open-source tooling to the community.

Abstract

This paper introduces a novel incremental distributed back-end algorithm for Collaborative Simultaneous Localization and Mapping (C-SLAM). For real-world deployments, robotic teams require algorithms to compute a consistent state estimate accurately, within online runtime constraints, and with potentially limited communication. Existing centralized, decentralized, and distributed approaches to solving C-SLAM problems struggle to achieve all of these goals. To address this capability gap, we present Incremental Manifold Edge-based Separable ADMM (iMESA) a fully distributed C-SLAM back-end algorithm that can provide a multi-robot team with accurate state estimates in real-time with only sparse pair-wise communication between robots. Extensive evaluation on real and synthetic data demonstrates that iMESA is able to outperform comparable state-of-the-art C-SLAM back-ends.

Figures (9)

• Figure 1: An illustration of the iMESA algorithm. When robots observe shared variables, they enforce equality using "biased priors" in their local factor-graph. Biased priors control constraint tightness with dual variables ($\lambda$) which are shown for robot A as purple in the robot's Bayes Tree. Over time, as communication is available, robots tighten equality constraints with dual gradient ascent to provide consistent solutions. Meanwhile, robots incorporate new measurements efficiently using the iSAM2 algorithm. Through this process, iMESA is able to accurately and efficiently solve incremental distributed C-SLAM problems.
• Figure 2: Example ground-truth synthetic datasets: (a) 3D PGO dataset from Exp. \ref{['sec:exp:models']}, (b) 25 robot 2D dataset from Exp. \ref{['sec:exp:scale']}, and (c) 5000 length 2D dataset from Exp. \ref{['sec:exp:lifelong']}. Each color is the trajectory of a different robot.
• Figure 3: iATE for our proposed algorithm iMESA ($\bigstar$) along with centralized ($\mdblkcircle$) and independent ($\blacklozenge$) baselines and prior works DDFSAM2 (), DLGBP ($\mdblksquare$), and Windowed DLGBP ($\blacktriangle$) on different C-SLAM formulations. Across problem formulations iMESA provides accurate results and outperforms state-of-the-art prior works.
• Figure 4: iATE for our proposed method iMESA ($\bigstar$) along with centralized ($\mdblkcircle$) and independent ($\blacklozenge$) baselines and prior works DDFSAM2 (), DLGBP ($\mdblksquare$), and Windowed DLGBP ($\blacktriangle$) as robot team size increases. Our proposed algorithm iMESA outperforms prior works and all methods provide consistent performance as team size grows.
• Figure 5: Total runtime for our proposed method iMESA ($\bigstar$) along with centralized ($\mdblkcircle$) and independent ($\blacklozenge$) baselines and prior works DDFSAM2 (), DLGBP ($\mdblksquare$), and Windowed DLGBP ($\blacktriangle$) as team size increases. All distributed methods scale well as team size increases, however, the centralized solver sees significantly increased runtime as team size grows.

Theorems & Definitions (11)

• Remark 1: Generic C-SLAM vs. PGO
• Remark 2: Constraint Functions
• Remark 3: MESA Convergence
• Remark 4: Batch C-SLAM Amortization
• Remark 5: Bookkeeping Initial Values
• Remark 6: Biased Prior Cache
• Remark 7: iSAM2 Implementation Details
• Remark 8: Two Stage Communication
• Remark 9: iMESA Convergence
• Remark 10: Relationship to iDFGO