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 ($q_i$) and translation ($t_i$) variables by introducing auxiliary variables $p_i$ and $s_i$, with equality constraints:

$$p_i = q_i,\quad t_i = s_i,\quad p_i,q_i\in \mathcal{U},\quad t_i,s_i\in \mathbb{R}^3$$

The objective splits as $f(p,q,t,s) + g(p,q)$, where $f$ aggregates translation penalties and $g$ rotation penalties, subject to linear constraints $p-q=0$, $t-s=0$. This approach is designed to enable per-vertex closed-form updates.

3. Augmented Lagrangian Construction

PRADMM employs an augmented Lagrangian with dual multipliers $\lambda_i\in \mathbb{R}^4$ for $p_i-q_i=0$ and $z_i\in \mathbb{R}^3$ for $t_i-s_i=0$, with penalties $\beta_1, \beta_2 > 0$:

$$\mathcal{L}_\beta(x, \lambda, z) = f(x) + g(x_1, x_2) - \sum_i \langle \lambda_i, p_i-q_i \rangle + \frac{\beta_1}{2}\|p_i-q_i\|^2 - \sum_i \langle z_i, t_i-s_i \rangle + \frac{\beta_2}{2}\|t_i-s_i\|^2$$

where $x_1=p$, $x_2=q$, $x_3=t$, $x_4=s$. 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 $\tau\in (0,2)$ and block-diagonal proximal regularizers $H_k$:

1. $p$-subproblem (sphere TRS):

$$p^{k+1} = \arg\min_{p\in \mathcal{U}^n} \mathcal{L}_\beta(p, q^k, t^k, s^k, \lambda^k, z^k) + \frac{1}{2}\|p-p^k\|_{H_1}^2$$

For each $i$:

$$p_i^{k+1} = \arg\min_{\|p_i\|=1} \frac{1}{2} p_i^\top A_{1,i}^k p_i + (b_{1,i}^k)^\top p_i$$

• Isotropic case: $p_i^{k+1}$ is the normalized $-b_{1,i}^k$.
• Anisotropic case: rightmost eigenpair solution.

2. $q$-subproblem (Euclidean least-squares):

$q_i^{k+1} = (A_{2,i}^k)^{-1} b_{2,i}^k$

3. $t$-subproblem (Euclidean least-squares):

$t_i^{k+1} = (A_{3,i}^k)^{-1} b_{3,i}^k$

4. $s$-subproblem (Euclidean least-squares):

$s_i^{k+1} = (A_{4,i}^k)^{-1} b_{4,i}^k$

5. Dual updates (over-relaxed):

$$\lambda_i^{k+1} = \lambda_i^k - \tau \beta_1 (p_i^{k+1} - q_i^{k+1}),\quad z_i^{k+1} = z_i^k - \tau \beta_2 (t_i^{k+1} - s_i^{k+1})$$

All updates are performed per-vertex in parallel. The per-vertex cost is $O(\deg(i))$, and when average degree $s=m/n$ is constant, overall iteration cost is $O(ns)=O(m)$.

PRADMM Pseudocode

Initialize p⁰,q⁰,t⁰,s⁰ randomly (q⁰ on 𝕌), λ⁰,z⁰, choose β₁,β₂>0, τ∈(0,2), H₁…H₄. for k=0,1,2,… until convergence do for i=1…n (in parallel) do compute A_{1,i}^k,b_{1,i}^k → p_i^{k+1} (TRS on sphere) compute A_{2,i}^k,b_{2,i}^k → q_i^{k+1} (LS) compute A_{3,i}^k,b_{3,i}^k → t_i^{k+1} (LS) compute A_{4,i}^k,b_{4,i}^k → s_i^{k+1} (LS) end for λ^{k+1} = λ^k − τ β₁ (p^{k+1}−q^{k+1}) z^{k+1} = z^k − τ β₂ (t^{k+1}−s^{k+1}) end for

5. Convergence Properties

Under the assumptions that $f,g$ are Lipschitz-smooth, bounded below, block-multi-convex, and that the $q$-block lies on a smooth compact manifold (the sphere), PRADMM converges as follows for any $\tau\in(0,2)$ and sufficiently large $\beta=O(1)$:

• A merit function $\Psi$ 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 ($s = m/n$), each per-vertex subproblem for $p$, $q$, $t$, and $s$ incurs $O(s)$ computational cost. Consequently, a full iteration entails $O(ns)=O(m)$ work. In comparison, traditional methods such as Gauss–Newton or factor-graph solvers generally exhibit $O((m+n)^{1.5})$ 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 $n=5,000$): PRADMM matches or exceeds state-of-the-art accuracy metrics (Rel.Err, NRMSE), while achieving 2–10$\times$ speedups over SE-Sync and over 100$\times$ speedups compared to manifold Gauss–Newton for large $n$.
• Cube grids ($n=\hat{n}^3$ up to $\hat{n}=10$): PRADMM maintains sub-second solutions for $m\approx 4,000$ and Rel.Err$\approx$0.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$\times$ 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 $n$ vertices with near-constant computation per vertex.
• Over-relaxed dual ascent permitting relaxation parameters $\tau\in(0,2)$.
• 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).