Sim(3) Pose Graphs in SLAM & SfM

• Sim(3) pose graphs are a geometric representation that models poses using similarity transformations combining translation, rotation, and uniform scale.
• They utilize matrix formulations and graph-theoretic techniques, such as spanning tree counts and D-optimality, to enforce constraints in SLAM and SfM.
• Advanced methods like convex relaxations, robust estimators, and Lie algebra propagation ensure reliable optimization and uncertainty quantification in complex mapping tasks.

A Sim(3) pose graph is a combinatorial and geometric structure in which each node represents a pose parameterized by a Sim(3) transformation — that is, a similarity transformation in three dimensions comprising translation, rotation, and uniform scale. The edges represent relative measurements or constraints between these poses. This extended transformation group is central to problems in simultaneous localization and mapping (SLAM), structure-from-motion (SfM), and multi-robot localization involving scale ambiguity or where global scale changes must be estimated jointly with orientation and position.

1. Sim(3) Pose Graph Definition and Representation

In a Sim(3) pose graph, each node $i$ carries a pose consisting of a rotation $R_i \in SO(3)$, a translation $t_i \in \mathbb{R}^3$, and a uniform scale $s_i \in \mathbb{R}_+$. An edge $(i,j)$ encodes a relative measurement — either directly in Sim(3) (i.e., with a measured similarity transformation) or as a composition of separate relative pose components (translation, rotation, scale).

The general Sim(3) transformation is represented as: $$T_i = \begin{bmatrix} s_i R_i & t_i \ 0 & 1 \end{bmatrix} \in \mathrm{Sim}(3)$$ and an edge measurement encodes a constraint of the form

$T_j \approx T_i \cdot T_{ij}$

where $T_{ij}$ is the measured relative Sim(3) transformation.

The optimization problem is typically formulated as minimization over all node poses: $$\min_{T_1,\dots,T_n} \sum_{(i,j) \in \mathcal{E}} \| \operatorname{Log}(T_j^{-1} T_i T_{ij}) \|^2_{\Sigma_{ij}^{-1}}$$ where $\operatorname{Log}$ maps Sim(3) to its Lie algebra sim(3) (a 7-dimensional vector), and $\Sigma_{ij}$ is the noise covariance associated with the measurement.

2. Graph-Theoretic and D-Optimality Principles

Pose graph design and analysis fundamentally exploit graph-theoretic properties. In SLAM, D-optimality refers to minimizing the determinant of the trajectory estimation covariance, which, under mild conditions, is well-approximated by maximizing the (weighted) number of spanning trees in the measurement graph (Khosoussi et al., 2016). For Sim(3) graphs, this involves defining edge weight functions for translation ($w_p$), rotation ($w_r$), and scale ($w_s$):

$$Y_w(G) = 2\tau_{w_p}(G) + \tau_{w_r}(G) + \tau_{w_s}(G)$$

where $\tau_{w}(G) = \log t_{w}(G)$ and $t_{w}(G)$ is the weighted number of spanning trees. This monotone log-submodular objective (proven in (Khosoussi et al., 2016)) underpins approximation algorithms with polynomial-time guarantees for constructing sparse, yet well-connected, Sim(3) pose graphs.

3. Robust Optimization and Convex Relaxation

Sim(3) pose graph optimization is highly nonconvex due to manifold structure and coupling between parameters. Robust estimators — including robust unsquared $\ell_2$, $\ell_1$, and Huber loss functions — enable resilience to heavy-tailed noise and outliers in both translation, rotation, and potentially scale (Carlone et al., 2018). To address computational intractability, convex relaxations via semidefinite programming (SDP) are constructed by “lifting” the problem (encoding pose constraints into matrix variables):

• Relax rank constraints to yield SDPs,
• Use “2-stage” methods: first solve for rotations (or similarity transformations) in the lifted space, then estimate translations and scales,
• Achieve global or near-global optima without dependence on initialization, a key advantage when scale ambiguity complicates the search for good starting solutions.

These relaxations can be augmented to handle the 7-dimensional structure of Sim(3), by appropriately extending the matrix blocks and constraints in the convex program.

4. Uncertainty Quantification and Lie Algebra Propagation

For system safety and downstream decision-making, rigorous uncertainty quantification is essential. Recent frameworks characterize pose uncertainty not in coordinate charts, but directly in the Lie algebra of the transformation group, allowing for proper propagation of uncertainty through nonlinear operations (composition, inversion, relative pose) and capturing joint correlations induced by the graph structure (Mangelson et al., 2019). Given a pose $T = \exp(\xi^\wedge)\bar{T}$ with $\xi \sim \mathcal{N}(0, \Sigma)$ in sim(3), the uncertainty of composed transformations propagates via adjoint operators:

• For $T_{ik} = T_{ij} \cdot T_{jk}$,

$$\xi_{ik} = \xi_{ij} + \mathrm{Ad}_{\bar{T}_{ij}}\xi_{jk} + \text{(higher order terms)}$$

• The covariance follows: $$\Sigma_{ik} \approx \Sigma_{ij} + \mathrm{Ad}_{\bar{T}_{ij}}\Sigma_{jk}\mathrm{Ad}_{\bar{T}_{ij}}^\top + \text{(cross terms)}$$ This approach ensures that covariances respect the group structure, and is extensible to Sim(3) pose graphs with proper handling of the scale parameter.

5. Algorithmic Strategies and Practical Solvers

Several algorithmic strategies have demonstrated strong performance for Sim(3) pose graph optimization:

• Initialization via Chordal or Bayesian Methods: Robust initialization using orientation-only information or Bayesian MCMC on manifold representations is crucial, especially in high-noise or ambiguous-scale settings (Birdal et al., 2018, Nasiri et al., 2018).
• Cycle Basis Parameterization: Re-parameterizing the problem in terms of cycle space (encoding loop closure consistency rather than pose vertices) reduces dimensionality for sparse graphs and accelerates convergence. Minimum or incremental cycle bases can be efficiently computed and adapted to Sim(3) by extending the cycle constraint to closure of similarity transformations (Bai et al., 2022, Forsgren et al., 2022).
• Proximal and Riemannian ADMM: Novel nonconvex solvers, e.g., proximal linearized Riemannian ADMM (PieADMM), leverage splitting of variables (introducing auxiliary copies for unit quaternions) and closed-form projections to the manifold for efficient and parallel updates (Chen et al., 29 Apr 2024). This method benefits from the spherical geometry of quaternions and low memory footprint.
• Distributed and Consensus-based Protocols: For large-scale or multi-agent problems, distributed consensus algorithms minimize geodesic errors over Sim(3), updating each node’s state using only local neighbor information (Cristofalo et al., 2020). These are scalable, robust to communication failures, and applicable to collaborative SLAM scenarios.

6. Sparsification and Graph Synthesis for Scalability

Scalability in lifelong or large-scale SLAM applications requires systematic graph sparsification. Spectral sparsification via maximization of algebraic connectivity (second-smallest Laplacian eigenvalue) enables the retention of “important” measurements and preservation of estimation quality (Doherty et al., 2022). The methodology involves convex relaxation (e.g., via Frank–Wolfe optimization) and can be generalized to Sim(3) graphs by augmenting Laplacian blocks to incorporate scale influence. The process includes post hoc performance certification using duality gaps.

For pose graph design, submodular objectives (e.g., weighted tree connectivity) guarantee that greedy or convex-relaxed edge selection algorithms yield provably near-optimal designs even when measurement noise in scale, rotation, and translation differ (Khosoussi et al., 2016).

7. Applications: SLAM, SfM, Robotics, and Hand-Eye Calibration

Sim(3) pose graphs are fundamental in multi-robot SLAM, collaborative mapping, and global structure-from-motion, particularly under scale ambiguity (e.g., monocular vision, loop closures with scale drift). They are employed for robust hand-eye calibration in industrial robotics using SLAM-inspired optimization frameworks (Evangelista et al., 2023), for cycle-based optimization in resource-constrained platforms, and as the formal basis for large-scale simulated datasets supporting benchmarking and reproducibility (Sundin et al., 2023).

Real-world challenges addressed include cooperative vehicle localization (by fusing spatial and temporal observations), robust handling of outliers, principled uncertainty quantification, and seamless integration of lower-DOF observations via “lifting” to full Sim(3) constraints (Shen et al., 2017, Forsgren et al., 2022).

---

In summary, the Sim(3) pose graph is a mathematically rigorous and practically essential representation underpinning modern high-dimensional mapping, localization, and multi-sensor calibration problems. Recent advances combine graph theory, convex and manifold optimization, robust statistics, and distributed systems theory, providing strong guarantees for reliability, scalability, and uncertainty management in large and complex applications.