Distributed Pose Graph Optimization via Continuous Riemannian Dynamics

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.

• The paper presents CORD, a distributed PGO framework that recasts optimization as a continuous-time dynamical system on Lie groups.
• It leverages second-order dynamics with inertia and damping to accelerate convergence and mitigate delays in multi-robot communication.
• Empirical evaluations demonstrate CORD’s robustness and superior performance against state-of-the-art methods in both synchronous and asynchronous settings.

Distributed Pose Graph Optimization via Continuous Riemannian Dynamics: A Technical Overview

Introduction and Motivation

Pose Graph Optimization (PGO) is the computational backbone of collaborative simultaneous localization and mapping (CSLAM), a foundational component for scalable and accurate multi-robot autonomy in GNSS-denied or bandwidth-constrained environments. While distributed PGO alleviates the scalability bottlenecks and single points of failure inherent to centralized approaches, existing distributed solvers often trade off convergence speed, parallelizability, and robustness to asynchronous or delayed communication. The work "Distributed Pose Graph Optimization via Continuous Riemannian Dynamics" (2605.11210) introduces CORD, a general-purpose distributed PGO algorithm. CORD leverages a second-order continuous-time dynamical system evolving on product Lie groups, endowing the optimization process with inertia and damping—mechanisms that systematically accelerate convergence and mitigate the effects of staleness endemic to multi-robot communication.

Continuous-Time Riemannian Dynamics for PGO

Traditional distributed PGO approaches operate via either first-order (gradient-based) or carefully structured second-order (e.g., Gauss-Newton, MM) updates. CORD introduces a fundamentally different perspective by recasting the PGO objective as the potential energy in a physical system, with robot poses modeled as massive particles on $SE(3)^N$ subject to inertia and dissipation.

The approach utilizes a damped Euler–Poincaré system on the product of $SE(3)$ Lie groups, with the system's Lagrangian derived as the difference between kinetic and potential energies, and damping injected to ensure convergence. Unlike first-order (gradient) flows, the inclusion of mass and damping in the system induces a second-order ODE:

$$\frac{d}{dt}(M\xi) = -\nabla \mathcal{C}(X) - D\xi + \mathrm{ad}^*_\xi(M\xi),$$

where $M$ and $D$ are positive-definite mass (inertia) and damping matrices, $\xi$ is the body velocity in the Lie algebra, and the co-adjoint operator captures geometric curvature effects on the manifold. This dynamical systems viewpoint enables both acceleration in synchronous updates and stability under asynchronous, delayed neighbor information.

Figure 1: The proposed approach formulates pose graph optimization (PGO) as a continuous-time dynamical system evolving on the direct product $\mathcal{M}$ of $SE(3)$ Lie groups, governed by a damped Euler–Poincaré equation. The black trajectory shows rapid, oscillation-suppressed convergence to a PGO critical point, outperforming Jacobi-like updates (gray path).

The equilibrium points of these dynamics correspond exactly with the first-order critical points of the original PGO objective. Notably, when the damping dominates (overdamped limit), the equations recover Riemannian gradient descent and Gauss–Newton updates as special cases, providing a strict generalization.

Distributed, Parallel, and Asynchronous Execution

CORD is designed for natural distributed decomposition. By using block-diagonal mass and damping matrices, robots independently integrate local ODEs, with inter-robot communication only required for the estimation of inter-robot constraints—aggregated via neighbor communication.

Crucially, CORD supports asynchronous optimization under delayed communications. Instead of naïvely using stale neighbor poses, each robot transmits both its pose and body velocity. This enables each agent to perform principled neighbor state prediction (using local integration of the received velocity). This predictive mechanism substantially counteracts convergence degradation due to delayed or lossy communication.

Figure 2: Trajectory estimates for multiple robots on benchmark PGO datasets under CORD, with color indicating robot identity, highlighting accurate distributed trajectory recovery.

Theoretical Convergence and Energy Dissipation

The authors present a rigorous analysis of the discretized geometric integrator—specifically, a semi-implicit Euler scheme—that underpins CORD. Using a local Lipschitz-type gradient assumption, it is shown that the total “energy” of the numerical system (kinetic plus potential) dissipates under an explicit step-size condition, paralleling classical Lyapunov stability analyses. The analysis yields a closed-form sufficient step size bound ensuring monotonic descent of the system's total energy, which is robust under moderate discretization and guides practical implementation.

Figure 3: Relative mass variation and system energy dissipation for state-dependent parameters on chordal datasets, demonstrating monotonic energy convergence and minor mass variation in practice.

Empirical Evaluation

Benchmark evaluations encompass both chordal and geodesic distance-based PGO problems, using both synthetic and real SLAM benchmarks.

• Synchronous setting: CORD achieves top or near-top optimality gaps compared to state-of-the-art AMM-PGO and MESA solvers across all tested datasets for both chordal and geodesic error metrics.
• Synchronous performance profiles (see Figure 4) show that CORD solves more problems within tight optimality thresholds per iteration than DJ and is competitive with highly specialized accelerated baselines.

  Figure 4: Performance profiles of synchronous methods on 3D SLAM benchmarks, showing fraction of problems solved versus iteration for chordal (left) and geodesic (right) PGO.

• Asynchronous setting: Under fixed or random communication delays, CORD is consistently more robust than Jacobi-type (DJ) or Nesterov-based baselines, sustaining low optimality gaps and stable progress even with random packet loss and heterogeneous delay.

  Figure 5: Optimality gap trajectories as a function of communication delay steps in asynchronous simulations. CORD maintains convergence, while DJ exhibits oscillatory or divergent behaviors at large delays.

An ablation study confirms that neighbor prediction, enabled by body velocity communication, is critical for maintaining accuracy and preventing divergence in asynchronous settings. State-dependent mass/damping matrices yield marginal gains, indicating a favorable computational-accuracy trade-off.

Practical and Theoretical Implications

CORD generalizes a spectrum of distributed second-order PGO algorithms, subsuming and extending gradient and Gauss–Newton updates via its dynamical formulation. The acceleration from added inertia and active compensation for delayed/stale information enables scalable, energy-dissipative distributed optimization suitable for large, bandwidth- or power-constrained multi-robot teams.

On the theoretical front, CORD's variational dynamics approach could be extended to general distributed factor graph optimization over matrix Lie groups and inspires future work in adaptive parameterization, step size adaptation, and global optimality certification via distributed verification.

Conclusion

CORD is a distributed PGO framework built upon continuous-time Riemannian dynamics, providing both a theoretical foundation and practical algorithm for accelerated, robust, and parallel pose graph optimization in multi-robot systems. Its dynamical systems approach unifies and extends prior methods, attaining high empirical performance and analytic guarantees even under challenging asynchronous conditions. Extensions to more general distributed factor graph optimization and adaptive or learned parameter selection are promising avenues for broadening its applicability.