Distributed Pose Graph Optimization via Continuous Riemannian Dynamics

2026-05-11

AI Summary

This work addresses the challenges of convergence and communication efficiency in distributed pose graph optimization (PGO) for multi-robot systems by proposing a novel approach based on a second-order continuous dynamical system on Lie groups. By introducing a damped particle model, the equilibrium points of the system are aligned with the first-order critical points of PGO. A fully distributed optimization algorithm is then constructed using the damped Euler–Poincaré equations together with a semi-implicit geometric integrator. The method jointly models states and velocities to enable neighbor state prediction, significantly enhancing convergence under high-latency communication, and provides a unified generalization of existing algorithms such as Riemannian gradient descent and Gauss–Newton. Experimental results demonstrate that the proposed solver consistently outperforms state-of-the-art distributed baselines on standard PGO datasets under both synchronous and asynchronous settings.

Abstract

We present a framework for distributed Pose Graph Optimization (PGO) by formulating the problem as a second-order continuous-time dynamical system evolving on Lie groups. By modeling pose variables as massive particles subject to damping, the equilibrium points of the resulting Riemannian dynamics coincide with first-order critical points of the original PGO problem. Using the governing damped Euler--Poincaré equations and a semi-implicit geometric integrator, we design an optimization algorithm that generalizes existing algorithms such as Riemannian gradient descent and Gauss--Newton. In multi-robot settings, we present a fully distributed and parallel method based on block-diagonal mass and damping matrices, where each robot solves an ordinary differential equation for its own poses with minimal communication overhead. Moreover, modeling both state and velocity enables principled neighbor prediction that significantly improves convergence under delayed communication. Theoretically, we present an analysis and establish sufficient condition that ensures energy dissipation under the employed geometric discretization scheme. Experiments on benchmark PGO datasets demonstrate that the proposed solver achieves superior performance compared to state-of-the-art distributed baselines in both synchronous and asynchronous regimes.

Problem

• Distributed Pose Graph Optimization
• Multi-robot systems
• Communication delay
• Riemannian dynamics
• Lie groups

Innovation

• distributed pose graph optimization
• Riemannian dynamics
• geometric integration
• multi-robot systems
• damped Euler–Poincaré equations