Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Non-convex Pose Graph Optimization in SLAM via Proximal Linearized Riemannian ADMM

Xin Chen, Chunfeng Cui, Deren Han, Liqun Qi

TL;DR

This work tackles non-convex pose graph optimization for SLAM by adopting an augmented unit-quaternion representation and a von Mises-Fisher noise model on rotations. It introduces PieADMM, a proximal linearized Riemannian ADMM that updates quaternion-based rotations, translations, and auxiliary variables in parallel with closed-form subproblems, achieving convergence to an $ ext{ε}$-stationary point with iteration complexity $O(1/ ext{ε}^{2})$. The approach leverages simple quaternion normalization for projection, reducing memory and computation while preserving feasibility on the quaternion manifold. Empirical results on two synthetic datasets and four 3D-SLAM benchmarks demonstrate favorable accuracy and faster convergence compared to manifold Gauss-Newton and Levenberg–Marquardt baselines. The method offers a scalable, memory-efficient framework for large-scale non-convex PGO on spherical manifolds with practical impact for SLAM systems.

Abstract

Pose graph optimization (PGO) is a well-known technique for solving the pose-based simultaneous localization and mapping (SLAM) problem. In this paper, we represent the rotation and translation by a unit quaternion and a three-dimensional vector, and propose a new PGO model based on the von Mises-Fisher distribution. The constraints derived from the unit quaternions are spherical manifolds, and the projection onto the constraints can be calculated by normalization. Then a proximal linearized Riemannian alternating direction method of multipliers (PieADMM) is developed to solve the proposed model, which not only has low memory requirements, but also can update the poses in parallel. Furthermore, we establish the iteration complexity of $O(1/ε^{2})$ of PieADMM for finding an $ε$-stationary solution of our model. The efficiency of our proposed algorithm is demonstrated by numerical experiments on two synthetic and four 3D SLAM benchmark datasets.

Non-convex Pose Graph Optimization in SLAM via Proximal Linearized Riemannian ADMM

TL;DR

This work tackles non-convex pose graph optimization for SLAM by adopting an augmented unit-quaternion representation and a von Mises-Fisher noise model on rotations. It introduces PieADMM, a proximal linearized Riemannian ADMM that updates quaternion-based rotations, translations, and auxiliary variables in parallel with closed-form subproblems, achieving convergence to an -stationary point with iteration complexity . The approach leverages simple quaternion normalization for projection, reducing memory and computation while preserving feasibility on the quaternion manifold. Empirical results on two synthetic datasets and four 3D-SLAM benchmarks demonstrate favorable accuracy and faster convergence compared to manifold Gauss-Newton and Levenberg–Marquardt baselines. The method offers a scalable, memory-efficient framework for large-scale non-convex PGO on spherical manifolds with practical impact for SLAM systems.

Abstract

Pose graph optimization (PGO) is a well-known technique for solving the pose-based simultaneous localization and mapping (SLAM) problem. In this paper, we represent the rotation and translation by a unit quaternion and a three-dimensional vector, and propose a new PGO model based on the von Mises-Fisher distribution. The constraints derived from the unit quaternions are spherical manifolds, and the projection onto the constraints can be calculated by normalization. Then a proximal linearized Riemannian alternating direction method of multipliers (PieADMM) is developed to solve the proposed model, which not only has low memory requirements, but also can update the poses in parallel. Furthermore, we establish the iteration complexity of of PieADMM for finding an -stationary solution of our model. The efficiency of our proposed algorithm is demonstrated by numerical experiments on two synthetic and four 3D SLAM benchmark datasets.
Paper Structure (17 sections, 11 theorems, 98 equations, 8 figures, 4 tables, 1 algorithm)

This paper contains 17 sections, 11 theorems, 98 equations, 8 figures, 4 tables, 1 algorithm.

Table of Contents

  1. Introduction
  2. Literature Review
  3. Contributions
  4. Notation and preliminaries
  5. Quaternion and pose
  6. Manifolds
  7. Optimization theory over manifolds
  8. The Augmented Unit Quaternion Model
  9. Proximal Linearized Riemannian ADMM
  10. Subproblems
  11. Convergence Analysis
  12. First-Order Optimality Conditions
  13. Iteration Complexity
  14. Numerical experiments
  15. Synthetic datasets
  16. ...and 2 more sections

Key Result

Lemma 2.1

kuipers1999quaternions Given a unit quaternion $\tilde{q}=[q_0,\bm{q}]=[q_0,q_1,q_2,q_3] \in \mathbb{U}$ and a vector $\bm{t} \in \mathbb{R}^{3}$. Then $[0,R\bm{t}]=\tilde{q}[0,\bm{t}]\tilde{q}^{*}$, where the rotation matrix $R \in SO(3)$ satisfies

Figures (8)

  • Figure 1: The illustration of the von Mises-Fisher distribution with $\bm{\mu}=[0,0,1]^{\top}$ and $\kappa=1,~5,~20$, respectively.
  • Figure 2: The trajectory of circular ring datasets with $n=100$, $m=100$, $\sigma_{r}=0.01$ and $\sigma_{t}=0.05$. The blue one is the real trajectory and the black one is the noisy trajectory. The other three dotted lines are the recovered trajectory by different algorithms.
  • Figure 3: Performance of the three methods versus CPU time for circular ring datasets with $n=100$, $m=100$ under different initialization techniques . The noise level of the first row is $\sigma_{r}=0.01$ and $\sigma_{t}=0.05$, and the second row is $\sigma_{r}=0.03$ and $\sigma_{t}=0.1$.
  • Figure 4: Performance of the three methods for circular ring datasets under different number of poses $n$. The relative noise level is $\sigma_{r}^{rel}=0.03$ and $\sigma_{t}^{rel}=0.1$.
  • Figure 5: The comparison of cube trajectory, where the first row is $\hat{n}=5$ and the second row is $\hat{n}=8$, respectively. The color change of the vertices indicates the direction of the trajectory.
  • ...and 3 more figures

Theorems & Definitions (23)

  • Lemma 2.1
  • Lemma 2.2
  • Lemma 2.3
  • Definition 2.1
  • Definition 2.2
  • Definition 2.3
  • Definition 2.4
  • Lemma 2.4
  • Lemma 2.5
  • Theorem 5.1
  • ...and 13 more