PRADMM for Pose Graph Optimization

• The paper introduces PRADMM, which decouples global interactions via variable splitting and closed-form per-vertex updates, reducing computation compared to traditional methods.
• PRADMM leverages an augmented Lagrangian and Riemannian convergence proofs under mild manifold conditions to ensure robust, global convergence.
• Empirical evaluations on synthetic and real-world datasets demonstrate that PRADMM achieves 2–10× speedups and competitive accuracy (Rel.Err and NRMSE) over state-of-the-art optimizers.

The Parallelizable Riemannian Alternating Direction Method of Multipliers (PRADMM) is a specialized optimization algorithm for non-convex pose graph optimization (PGO), targeting large-scale robotics and SLAM applications. By leveraging variable splitting and equality-constrained reformulation, PRADMM achieves closed-form updates per vertex and enables efficient parallelization, ensuring near-linear scalability with graph size. Its convergence is established via Riemannian arguments under mild smoothness and manifold assumptions, and empirical benchmarks demonstrate superior performance compared to traditional and state-of-the-art methods (Chen et al., 22 Jan 2026).

1. Non-Convex Pose Graph Optimization Formulation

Pose graph optimization underpins numerous robot perception and navigation systems, providing the core estimation for SLAM. Let $\mathcal{G}=(\mathcal{V},\mathcal{E})$ be a directed graph with $n=|\mathcal{V}|$ poses and $m=|\mathcal{E}|$ relative-pose measurements. Each pose $i$ is characterized by a rotation $R_i\in SO(3)$ and translation $t_i\in \mathbb{R}^3$, with measurements $(R_{ij}, t_{ij})$.

The maximum-likelihood formulation is:

$$\min_{ \{R_i\}\in SO(3),\, \{t_i\}\in \mathbb{R}^3 } \sum_{(i,j)\in\mathcal{E}} \| R_i^T(t_j-t_i)- t_{ij} \|_{\Sigma_1}^2 + \| \log( R_i^T R_j R_{ij}^T ) \|_{\Sigma_2}^2$$

An equivalent representation uses unit quaternions $q_i\in \mathcal{U}\subset \mathbb{R}^4$:

$$\min_{q_i\in \mathcal{U},\, t_i\in\mathbb{R}^3} \sum_{(i,j)} \| M(q_i)[0;t_i] - [0;t_{ij}] - M(q_i) D p_j \|^2 + \dots \quad\text{subject to}\quad \|q_i\|=1$$

This formulation poses global coupling across the graph, impeding parallelization and closed-form updates.

2. Variable-Splitting and Reformulation

To decouple global interactions, PRADMM duplicates quaternion ($n=|\mathcal{V}|$0) and translation ($n=|\mathcal{V}|$1) variables by introducing auxiliary variables $n=|\mathcal{V}|$2 and $n=|\mathcal{V}|$3, with equality constraints:

$n=|\mathcal{V}|$4

The objective splits as $n=|\mathcal{V}|$5, where $n=|\mathcal{V}|$6 aggregates translation penalties and $n=|\mathcal{V}|$7 rotation penalties, subject to linear constraints $n=|\mathcal{V}|$8, $n=|\mathcal{V}|$9. This approach is designed to enable per-vertex closed-form updates.

3. Augmented Lagrangian Construction

PRADMM employs an augmented Lagrangian with dual multipliers $m=|\mathcal{E}|$0 for $m=|\mathcal{E}|$1 and $m=|\mathcal{E}|$2 for $m=|\mathcal{E}|$3, with penalties $m=|\mathcal{E}|$4:

$m=|\mathcal{E}|$5

where $m=|\mathcal{E}|$6, $m=|\mathcal{E}|$7, $m=|\mathcal{E}|$8, $m=|\mathcal{E}|$9. This structure ensures that the update subproblems for each variable block can be solved independently with closed-form solutions.

4. PRADMM Iterative Update Scheme

Each iteration applies a five-step blockwise update with relaxation parameter $i$0 and block-diagonal proximal regularizers $i$1:

1. $i$2-subproblem (sphere TRS):

$i$3

For each $i$4:

$i$5

• Isotropic case: $i$6 is the normalized $i$7.
• Anisotropic case: rightmost eigenpair solution.

2. $i$8-subproblem (Euclidean least-squares):

$i$9

3. $R_i\in SO(3)$0-subproblem (Euclidean least-squares):

$R_i\in SO(3)$1

4. $R_i\in SO(3)$2-subproblem (Euclidean least-squares):

$R_i\in SO(3)$3

5. Dual updates (over-relaxed):

$R_i\in SO(3)$4

All updates are performed per-vertex in parallel. The per-vertex cost is $R_i\in SO(3)$5, and when average degree $R_i\in SO(3)$6 is constant, overall iteration cost is $R_i\in SO(3)$7.

PRADMM Pseudocode

$$\min_{ \{R_i\}\in SO(3),\, \{t_i\}\in \mathbb{R}^3 } \sum_{(i,j)\in\mathcal{E}} \| R_i^T(t_j-t_i)- t_{ij} \|_{\Sigma_1}^2 + \| \log( R_i^T R_j R_{ij}^T ) \|_{\Sigma_2}^2$$2

5. Convergence Properties

Under the assumptions that $R_i\in SO(3)$8 are Lipschitz-smooth, bounded below, block-multi-convex, and that the $R_i\in SO(3)$9-block lies on a smooth compact manifold (the sphere), PRADMM converges as follows for any $t_i\in \mathbb{R}^3$0 and sufficiently large $t_i\in \mathbb{R}^3$1:

• A merit function $t_i\in \mathbb{R}^3$2 decreases monotonically and is lower bounded.
• The primal and dual residuals vanish asymptotically.
• All limit points reached by the iterates are first-order stationary.
• Invoking a Riemannian Kurdyka–Łojasiewicz argument, the entire sequence is guaranteed to converge (finite-length property).

This suggests robust theoretical guarantees for practical deployment in large-scale graph optimization tasks.

6. Computational Complexity and Parallelization

With constant average graph degree ($t_i\in \mathbb{R}^3$3), each per-vertex subproblem for $t_i\in \mathbb{R}^3$4, $t_i\in \mathbb{R}^3$5, $t_i\in \mathbb{R}^3$6, and $t_i\in \mathbb{R}^3$7 incurs $t_i\in \mathbb{R}^3$8 computational cost. Consequently, a full iteration entails $t_i\in \mathbb{R}^3$9 work. In comparison, traditional methods such as Gauss–Newton or factor-graph solvers generally exhibit $(R_{ij}, t_{ij})$0 or worse scaling. PRADMM's per-vertex update structure lends itself to trivial parallelization, facilitating near-constant memory and computation scaling with respect to the graph size.

7. Empirical Evaluation

Extensive validation is provided on synthetic and real-world datasets:

• Synthetic circular ring and cube datasets (up to $(R_{ij}, t_{ij})$1): PRADMM matches or exceeds state-of-the-art accuracy metrics (Rel.Err, NRMSE), while achieving 2–10$(R_{ij}, t_{ij})$2 speedups over SE-Sync and over 100$(R_{ij}, t_{ij})$3 speedups compared to manifold Gauss–Newton for large $(R_{ij}, t_{ij})$4.
• Cube grids ($(R_{ij}, t_{ij})$5 up to $(R_{ij}, t_{ij})$6): PRADMM maintains sub-second solutions for $(R_{ij}, t_{ij})$7 and Rel.Err$(R_{ij}, t_{ij})$80.14.
• Real-world 3D SLAM benchmarks (tinyGrid, garage, sphere1/sphere2, torus3D; up to 9,000 edges): PRADMM executes in 0.3–1.0s, matches SE-Sync’s error performance in rotation and translation, and is 3–20$(R_{ij}, t_{ij})$9 faster.

Performance Metrics Table

Dataset	Solver	Rel.Err	NRMSE	Time (s)
Circular Ring, n=100	SE-Sync	0.0711	0.0354	0.18
	PRADMM	0.0689	0.0343	0.065
Circular Ring, n=1000	SE-Sync	0.0463	0.0232	0.38
	PRADMM	0.0457	0.0229	0.22
Circular Ring, n=5000	SE-Sync	0.0451	0.0225	0.72
	PRADMM	0.0439	0.0219	0.26

A plausible implication is that PRADMM is well-suited for real-time and large-scale graph optimization in SLAM, with a clear advantage in scalability and computational efficiency compared to established solvers.

8. Summary of Algorithmic Features

PRADMM, as developed by Chen et al. (2025), combines the following key features:

• Structured variable splitting into four blocks, each block solvable in closed-form or via simple computations.
• Natural parallelization across $$\min_{ \{R_i\}\in SO(3),\, \{t_i\}\in \mathbb{R}^3 } \sum_{(i,j)\in\mathcal{E}} \| R_i^T(t_j-t_i)- t_{ij} \|_{\Sigma_1}^2 + \| \log( R_i^T R_j R_{ij}^T ) \|_{\Sigma_2}^2$$0 vertices with near-constant computation per vertex.
• Over-relaxed dual ascent permitting relaxation parameters $$\min_{ \{R_i\}\in SO(3),\, \{t_i\}\in \mathbb{R}^3 } \sum_{(i,j)\in\mathcal{E}} \| R_i^T(t_j-t_i)- t_{ij} \|_{\Sigma_1}^2 + \| \log( R_i^T R_j R_{ij}^T ) \|_{\Sigma_2}^2$$1.
• Provable global convergence to stationary points, contingent on mild smoothness and manifold properties.
• Demonstrable superiority in computational time and robustness when applied to large synthetic and real-world PGO datasets (Chen et al., 22 Jan 2026).