Distributed Pose Graph Optimization via Continuous Riemannian Dynamics

TL;DR

This paper introduces a novel distributed pose graph optimization framework using continuous-time Riemannian dynamics, improving convergence and communication efficiency in multi-robot systems.

Contribution

It formulates PGO as a second-order dynamical system on Lie groups and develops a distributed algorithm with neighbor prediction and energy dissipation analysis.

Findings

Achieves superior performance over state-of-the-art distributed methods.

Effectively handles delayed communication in multi-robot scenarios.

Demonstrates convergence in benchmark datasets.

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\'e 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.