riMESA: Consensus ADMM for Real-World Collaborative SLAM

Daniel McGann; Michael Kaess

cs.ROMar 1, 2026

riMESA: Consensus ADMM for Real-World Collaborative SLAM

Authors: Daniel McGann, Michael Kaess

Organizations: The authors are with the Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USA.

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 & tables

Function	$z\in$	$q(\theta,z)$	$z^{k+1}_{(i,j)_{s}}$
Geodesic	$\mathrm{SE}(N)$	$\mathrm{Log}\left(z^{-1}\circ\theta\right)$	$\mathrm{SPLIT}\left(\theta_{s_{i}},\theta_{s_{j}},0.5\right)$
Apx.-Geo.	$\mathbb{R}^{p}$	$\mathrm{Log}\left(\theta\right)-z$	$\frac{1}{2}\left(\mathrm{Log}\left(\theta_{s_{i}}\right)+\mathrm{Log}\left(\theta_{s_{j}}\right)\right)$
Split	$\mathrm{SE}(N)$	$\begin{bmatrix}\mathrm{Log}\left(R_{z}^{-1}R_{\theta}\right)\\ t_{\theta}-t_{z}\end{bmatrix}$	$\mathrm{SPLIT}\left(\theta_{s_{i}},\theta_{s_{j}},0.5\right)$
Chordal	$\mathbb{R}^{N^{2}+N}$	$\mathrm{Vec}(\theta)-z$	$\frac{1}{2}\left(\mathrm{Vec}\left(\theta_{s_{i}}\right)+\mathrm{Vec}\left(\theta_{s_{j}}\right)\right)$

TABLE I : Constraint functions for

\mathrm{SE}(N)

objects and their corresponding closed-form solutions to ( 17 ). Where SPLIT interpolates the translation component linearly and the rotation component spherically [ 77 ] , Vec returns a vector of the objects non-constant matrix elements [ 78 ] , and

p

is the dimension of the tangent space for SE

(N)

. Manifold object notation is derived from the work of Solà et al. [ 79 ] . The geodesic

z

update is approximated by the case where

\lambda_{(i,j)_{s}}=\lambda_{(j,i)_{s}}=\mathbf{0}

Panel of Figure 2 — Fig. 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.

Panel of Figure 3 — Fig. 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.

Fig. 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

Panel of Figure 5 — Fig. 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.

Panel of Figure 6 — Fig. 6 : Metric performance for iMESA ( $\pentagonblack$ ), riMESA ( $\bigstar$ ), kiMESA ( $\mdblksquare$ ), DDF-SAM2 ( ), and DLGBP ( $\blacktriangle$ ) on various planar-constrained problem scenarios for different communication conditions. The baselines Centralized Oracle ( $\mdblkcircle$ ) and Independent ( $\blacklozenge$ ) are not dependent on communication and are plotted as horizontal lines. Communication quality, defined by [delay ( $b_{c}$ ), rate ( $r_{c}$ ), max-distance ( $d_{c}$ )], improves from a to e, where a= $[10\mathfrak{s},1\mathfrak{r},30m]$ , b= $[2\mathfrak{s},1\mathfrak{r},35m]$ , c= $[0\mathfrak{s},1\mathfrak{r},40m]$ , d= $[0\mathfrak{s},5\mathfrak{r},45m]$ , e= $[0\mathfrak{s},10\mathfrak{r},50m]$ . Higher quality communication enables riMESA to provide better results across all scenarios, but particularly for low signal-to-noise problems like (c) Range-Only SLAM.

Panel of Figure 7 — Fig. 7 : riMESA ablation studies. In (a) we compare riMESA ( $\bigstar$ ) to a version without dual-decay ( –D ), a version without robust kernels on weighted biased priors ( –R ), and a version without both of these components ( –B ). In (b) we compare riMESA ( $\bigstar$ ) to a version that uses only local estimates to initialize shared variables ( L ) and a version that uses only estimates from owner robots to initialize shared variables ( O ). Both ablation studies validate that the components improve the performance of riMESA.

Panel of Table 8 — TABLE II : iATE (translation) performance for all methods and baselines on the COSMO-Bench Datasets [ 87 ] . For each trial, we provide results from datasets generated with both communication models (Wi-Fi and Pro-Radio). The best-performing method (excluding baselines) for each dataset is bolded. The proposed algorithm, riMESA (highlighted in gray), performs consistently across all datasets.

Panel of Figure 9 — Fig. 8 : Qualitative results from riMESA on a sample of COSMO-Bench datasets. Colors represent different robots. The pointcloud map is constructed by transforming LiDAR scans according to the riMESA solution. Visible by the alignment between each robot’s map, riMESA is able to provide accurate and consistent state estimates.

Panel of Table 10 — TABLE III : iATE (translation) performance for all methods and baselines on the Nebula Datasets [ 24 ] released by COSMO-Bench [ 87 ] . The best-performing method (excluding baselines) for each dataset is bolded. The proposed algorithm is able to closely match the performance of the Centralized GNC baseline (expected performance upper-bound), though all methods do generally well on these datasets.

Panel of Figure 11 — Fig. 9 : The runtime performance (cumulative and per-update) on main_campus_wifi . (a) Distributed algorithm performance – iMESA ( $\pentagonblack$ ), riMESA ( $\bigstar$ ), kiMESA ( $\mdblksquare$ ), DDF-SAM2 ( ), DLGBP ( $\blacktriangle$ ), and Independent ( $\blacklozenge$ ). (b) Centralized baseline performance – Centralized Oracle ( $\mdblkcircle$ ), Centralized GNC ( $\hexagonblack$ ), and Centralized PCM ( \Octosteel ). The real-time bounds for the dataset are shown by black lines ( $\strns$ ), and we note that the update real-time bound is not constant as the gap between measurements varies. Apart from DDF-SAM2, all distributed methods are able to maintain real-time performance. While the Centralized Oracle can maintain real-time performance, the robust centralized methods required for real-world operation are too computationally expensive to operate in real-time. Note: The y-axis scales are linear up to 0.01s and 1s for update/cumulative runtime plots, respectively, and log-scale above these thresholds.

Simultaneous Localization and Mapping

Simultaneous localization and mapping estimates a robot’s motion while building a map of an unknown environment.

See all 197 papers

SGM-SLAM: Scene Graph Matching for Data-Efficient Distributed SLAM
Jun 15, 2026Yewei Huang, Tixiao Shan, Abhinav Rajvanshi +4Simultaneous Localization and Mapping Multi-Robot Systems
Dartmouth College, 100 N Main St, Hanover, NH 03755, USA · SRI International, 201 Washington Rd, Princeton, NJ 08540, USA · Stevens Institute of Technology, 1 Castle Point Terrace, Hoboken, NJ 07030, USA
Edge-Assisted Multi-Robot Visual-Inertial SLAM with Efficient Communication
Mar 11, 2026Xin Liu, Shuhuan Wen, Jing Zhao +2Simultaneous Localization and Mapping Visual-Inertial SLAM
the Department of Key Lab of Industrial Computer Control Engineering of Hebei Province, Engineering Research Center of the Ministry of Education for Intelligent Control System and Intelligent Equipment, Yanshan University, Qinhuangdao, 066004, China. · Intelligent Transport System Research Center, Wuhan University of Technology, Wuhan, China · the Department of Civil and Environmental Engineering at the University of Alberta, Canada. +1
Consistent Distributed Invariant EKF SLAM with Point and Object Features
Oct 22, 2026Mengya Xu, Xianjia Yu, Ha Sier +2Simultaneous Localization and Mapping Invariant EKF

Jun 15, 2026J/K move · Enter open · S save

SGM-SLAM: Scene Graph Matching for Data-Efficient Distributed SLAM

Yewei Huang, Tixiao Shan, Abhinav Rajvanshi, Niluthpol Chowdhury Mithun+3

Dartmouth College, 100 N Main St, Hanover, NH 03755, USA · SRI International, 201 Washington Rd, Princeton, NJ 08540, USA · Stevens Institute of Technology, 1 Castle Point Terrace, Hoboken, NJ 07030, USA

Simultaneous Localization and Mapping Multi-Robot Systems

Open paper AskPDF arXiv

We introduce a data-efficient distributed Simultaneous Localization and Mapping (SLAM) framework for multi-robot operation in GPS-denied and communication-constrained environments. The framework is designed for teams of robots equipped with LiDAR, cameras, and inertial sensors, where robots may operate from independently initialized local coordinate frames and share only limited spatial overlap. Our approach constructs a scene graph by using fused RGB-LiDAR point clouds to generate both a semantically segmented point cloud layer, and a layer of discrete bounded objects, to accompany estimated robot trajectories. Scene graph matching is performed collaboratively through exchanging and matching object data with neighboring robots. To maximize communication efficiency, we utilize a multi-step data exchange and optimization process. We demonstrate the effectiveness and communication efficiency of the proposed approach using real-world datasets containing limited trajectory overlap, viewpoint variation, repeated structures, and degraded visual conditions.

Abstract

Simultaneous Localization and Mapping

Explore similar work

SGM-SLAM: Scene Graph Matching for Data-Efficient Distributed SLAM

Edge-Assisted Multi-Robot Visual-Inertial SLAM with Efficient Communication

Consistent Distributed Invariant EKF SLAM with Point and Object Features