New distributed PGO algorithm beats baselines using Riemannian dynamics

This paper presents a novel distributed Pose Graph Optimization (PGO) approach that reframes the optimization problem as a second-order continuous-time dynamical system evolving on Lie groups. By modeling each pose as a massive particle subject to damping, the equilibrium points of the resulting Riemannian dynamics correspond exactly to the first-order critical points of the original PGO problem. The authors leverage damped Euler–Poincaré equations combined with a semi-implicit geometric integrator to design an optimization algorithm that unifies and generalizes classic methods like Riemannian gradient descent and Gauss–Newton. The key innovation for multi-robot environments is a fully distributed, parallel method relying on block-diagonal mass and damping matrices, where each robot solves an ordinary differential equation for its own poses with minimal cross-robot communication.

Furthermore, the framework’s modeling of both state and velocity enables a principled neighbor prediction strategy that significantly improves convergence under delayed communication — a critical factor in real-world multi-robot deployments. Theoretical analysis establishes sufficient conditions ensuring energy dissipation under the chosen geometric discretization scheme. Experimental results on benchmark PGO datasets demonstrate that the proposed solver consistently outperforms state-of-the-art distributed baselines, achieving superior convergence rates in both synchronous and asynchronous regimes. This work offers a strong theoretical foundation and practical algorithms for scaling SLAM and other pose estimation tasks across teams of robots.