FAST-Sync: Fast Group Synchronization for any Matrix Lie Group

Modern robotic perception increasingly involves large-scale geometric optimization problems distributed across multiple robots or sessions. However, existing distributed solvers often depend on brittle hand tuning and primarily target rigid body pose graphs. To address this, we present DeepCORD, a learning-augmented framework for distributed factor graph optimization on general matrix Lie groups. By unfolding a parallel and accelerated Riemannian optimizer into differentiable iterations, DeepCORD learns a self-supervised feedback policy that dynamically adapts solver parameters according to the optimization phase and communication status. The resulting method enables adaptive distributed optimization over matrix Lie groups under both synchronous and asynchronous communication regimes. Extensive experiments on real-world $\mathrm{SE}$(3) pose graph optimization and $\mathrm{SL}$(4) projective submap alignment show that our method achieves lower objective values than existing distributed baselines on most benchmarks across realistic operating scenarios.

Robust and efficient state estimation is crucial for perception, navigation, and control in robotics. State estimation problems are conveniently modeled using the factor-graph framework as enabled by modern software packages such as GTSAM or g2o. However, the standard solvers included in such frameworks are local and may converge to poor local minima, posing significant safety concerns. Conversely, techniques based on convex relaxations have been shown to provide a means of globally solving or certifying many state estimation problems. However, these relaxations 1) often require substantial effort to formulate, and 2) may incur significantly higher cost compared to efficient local solvers, as they require solving a large semidefinite program (SDP). In this work, we address both shortcomings by 1) creating a new procedure within the GTSAM framework for automatically constructing convex SDP relaxations for any factor graphs with common factor and variable types, and by 2) exploiting the Bayes tree constructions native to GTSAM to decompose the SDP problem, leading to significant speedup in solver time for chordally sparse problems. We demonstrate the favorable scaling of this structure-exploiting global estimator compared to standard local solvers for two case studies: A 3D pose-graph SLAM problem with a ring factor graph and a 2D localization problem with a chain factor graph. The software framework is available at https://github.com/borglab/gtsam.

Accurate, robust, and adaptive localization is essential for various robotic operations. This paper proposes a new message passing (MP) algorithm for realizing collaborative localization in a distributed manner. The algorithm unifies Gaussian belief propagation (GBP) and mean-field (MF) approximation, where GBP preserves dependencies among robot states, and MF enables estimation of noise statistics. To effectively handle non-conjugate terms from nonlinear measurement models, the algorithm adopts a parametric formulation in which these terms are treated by gradient estimators. Beyond linearization and sampling, we further design a normalizing flow (NF)-based gradient estimator, enabling learnable sampling. End-to-end training tunes NF parameters according to the behavior of MP, improving the overall estimation performance. To support estimation of practical robotic states that involve rotations, the method is then extended to Lie group state spaces. Finally, the method is applied to multirobot localization task fusing odometry, global navigation satellite system (GNSS) measurements, and inter-robot ultra wideband (UWB) ranging. Simulations and experiments on autonomous surface vehicles (ASVs) demonstrate its improved accuracy, robustness, and adaptability.