PaperlayerView exact revision on arXiv

Source-linked AI summary

Pose Graph Optimization in the Complex Domain: Lagrangian Duality, Conditions For Zero Duality Gap, and Optimal Solutions

Giuseppe Calafiore, Luca Carlone, Frank Dellaert

arXiv:1505.03437v1cs.RO

TL;DR

PGO is nonconvex, and existing iterative techniques do not guarantee a global solution. The paper analyzes PGO through complex-domain Lagrangian duality and develops algorithms around the resulting penalized pose graph matrix. When that matrix has a single zero eigenvalue, the duality gap is zero and the primal solution is unique; the authors identify cases without that property as a limitation to guaranteed optimality.

  • Problem

    PGO is nonconvex, and iterative techniques provide only local-convergence guarantees; global optimality and uniqueness are not assured for general instances.

    Source: §1 · p. 3

  • Method

    The paper formulates PGO and its Lagrangian dual in the complex domain, then uses the penalized pose graph matrix to derive an optimality condition and algorithm.

    Source: §1 · p. 6

  • Results

    With a single zero eigenvalue in the penalized pose graph matrix, the duality gap is zero, the primal solution is unique up to an arbitrary roto-translation, and it is recoverable by scaling the corresponding eigenvector.

    Source: §1 · p. 6

  • Takeaways & Limitations

    The authors say the SZEP case is most relevant in practice because the vast majority of robotics problems fall into it.

    Source: §5.3 · p. 28

    When the penalized pose graph matrix has multiple zero eigenvalues, the authors cannot guarantee an optimal solution; their null-space approach may be suboptimal.

    Source: §5.2 · p. 27

Abstract

from arXiv · show

Pose Graph Optimization (PGO) is the problem of estimating a set of poses from pairwise relative measurements. PGO is a nonconvex problem, and currently no known technique can guarantee the computation of an optimal solution. In this paper, we show that Lagrangian duality allows computing a globally optimal solution, under certain conditions that are satisfied in many practical cases. Our first contribution is to frame the PGO problem in the complex domain. This makes analysis easier and allows drawing connections with the recent literature on unit gain graphs. Exploiting this connection we prove non-trival results about the spectrum of the matrix underlying the problem. The second contribution is to formulate and analyze the dual problem in the complex domain. Our analysis shows that the duality gap is connected to the number of eigenvalues of the penalized pose graph matrix, which arises from the solution of the dual. We prove that if this matrix has a single eigenvalue in zero, then (i) the duality gap is zero, (ii) the primal PGO problem has a unique solution, and (iii) the primal solution can be computed by scaling an eigenvector of the penalized pose graph matrix. The third contribution is algorithmic: we exploit the dual problem and propose an algorithm that computes a guaranteed optimal solution for PGO when the penalized pose graph matrix satisfies the Single Zero Eigenvalue Property (SZEP). We also propose a variant that deals with the case in which the SZEP is not satisfied. The fourth contribution is a numerical analysis. Empirical evidence shows that in the vast majority of cases (100% of the tests under noise regimes of practical robotics applications) the penalized pose graph matrix does satisfy the SZEP, hence our approach allows computing the global optimal solution. Finally, we report simple counterexamples in which the duality gap is nonzero, and discuss open problems.

1 Introduction

PGO estimates robot poses from relative measurements, but its nonconvexity leaves existing iterative methods without guarantees of global optimality or uniqueness. The paper uses complex-domain Lagrangian duality to characterize when global optimality is guaranteed, develops algorithms for both qualifying and nonqualifying cases, and reports practical-noise tests and counterexamples.

1 Introduction

  • Robot pose estimates support high-level tasks and placing sensor footprints in a common frame for mapping, navigation, and path planning.The measurements include relative motion between consecutive poses and loop closures linking nonconsecutive poses.Source: §1 · pp. 2–3
  • Iterative PGO solvers can converge only locally and may miss the global minimum at relatively small noise, while general-instance uniqueness is unknown.The gap motivates methods that can certify global optimality.Source: §1 · pp. 3–4
  • The complex formulation connects PGO to unit gain graphs and yields results on the spectrum of the pose graph matrix.The paper also formulates the Lagrangian dual in the complex domain and presents an SDP relaxation as the dual of the dual problem.Source: §1 · p. 5
  • When the penalized pose graph matrix has a single zero eigenvalue, the duality gap is zero, the primal solution is unique up to an arbitrary roto-translation, and it can be recovered by scaling the corresponding eigenvector.The paper connects the duality gap to the number of zero eigenvalues of this matrix.Source: §1 · p. 6
  • The proposed algorithm guarantees an optimal PGO solution when the penalized pose graph matrix satisfies the Single Zero Eigenvalue Property (SZEP); a variant handles cases without SZEP but may be suboptimal.The variant is reported to perform well in practice and outperform related approaches.Source: §1 · p. 6
  • In Monte Carlo tests with rotation noise below 0.3 rad and translation noise below 0.5 m, the penalized pose graph matrix always satisfied SZEP; larger noise produced cases with multiple zero eigenvalues and nonzero duality gap.The authors report these practical-noise tests as enabling optimal solutions and include counterexamples with nonzero gap.Source: §1 · p. 6

2 Notation and preliminary concepts

The preliminary section establishes notation and graph concepts, then introduces scalar multiples of planar rotations as the bridge to complex-variable representations used later.

2.3 The set αSO(2)

  • The paper uses αSO(2) for scalar multiples of 2D rotation matrices, whose actions can be represented by complex multiplication.The set is closed under matrix multiplication and addition, and multiplication is commutative.Source: §2 · p. 6, §2.3 · pp. 7–8

2 Notation and preliminary concepts

  • The preliminary section identifies notation, graph-theoretic concepts, and αSO(2) properties as foundations for the paper's complex-variable formulation.It notes that graph terminology can be skipped by expert readers.Source: §2 · p. 6

2.1 Notation

  • The notation section defines conventions for set cardinality, real and complex numbers, identity and zero matrices, vectors of ones, and matrix entries and blocks.The paper uses 2×2 blocks, making its block-entry notation unambiguous.Source: §2.1 · p. 7

2.2 Graph terminology

  • A directed graph is specified by vertices and ordered edges; its incidence matrix records each edge with −1 at its tail and +1 at its head.The incidence matrix has one row per edge and two nonzero entries per row.Source: §2.2 · p. 7

3 Pose graph optimization in the complex domain

This section formulates planar pose graph optimization as a quadratic problem with nonconvex rotation constraints, develops real and complex matrix representations, and relates the complex structure to unit gain graph incidence matrices. It analyzes the resulting matrices’ spectra, including zero eigenvalues of the complex anchored pose graph matrix and the correspondence between the real and complex spectra.

3.1 Standard PGO

  • The quadratic PGO problem is generally hard because of nonconvex rotation constraints, while tree and balanced-graph instances attain zero optimal cost.For trees, measurements can be concatenated to construct a zero-cost solution; balanced graphs also have an optimal solution with zero cost.Source: §3.1 · pp. 11–12

3.2 Matrix formulation and anchoring

  • The real pose graph matrix W is positive semidefinite with at least two zero eigenvalues, and anchoring a node removes the translation ambiguity.Its 2 × 2 blocks are multiples of rotation matrices, with diagonal blocks nonnegative multiples of the identity; the null directions reflect invariance to global position translation.Source: §3.2 · pp. 13–14

3.3 To complex domain

  • The complex reformulation removes the real representation’s duplicated eigenvalues and expresses the anchored cost using a complex matrix and unit-modulus rotation variables.Each eigenvalue of the real matrix is repeated twice, whereas the complex representation has no such redundancy.Source: §3.3 · pp. 15–16

3.4 Analysis of the real and complex pose graph matrices

  • The complex pose graph matrix factors through the anchored incidence matrix and a complex incidence matrix that is a unit gain graph matrix.The authors use this connection to motivate characterizing zero eigenvalues of the complex matrix using unit gain graph spectral results.Source: §3.4 · pp. 17–18
  • The complex anchored pose graph matrix ˜W has a single zero eigenvalue if and only if the pose graph is balanced or is a tree.Source: §3.4 · p. 19
  • The eigenvalues of the real pose graph matrix W are those of ˜W, each repeated twice.W has 2(2n−1) eigenvalues, corresponding to the 2n−1 eigenvalues of ˜W repeated twice.Source: §3.4 · p. 19

4 Lagrangian duality in PGO

The section derives a convex Lagrangian dual and relates its optimal solutions to the primal PGO problem through the penalized pose graph matrix. It also gives rank and graph-structure conditions under which the duality gap is zero and the primal solution can be recovered.

4.1 The dual problem

  • If a dual minimizer is primal-feasible, it is primal-optimal and the duality gap is zero.The dual is a convex semidefinite program that provides a lower bound on the primal optimum.Source: §4.1 · pp. 20–21

4.2 SDP relaxation and the dual of the dual

  • A rank-one optimal solution of the SDP relaxation certifies zero relaxation gap and yields a primal optimum by spectral decomposition.The Lagrangian dual and the SDP relaxation are dual to each other and have the same optimal value.Source: §4.2 · p. 22

4.3 Analysis of the dual problem

  • The dual optimum is attained at a finite point, and its penalized pose graph matrix has at least one zero eigenvalue.The multiplicity of that eigenvalue determines the form of the SDP optimizer used in the zero-gap analysis.Source: §4.3 · pp. 23–24
  • When the penalized pose graph matrix has a simple zero eigenvalue, the duality gap is zero and the primal optimum is unique up to an arbitrary rotation.The primal solution can be recovered from the one-dimensional null space of the penalized matrix.Source: §4.3 · pp. 24–25
  • Strong duality also holds for balanced pose graphs and for pose graphs whose underlying graph is a tree.These cases attain a primal optimum of zero.Source: §4.3 · p. 26

5 Algorithms

The algorithm solves the dual problem and uses the penalized pose graph matrix to decide how to construct a primal estimate. With the SZEP it recovers a guaranteed optimum; without it, the method returns an estimate that is not guaranteed optimal.

5.1 Case 1: ˜W(λ⋆) satisfies the SZEP

  • Under the SZEP, the one-dimensional null space makes its zero-eigenvalue eigenvector a scalar multiple of the primal optimum, which can be recovered by scaling.The scaling enforces the primal unit-norm constraints.Source: §5.1 · p. 26

5.2 Case 2: ˜W(λ⋆) does not satisfy the SZEP

  • Without the SZEP, the null-space method searches for a vector in the penalized matrix’s null space that is close to the feasible set, but its estimate may be suboptimal.It represents null-space vectors as Vz and uses convex inequality constraints in the optimization.Source: §5.2 · pp. 27–28

5 Algorithms

  • The algorithm solves the dual problem, then uses the zero-eigenvalue multiplicity to choose between guaranteed optimal recovery and a potentially suboptimal estimate.It reports an optimality certificate when the SZEP holds.Source: §5 · p. 26, §5.3 · p. 28

5.3 Pseudocode and implementation details

  • In the multiple-zero case, the algorithm computes a null-space basis, solves the convex problem, then normalizes the pose entries; its optimality status remains unknown.The pseudocode sets the certificate to unknown in this branch.Source: §5.3 · pp. 28–29

6 Numerical Analysis and Discussion

Across the tested practical noise regimes, the penalized pose graph matrix often satisfies the SZEP, enabling Algorithm 1 to return globally optimal solutions; tests also expose cases where the property fails. The section compares solver performance in both regimes and shows that graph structure and translation measurements can change whether the SZEP holds.

Spectrum of ˜W

  • For non-tree graphs, the smallest eigenvalue of ˜W is zero only when the graph is balanced, and it moves away from zero as rotation or translation noise increases.The analysis used 100 Monte Carlo runs and reported the minimum observed eigenvalue.Source: §6 · p. 30, Figure 4 · p. 30

Duality gap is zero in many cases

  • 100% of tests satisfied the SZEP for rotation noise σR ≤ 0.5 rad with translation noise fixed at 0.1 m; the rate remained above 90% at σR = 1 rad.With uniformly distributed rotation noise, 69% of tests satisfied the SZEP.Source: §6 · p. 31
  • 98% of tests satisfied the SZEP at translation noise standard deviation 1 m, while uniform translation noise produced 68% successful tests.At σR = σ∆ = 1, success was 100% for Pc = 0 and for Pc > 0.4, with some failures at Pc = 0.2.Source: §6 · pp. 31–32, Figure 5 · p. 31
  • The SZEP rate increased with node count at σR = σ∆ = 1, while tests at noise levels 0.1 and 0.5 satisfied it in 100% of cases.The tested node counts were n = 10, 20, 30, 40, and 50; larger Monte Carlo sweeps were limited by SDP solver scaling.Source: §6 · p. 32

Performance of Algorithm 1

  • For SZEP-satisfying cases with high measurement noise, Algorithm 1 attained the optimum while Gauss–Newton initialized at ground truth could still fail to converge to it.Figure 6 compares the optimal objective f⋆ with fGN under different noise setups, fixing n = 10 and Pc = 0.1.Source: §6 · p. 33, Figure 6 · p. 33
  • When the SZEP failed, the null space approach outperformed Eig and SDP and had performance comparable to Gauss–Newton initialized at ground truth.After Gauss–Newton refinement, NS was less prone than Eig, SDP, and EigR to converge to a bad minimum.Source: §6 · pp. 34–35, Figure 7 · p. 34

Chain graph counterexample and discussion

  • Although a balanced chain graph would need to satisfy the SZEP, a five-node chain example fails it; removing a node can change duality properties despite preserving each loop’s accumulated errors.Source: §6 · p. 36
  • Scaling down translation measurements in the chain counterexample can restore the SZEP; the second eigenvalue is positive when the scale is ≤ 0.4.This shows the SZEP depends on translation measurements as well as the rotation subproblem.Source: §6 · pp. 36–37, Figure 9 · p. 36

7 Conclusion

The paper uses Lagrangian duality to characterize when PGO has a zero duality gap and a unique primal solution, and gives an algorithm guaranteed optimal under the SZEP. It reports that the SZEP holds at practical robotics noise levels and that its satisfaction also depends on graph and measurement factors.

7 Conclusion

  • When the penalized pose graph matrix has a single zero eigenvalue, the duality gap is zero and the primal solution is unique up to an arbitrary roto-translation.In this case, the primal solution can be computed by scaling an eigenvector of that matrix.Source: §7 · p. 37
  • The proposed algorithm returns a guaranteed optimal PGO solution when the SZEP holds and provides an empirically very good estimate when it fails.Source: §7 · p. 37
  • The paper reformulates PGO in complex variables, connects it to unit gain graphs, and derives spectral results for the pose graph matrix.Source: §7 · p. 37
  • The numerical results report that the SZEP holds at practical robotics noise levels, while its satisfaction depends on graph connectivity, pose count, and measurement noise.Source: §7 · p. 37

8 Appendix

The appendix proves zero-cost results by constructing poses that satisfy measurements along a tree. For balanced graphs, the construction also satisfies measurements on the remaining edges because measurements compose consistently around cycles.

8.1 Proof of Proposition 1: Zero Cost in Trees

  • For a tree, fixing a root pose and propagating poses to successive neighbors constructs an estimate that annihilates the objective.Graph connectivity ensures the propagation reaches all nodes.Source: §8.1 · pp. 37–39

8.2 Proof of Proposition 2: Zero Cost in Balanced Graphs

  • For a connected balanced graph, a zero-cost solution on a spanning tree also satisfies the remaining chord measurements because measurements compose consistently around every cycle.Source: §8.2 · pp. 39–40

8.3 Proof of Proposition 4: properties of W

The proof shows that the pose graph matrix has at least two zero eigenvalues and has a structured arrangement of 2 × 2 blocks. Its diagonal blocks are nonnegative scalar multiples of the identity.

8.3 Proof of Proposition 4: properties of W

  • The pose graph matrix W has at least two zero eigenvalues because a connected graph Laplacian's constant-vector null mode yields a rank-two matrix in the nullspace of W.Source: §8.3 · p. 41
  • W is composed of 2 × 2 blocks in αSO(2), with diagonal blocks that are nonnegative multiples of I2.The proof establishes the block structure by examining its constituent matrices.Source: §8.3 · pp. 41–42

8.4 Proof of Proposition 5: Cost in the Complex Domain

The proof relates the real and complex formulations by representing vector dot products and matrix actions using complex numbers, then rewriting the real quadratic cost in complex form.

8.4 Proof of Proposition 5: Cost in the Complex Domain

  • The real formulation's vector dot products and actions of matrices in αSO(2) can be represented using complex numbers.Source: §8.4 · p. 42
  • Using these representations, the real quadratic cost x⊤Wx is rewritten with complex variables and entries of the complex pose graph matrix.Source: §8.4 · pp. 42–43

8.5 Proof of Proposition 6: Zero Eigenvalues in ˜W

For a connected graph, the number of zero eigenvalues of the complex pose graph matrix is determined by whether the graph is balanced. It is one for balanced graphs or trees, and zero otherwise.

8.5 Proof of Proposition 6: Zero Eigenvalues in ˜W

  • The proof counts zero eigenvalues using the dimension and rank of the complex pose graph matrix.It invokes the full rank of the anchored incidence matrix of a connected graph and the matrix factorization.Source: §8.5 · p. 43
  • N0 equals the number of balanced connected components; under the connected-graph assumption, it is one for balanced graphs or trees and zero otherwise.The proof uses the unit-gain-graph result for the complex incidence matrix.Source: §8.5 · pp. 43–44

8.6 Proof of Proposition 7: Spectrum of Complex and Real Pose Graph Matrices

The proof establishes that the complex and real anchored pose graph matrices have the same eigenvalues, with each eigenvalue occurring twice in the real matrix. It relates their eigenvectors through pairs of orthogonal vectors.

8.6 Proof of Proposition 7: Spectrum of Complex and Real Pose Graph Matrices

  • Each eigenvalue of the complex anchored pose graph matrix is also an eigenvalue of the real anchored pose graph matrix.The proof constructs a real eigenvector from an eigenvector of the complex matrix.Source: §8.6 · p. 44
  • A second eigenvector for the real matrix is constructed from the same complex eigenvector, and the two real eigenvectors are orthogonal and independent.The proof obtains the second vector by multiplying the eigenvector relation by e^(jπ/2).Source: §8.6 · pp. 44–45
  • Because the real matrix has twice as many eigenvalues as the complex matrix, each complex-matrix eigenvalue corresponds to exactly two real-matrix eigenvalues.The proof shows the corresponding real eigenvectors form an orthogonal pair.Source: §8.6 · p. 45

8.7 Proof of Theorem 1: Primal-dual Optimal Pairs

A primal-feasible vector from X(λ) is primal optimal, and its λ is dual optimal with zero duality gap. The proof combines weak duality with the fact that feasibility removes the constraint terms from the Lagrangian.

8.7 Proof of Theorem 1: Primal-dual Optimal Pairs

  • If x̃λ in X(λ) is primal feasible, it is primal optimal, λ is dual optimal, and the duality gap is zero.The conclusion follows by combining weak duality with equality of the feasible point's Lagrangian and objective value.Source: §8.7 · p. 45
  • For a primal-feasible xλ, the constraint terms in the Lagrangian disappear, so L(xλ, λ) equals f(xλ).This equality is used with the weak-duality bounds to show primal and dual optimal values coincide.Source: §8.7 · p. 45

8.8 Numerical Data For the Toy Examples in Section 6

The appendix provides numerical pose estimates for the toy examples and gives the format used for their relative measurements.

8.8 Numerical Data For the Toy Examples in Section 6

  • The toy-example pose estimates list five poses, each with three numerical components.The excerpt supplies values for x1 through x5.Source: §8.8 · p. 46
  • Each relative measurement is presented for an edge (i, j) as a translation vector Δij and angle θij.The listed format is [Δij⊤, θij].Source: §8.8 · p. 46

Related papers

Loading 1505.03437v1…