AI Research Portal

This paper introduces Parallelizable Riemannian Alternating Direction Method of Multipliers (PRADMM) for non-convex Pose Graph Optimization (PGO) in Simultaneous Localization and Mapping (SLAM). PRADMM reformulates PGO by duplicating variables and introducing equality constraints, enabling efficient parallel computation across graph vertices. The core idea is to achieve closed-form solutions for all subproblems, ensuring stable performance and near-constant computational complexity relative to graph scale. The method establishes global convergence under weaker conditions and permits extended relaxation step sizes. Empirical validation on synthetic and real-world 3D SLAM benchmarks demonstrates PRADMM's superior computational performance and scalability compared to state-of-the-art graph-SLAM methods.

Existing PGO solvers face several bottlenecks:

Computational Complexity: Current solvers exhibit polynomial growth in computational complexity with graph size, hindering real-time deployment in large-scale scenarios.

Decouples vertex correlations through variable splitting and constraint propagation, developing PRADMM with closed-form subproblem solutions. Each subproblem complexity is O(s), where s is the sparsity measurement, which is near-constant relative to graph scale.
Establishes global convergence under row rank-deficient coefficient matrices, a significantly weaker condition than existing requirements. It permits extended relaxed dual steps τ ∈ (0,2), surpassing conventional limits (τ ∈ (0, (1 + √5)/2) for convex and τ = 1 for non-convex cases).
Evaluates the algorithm on two synthetic datasets at varying scales and five widely-used 3D SLAM benchmark datasets. Experimental results demonstrate that PRADMM consistently outperforms state-of-the-art graph-SLAM methods.

The paper reformulates the Pose Graph Optimization (PGO) problem, which is initially expressed as minimizing a sum of squared errors involving relative pose measurements (rotations q and translations t) subject to unit quaternion constraints.

Objective: The original PGO problem is given by: min ||t_j - t_i - q_i t_ij p_i||² + ||p_i q_i q_ij - 1||² {q_i,t_i} (i,j)∈E s.t. q_i ∈ U, t_i ∈ R³, i = 1, 2, ..., n, where q_i ∈ U are unit quaternions and t_i ∈ R³ are 3-dimensional vectors.

Reformulation via Structured Splitting: To address scalability, the method introduces auxiliary variables p_i and s_i, reformulating the problem into an equivalent model: min Σ_{(i,j)∈E} {||t_j - s_i - q_i t_ij p_i||² + ||p_i q_i q_ij - 1||²} {p_i,q_i,t_i,s_i} s. t. p_i = q_i, t_i = s_i, p_i ∈ U, q_i ∈ R⁴, t_i ∈ R³, s_i ∈ R³, i = 1, ..., n. This reformulation transforms the quartic polynomial into a multi-linear model and decouples vertex interdependencies, enabling closed-form solutions and parallel computation.

PRADMM Algorithm: The problem is solved using an augmented Lagrangian function: Lβ(p, q, t, s, λ, z) = f(p,q,t,s) + g(p,q) + Σ_{i=1}^n {λ_i^T (p_i - q_i) + β1/2 ||p_i - q_i||²} + Σ_{i=1}^n {z_i^T (t_i - s_i) + β2/2 ||t_i - s_i||²} where λ ∈ R⁴ⁿ and z ∈ R³ⁿ are Lagrange multipliers, and β1, β2 > 0 are penalty parameters. The iterative scheme involves updating p, q, t, s, λ, and z.

Iterative Updates:

p-subproblem (3a): p^(k+1) ∈ arg min Lβ(p,q^k,t^k,s^k,λ^k,z^k) + Q1(p). This is solved in parallel for each p_i. If coefficient matrices are scalar, it has a direct solution. Otherwise, it's an eigenvalue problem solved by Algorithm 1.
q-subproblem (3b): q^(k+1) ∈ arg min Lβ(p^(k+1),q,t^k,s^k,λ^k,z^k) + Q2(q). Solved in parallel with a closed-form solution.
t-subproblem (3c): t^(k+1) ∈ arg min Lβ(p^(k+1),q^(k+1),t,s^k,λ^k,z^k) + Q3(t). Solved in parallel with a closed-form solution.
s-subproblem (3d): s^(k+1) ∈ arg min Lβ(p^(k+1),q^(k+1),t^(k+1),s,λ^k,z^k) + Q4(s). Solved in parallel with a closed-form solution.
Dual variable updates (3e, 3f): λ^(k+1) = λ^k - τβ1(p^(k+1) - q^(k+1)) z^(k+1) = z^k - τβ2(t^(k+1) - s^(k+1)) where τ ∈ (0,2) is the relaxed step size.

Objective Function: The PGO objective (1) minimizes the sum of squared errors for relative translation and rotation measurements. Rotations are unit quaternions (q_i ∈ U), and translations are 3D vectors (t_i ∈ R³).
Noise Model: Relative pose measurements are generated with noise: t_ij = R_i(t_j - t_i) + t_ε, where t_ε ~ N(0, σ_T²I_3). q_ij = q_i q_ε q_j⁻¹, where q_ε ~ vMF([1,0,0,0], κ⁻¹). Here, R_i is the rotation representation of vertex i in SO(3), and vMF denotes a von Mises-Fisher distribution.
Augmented Lagrangian: The augmented Lagrangian Lβ includes penalty parameters β1, β2 > 0 and Lagrange multipliers λ ∈ R⁴ⁿ, z ∈ R³ⁿ.
Optimization: The PRADMM algorithm (Algorithm 2) iteratively updates primal variables (p, q, t, s) and dual variables (λ, z).
- Subproblem Solutions: All subproblems admit closed-form solutions. The p-subproblem (5) is a special quadratic constraint quadratic programming with spherical constraint, which can be reformulated as an eigenvalue problem (Algorithm 1).
- Parallelization: Vertex interdependencies are decoupled, allowing for massively parallel computation of p, q, t, and s updates.
- Computational Complexity: The algorithm achieves O(m/n) computational complexity for both rotations and translations, which is near-constant relative to graph scale for fixed graph sparsity (constant ratio m/n).
- Relaxation Step Size: The relaxed dual step size τ is set to 1.4 in experiments and theoretically allowed in the interval (0,2).
Initial Points: All algorithms use chordal initialization.
Parameters:
- Penalty parameters: β1, β2 > 0.
- Relaxation step size: τ ∈ (0,2). Empirically set to τ = 1.4.
- Positive definite matrices: H_i > 0, i = 1,...,4. Empirically, H1 ∈ {0.1,1,10} and H_i = 0.001 for i = 2,3,4.
- Tolerance: tol = 10⁻⁴.
- Maximum iterations: MaxIter = 300 for PRADMM.
Experimental Setup: Experiments were performed using MATLAB 2020a on an Intel i7-10700F CPU desktop computer with 16GB of memory. Results are averaged over 5 runs.

Datasets:
- Synthetic Datasets:
  - Circular ring: Single loop with radius 2, odometric edges, m = n.
  - Cube dataset: Robot travels on a 2 × 2 × 2 grid world, random loop closures with probability P_cube. Total vertices n = ñ³, expectation of edges E(m) = 2(2ñ³ – 3ñ² + 1)P_cube + ñ³ - 1.
- Real-world Datasets: Five widely-used 3D SLAM benchmark datasets (e.g., sphere datasets).
Metrics:
- Relative Error (Rel. Err.)
- Normalized Root Mean Square Error (NRMSE)
- Loss(θ) (rotation angle error)
- Loss(t) (translation error)
- Time (s) (CPU time)
Baselines: mG-N, SE-sync, RS+PS, PieADMM, RGD, SOC.
Numeric Results:
- Circular Ring (m=n=100):
  - σ_T = 0.01, σ_R = 0.01: PRADMM (Rel. Err. 0.0689, NRMSE 0.0343, Time 0.065 s) outperforms baselines in time and achieves lowest error.
  - σ_T = 0.03, σ_R = 0.05: PRADMM (Rel. Err. 0.3085, NRMSE 0.1536, Time 0.034 s) outperforms baselines in time and achieves lowest error.
  - σ_T = 0.05, σ_R = 0.1: PRADMM (Rel. Err. 0.4724, NRMSE 0.2352, Time 0.042 s) outperforms baselines in time and achieves lowest error.
- Cube Datasets (n=7, P_cube=0.3, m=697): PRADMM achieves superior reconstruction accuracy (Rel. Err. 0.1478, NRMSE 0.1707) and competitive efficiency (Time 0.213 s) compared to alternative algorithms.
- Sphere Datasets (n=2200, m=8647): PRADMM is efficient in terms of CPU time (0.387 s) and translational errors (Loss(t) 8.13e+03), outperforming mGN, SE-sync, RS+PS, and PieADMM in time.
Runtime/Training Cost:
- PRADMM consistently achieves faster computation times on small-scale circular ring datasets (Table 2).
- For large-scale PGO problems, PRADMM maintains competitive efficiency, operating marginally slower than RS+PS but substantially outperforming others in computation speed.
- Figure 3 demonstrates PRADMM's stable efficiency with increasing graph size, indicating stronger scalability for large-scale applications.

No explicit paper-reported limitations were found.

Method evidence: "In this paper, by duplicating variables and introducing equality constraints, we reformulate the problem and propose a Parallelizable Riemannian Alternating Direction Method of Multipliers (PRADMM) to solve it efficiently. Compared with the state-of-the-art methods that usually exhibit polynomial time complexity growth with graph size, PRADMM enables efficient parallel computation across vertices regardless of graph size. Crucially, all subproblems admit closed-form solutions, ensuring PRADMM maintains exceptionally stable performance." (section: Abstract)
Result evidence: "Extensive empirical validation on two synthetic datasets and multiple real-world 3D SLAM benchmarks confirms the superior computational performance of PRADMM." (section: Abstract)
Limitation evidence: No explicit paper-reported limitations were found.

Parallelizable Riemannian Alternating Direction Method of Multipliers for Non-convex Pose Graph Optimization

Summary

Problem

Key Contributions

Method

Technical Details

Results

Weaknesses / Limitations

Evidence

Additional Figures