riMESA: Consensus ADMM for Real-World Collaborative SLAM

TL;DR

Robust Incremental Manifold Edge-based Separable ADMM (riMESA) is proposed -- a robust, incremental, and distributed C-SLAM back-end that is resilient to outliers, reliable in the face of limited communication, and can compute accurate state estimates for a multi-robot team in real-time.

Abstract

Collaborative Simultaneous Localization and Mapping (C-SLAM) is a fundamental capability for multi-robot teams as it enables downstream tasks like planning and navigation. However, existing C-SLAM back-end algorithms that are required to solve this problem struggle to address the practical realities of real-world deployments (i.e. communication limitations, outlier measurements, and online operation). In this paper we propose Robust Incremental Manifold Edge-based Separable ADMM (riMESA) -- a robust, incremental, and distributed C-SLAM back-end that is resilient to outliers, reliable in the face of limited communication, and can compute accurate state estimates for a multi-robot team in real-time. Through the development of riMESA, we, more broadly, make an argument for the use of Consensus Alternating Direction Method of Multipliers as a theoretical foundation for distributed optimization tasks in robotics like C-SLAM due to its flexibility, accuracy, and fast convergence. We conclude this work with an in-depth evaluation of riMESA on a variety of C-SLAM problem scenarios and communication network conditions using both synthetic and real-world C-SLAM data. These experiments demonstrate that riMESA is able to generalize across conditions, produce accurate state estimates, operate in real-time, and outperform the accuracy of prior works by a factor >7x on real-world datasets.

Figures (9)

• Figure 1: An illustration of riMESA operating on real-world data (kth_r3_00_proradio). riMESA estimates the state of a multi-robot team ($\mdwhtcircle$,$\mdwhtcircle$,$\mdwhtcircle$) from noisy, potentially incorrect measurements ($\smblkcircle$,$\smblkcircle$,$\smblkcircle$) using only sparse, unreliable communication (). riMESA is a C-ADMM-based distributed optimization algorithm in which robots locally constrain shared state using "biased priors" ($\smblkcircle$). Over time, as communication is available, riMESA tightens equality constraints with dual variables ($\lambda$) to provide consistent solutions for the team. Meanwhile, robots incorporate new measurements efficiently using the riSAM algorithm, which handles potential outlier measurements (origin=c]45$\circlerighthalfblack$) using M-Estimations and an incremental version of Graduated Non-Convexity, which efficiently updates only the relevant subproblem ($\mdblksquare$) at each timestep.
• Figure 2: Example groundtruth synthetic datasets. (a) An unconstrained 3D dataset. (b) A planar-constrained 3D C-PGO dataset. (c) A planar-constrained 3D landmark C-SLAM dataset. Each color represents a different robot with a trajectory length of 1000 poses.
• Figure 3: Metric performance for Centralized Oracle ($\mdblkcircle$), iMESA ($\pentagonblack$), riMESA ($\bigstar$), kiMESA ($\mdblksquare$), DDF-SAM2 (), DLGBP ($\blacktriangle$), and Independent ($\blacklozenge$) on various planar-constrained datasets for different levels of measurement noise. Only the yaw axis noise ($\sigma_{rz}$) is changed, and datasets use fixed $\sigma_r=0.25^\circ$ and $\sigma_t=0.05m$. Across all problem scenarios, our proposed method, riMESA, outperforms prior works and achieves the closest solution to that of the baselines that exploit oracle outlier information. However, there are some problem conditions for which riMESA struggles at high levels of measurement noise. Note: DLGBP is omitted from landmark scenarios as it does not support passive landmarks.
• Figure 4: Metric performance for Centralized Oracle ($\mdblkcircle$), iMESA ($\pentagonblack$), riMESA ($\bigstar$), kiMESA ($\mdblksquare$), DDF-SAM2 (), DLGBP ($\blacktriangle$), and Independent ($\blacklozenge$) on a unconstrained 3D C-PGO datasets for different levels of measurement noise. Only $\sigma_{r}$ is changed and datasets use fixed $\sigma_t=0.05m$.
• Figure 5: Metric performance for Centralized Oracle ($\mdblkcircle$), iMESA ($\pentagonblack$), riMESA ($\bigstar$), kiMESA ($\mdblksquare$), DDF-SAM2 (), DLGBP ($\blacktriangle$), and Independent ($\blacklozenge$) across different (a) operation lengths $L$ and (b) different team-sizes $R$. riMESA provides quality performance across all problem scales.

Theorems & Definitions (11)

• Remark 1: Generic C-SLAM vs. PGO
• Remark 2: MESA vs. MESA+
• Remark 3: Batch C-SLAM Amortization
• Remark 4: Shared Variable Identification
• Remark 5: riSAM Implementation Details
• Remark 6: Two-Stage Communication
• Remark 7: Use of Cached Data in Communication Incorporation
• Remark 8: Relationship to Prior Work
• Remark 9: Synthetic Dataset Timing
• Remark 10: Real-World Experiment Nuances