Lagrangian Duality in Complex Pose Graph Optimization

Abstract

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 efficient computation of a global 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 nontrival results about the spectrum of the matrix underlying the problem. The second contribution is to formulate and analyze the properties of the Lagrangian dual problem in the complex domain. The dual problem is a semidefinite program (SDP). 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 SDP. We prove that if this matrix has a single eigenvalue in zero, then (1) the duality gap is zero, (2) the primal PGO problem has a unique solution, and (3) 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. This variant, while possibly suboptimal, provides a very good estimate for PGO in practice. 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.

Appendix

Proof of Proposition 1: Zero Cost in Trees

We prove Proposition 1 by inspection, providing a procedure to build an estimate that annihilates every summand in (8). The procedure is as follows:

• 1. Select a root node, say the first node (p _i , r _i ), with i = 1, and set it to the origin, i.e., p _i  = 0₂, r _i  = [1 0]^⊤ (compare with (7) for θ _i  = 0);

• 2. For each neighbor j of the root i, if j is an outgoing neighbor, set r _j  = R _ij r _i , and p _j  = p _i + D _ij r _i , otherwise set r _j  = R _ji ^⊤ r _i , and p _j  = p _i + D _ji r _j ;

• 3. Repeat point 2 for the unknown neighbors of every node that has been computed so far, and continue until all poses have been computed.

Let us now show that this procedure produces a set of poses that annihilates the objective in (8). According to the procedure, we set the first node to the origin: p ₁ = 0₂, r ₁ = [1 0]^⊤; then, before moving to the second step of the procedure, we rearrange the terms in (8): we separate the edges into two sets $\mathcal{E} = \mathcal{E}_{1} \cup \bar{\mathcal{E}}_{1}$, where $\mathcal{E}_{1}$ is the set of edges incident on node 1 (the root), and $\bar{\mathcal{E}}_{1}$ are the remaining edges. Then the cost can be written as:

$$\displaystyle\begin{array}{rcl} f(p,r)& =& \sum _{(i,j)\in \mathcal{E}_{1}}\|p_{j} - p_{i} - D_{ij}r_{i}\|_{2}^{2} +\| r_{ j} - R_{ij}r_{i}\|_{2}^{2} + \\ & & +\sum _{(i,j)\in \bar{\mathcal{E}}_{1}}\|p_{j} - p_{i} - D_{ij}r_{i}\|_{2}^{2} +\| r_{ j} - R_{ij}r_{i}\|_{2}^{2}{}\end{array}$$

(48)

We can further split the set $\mathcal{E}_{1}$ into edges that have node 1 as a tail (i.e., edges in the form (1, j)) and edges that have node 1 as head (i.e., (j, 1)):

$$\displaystyle\begin{array}{rcl} f(p,r)& =& \sum _{(1,j),j\in \mathcal{N}_{ 1}^{\text{out}}}\|p_{j} - p_{1} - D_{1j}r_{1}\|_{2}^{2} +\| r_{ j} - R_{1j}r_{1}\|_{2}^{2} + \\ & & +\sum _{ (j,1),j\in \mathcal{N}_{1}^{\text{in}}}\|p_{1} - p_{j} - D_{j1}r_{j}\|_{2}^{2} +\| r_{ 1} - R_{j1}r_{j}\|_{2}^{2} + \\ & & \qquad +\sum _{(i,j)\in \bar{\mathcal{E}}_{1}}\|p_{j} - p_{i} - D_{ij}r_{i}\|_{2}^{2} +\| r_{ j} - R_{ij}r_{i}\|_{2}^{2} {}\end{array}$$

(49)

Now, we set each node j in the first two summands as prescribed in step 2 of the procedure. By inspection one can verify that this choice annihilates the first two summands and the cost becomes:

$$\displaystyle\begin{array}{rcl} f(p,r) =\sum _{(i,j)\in \bar{\mathcal{E}}_{1}}\|p_{j} - p_{i} - D_{ij}r_{i}\|_{2}^{2} +\| r_{ j} - R_{ij}r_{i}\|_{2}^{2}& &{}\end{array}$$

(50)

Now we select a node k that has been computed at the previous step, but has some neighbor that is still unknown. As done previously, we split the set $\bar{\mathcal{E}}_{1}$ into two disjoint subsets: $\bar{\mathcal{E}}_{1} = \mathcal{E}_{k} \cup \bar{\mathcal{E}}_{k}$, where the set $\mathcal{E}_{k}$ contains the edges in $\bar{\mathcal{E}}_{1}$ that are incident on k, and $\bar{\mathcal{E}}_{k}$ contains the remaining edges:

$$\displaystyle\begin{array}{rcl} f(p,r)& =& \sum _{\{(k,j),j\in \mathcal{N}_{ k}^{\text{out}}\}\cap \bar{\mathcal{E}}_{1}}\|p_{j} - p_{k} - D_{kj}r_{k}\|_{2}^{2} +\| r_{ j} - R_{kj}r_{k}\|_{2}^{2} + \\ & & +\sum _{\{ (j,k),j\in \mathcal{N}_{k}^{\text{in}}\}\cap \bar{\mathcal{E}}_{1}}\|p_{k} - p_{j} - D_{jk}r_{j}\|_{2}^{2} +\| r_{ k} - R_{jk}r_{j}\|_{2}^{2} + \\ & & \qquad +\sum _{(i,j)\in \bar{\mathcal{E}}_{k}}\|p_{j} - p_{i} - D_{ij}r_{i}\|_{2}^{2} +\| r_{ j} - R_{ij}r_{i}\|_{2}^{2} {}\end{array}$$

(51)

Again, setting neighbors j as prescribed in step 2 of the procedure, annihilates the first two summands in (51). Repeating the same reasoning for all nodes that have been computed, but still have unknown neighbors, we can easily show that all terms in (51) become zero (the assumption of graph connectivity ensures that we can reach all nodes), proving the claim.

Proof of Proposition 2: Zero Cost in Balanced Graphs

Similarly to section “Proof of Proposition 1: Zero Cost in Trees” in this Appendix, we prove Proposition 2 by showing that in balanced graphs one can always build a solution that attains zero cost.

For the assumption of connectivity, we can find a spanning tree $\mathcal{T}$ of the graph, and split the terms in the cost function accordingly:

$$\displaystyle\begin{array}{rcl} f(p,r)& =& \sum _{(i,j)\in \mathcal{T}}\|p_{j} - p_{i} - D_{ij}r_{i}\|_{2}^{2} +\| r_{ j} - R_{ij}r_{i}\|_{2}^{2} + \\ & & +\sum _{(i,j)\in \bar{\mathcal{T}}}\|p_{j} - p_{i} - D_{ij}r_{i}\|_{2}^{2} +\| r_{ j} - R_{ij}r_{i}\|_{2}^{2}{}\end{array}$$

(52)

where $\bar{\mathcal{T}}\doteq\mathcal{E}\setminus \mathcal{T}$ are the chords of the graph w.r.t. $\mathcal{T}$.

Then, using the procedure in section “Proof of Proposition 1: Zero Cost in Trees” in this Appendix we construct a solution {r _i ^⋆, p _i ^⋆} that attains zero cost for the measurements in the spanning tree $\mathcal{T}$. Therefore, our claim only requires to demonstrate that the solution built from the spanning tree also annihilates the terms in $\bar{\mathcal{T}}$:

$$\displaystyle\begin{array}{rcl} f(p^{\star },r^{\star }) =\sum _{ (i,j)\in \bar{\mathcal{T}}}\|p_{j}^{\star } - p_{ i}^{\star } - D_{ ij}r_{i}^{\star }\|_{ 2}^{2} +\| r_{ j}^{\star } - R_{ ij}r_{i}^{\star }\|_{ 2}^{2}& &{}\end{array}$$

(53)

To prove the claim, we consider one of the chords in $\bar{\mathcal{T}}$ and we show that the cost at {r _i ^⋆, p _i ^⋆} is zero. The cost associated to a chord $(i,j) \in \bar{\mathcal{T}}$ is:

$$\displaystyle{ \|p_{j}^{\star } - p_{ i}^{\star } - D_{ ij}r_{i}^{\star }\|_{ 2}^{2} +\| r_{ j}^{\star } - R_{ ij}r_{i}^{\star }\|_{ 2}^{2} }$$

(54)

Now consider the unique path $\mathcal{P}_{ij}$ in the spanning tree $\mathcal{T}$ that connects i to j, and number the nodes along this path as i, i + 1, …, j − 1, j.

Let us start by analyzing the second summand in (54), which corresponds to the rotation measurements. According to the procedure in section “Proof of Proposition 1: Zero Cost in Trees” in this Appendix to build the solution for $\mathcal{T}$, we propagate the estimate from the root of the tree. Then it is easy to see that:

$$\displaystyle{ r_{j}^{\star } = R_{ j-1j}\cdots R_{i+1i+2}R_{ii+1}r_{i}^{\star } }$$

(55)

where R _ii+1 is the rotation associated to the edge (i, i + 1), or its transpose if the edge is in the form (i + 1, i) (i.e., it is traversed backwards along $\mathcal{P}_{ij}$). Now we notice that the assumption of balanced graph implies that the measurements compose to the identity along every cycle in the graph. Since the chord (i, j) and the path $\mathcal{P}_{ij}$ form a cycle in the graph, it holds:

$$\displaystyle{ R_{j-1j}\cdots R_{i+1i+2}R_{ii+1} = R_{ij} }$$

(56)

Substituting (56) back into (55) we get:

$$\displaystyle{ r_{j}^{\star } = R_{ ij}r_{i}^{\star } }$$

(57)

which can be easily seen to annihilate the second summand in (54).

Now we only need to demonstrate that also the first summand in (54) is zero. The procedure in section “Proof of Proposition 1: Zero Cost in Trees” in this Appendix leads to the following estimate for the position of node j:

$$\displaystyle\begin{array}{rcl} p_{j}^{\star }& =& p_{ i}^{\star } + D_{ ii+1}r_{i}^{\star } + D_{ i+1i+2}r_{i+1}^{\star } + \cdots + D_{ j-1j}r_{j-1}^{\star } \\ & =& p_{i}^{\star } + D_{ ii+1}r_{i}^{\star } + D_{ i+1i+2}R_{ii+1}r_{i}^{\star } + \cdots + D_{ j-1j}R_{j-2j-1}\cdots R_{i+1i+2}R_{ii+1}r_{i}^{\star } \\ & =& p_{i}^{\star } + \left (D_{ ii+1} + D_{i+1i+2}R_{ii+1} + \cdots + D_{j-1j}R_{j-2j-1}\cdots R_{i+1i+2}R_{ii+1}\right )r_{i}^{\star } {}\end{array}$$

(58)

The assumption of balanced graph implies that position measurements compose to zero along every cycle, hence:

$$\displaystyle\begin{array}{rcl} \varDelta _{ij}& =& \varDelta _{ii+1} + R_{ii+1}\varDelta _{i+1i+2} + R_{i+1i+2}R_{ii+1}\varDelta _{i+2i+3} + \cdots \\ & & +R_{j-2j-1}\cdots R_{i+1i+2}R_{ii+1}\varDelta _{j-1j} {}\end{array}$$

(59)

or equivalently:

$$\displaystyle\begin{array}{rcl} D_{ij}& =& D_{ii+1} + D_{i+1i+2}R_{ii+1} + \cdots \\ & & +D_{j-1j}R_{j-2j-1}\cdots R_{i+1i+2}R_{ii+1}{}\end{array}$$

(60)

Substituting (60) back into (58) we obtain:

$$\displaystyle{ p_{j}^{\star } = p_{ i}^{\star } + D_{ ij}r_{i}^{\star } }$$

which annihilates the first summand in (54), concluding the proof.

Proof of Proposition 4: Properties of $\mathcal{W}$

Let us prove that $\mathcal{W}$ has (at least) two eigenvalues in zero. We already observed that the top-left block of $\mathcal{W}$ is $\bar{\mathcal{L}} = \mathcal{L}\otimes I_{2}$, where $\mathcal{L}$ is the Laplacian matrix of the graph underlying the PGO problem. The Laplacian $\mathcal{L}$ of a connected graph has a single eigenvalue in zero, and the corresponding eigenvector is 1 _n (see, e.g., [18, Sects. 1.2–1.3]), i.e., $\mathcal{L}\cdot 1_{n} = 0$. Using this property, it is easy to show that the matrix N ≐[0 _n ^⊤ 1 _n ^⊤]^⊤⊗ I ₂ is in the nullspace of $\mathcal{W}$, i.e., $\mathcal{W}N = 0$. Since N has rank 2, this implies that the nullspace of $\mathcal{W}$ has at least dimension 2, which proves the first claim.

Let us now prove that the matrix $\mathcal{W}$ is composed by 2 × 2 blocks $[\mathcal{W}]_{ij}$, with $[\mathcal{W}]_{ij} \in \alpha SO(2)$, $\forall i,j = 1,\ldots,2n$, and $[\mathcal{W}]_{ii} =\alpha _{ii}I_{2}$ with α _ii  ≥ 0. We prove this by direct inspection of the blocks of $\mathcal{W}$. Given the structure of $\mathcal{W}$ in (14), the claim reduces to proving that the matrices $\bar{\mathcal{L}}$, $\bar{Q}$, and $\bar{A}^{\top }\bar{D}$ are composed by 2 × 2 blocks in α S O(2), and the diagonal blocks of $\bar{\mathcal{L}}$ and $\bar{Q}$ are multiples of the identity matrix. To this end, we start by observing that $\bar{\mathcal{L}} = \mathcal{L}\otimes I_{2}$, hence all blocks in $\bar{\mathcal{L}}$ are multiples of the 2 × 2 identity matrix, which also implies that they belong to α S O(2). Consider next the matrix $\bar{Q}\doteq\bar{D}^{\top }\bar{D} +\bar{ U}^{\top }\bar{U}$. From the definition of $\bar{D}$ it follows that $\bar{D}^{\top }\bar{D}$ is zero everywhere, except the 2 × 2 diagonal blocks:

$$\displaystyle{ [\bar{D}^{\top }\bar{D}]_{ ii} =\sum _{j\in \mathcal{N}_{ i}^{\text{out}}}\|\varDelta _{ij}\|_{2}^{2}I_{ 2},\qquad i = 1,\ldots,n. }$$

(61)

Similarly, from simple matrix manipulation we obtain the following block structure of $\bar{U}^{\top }\bar{U}$:

$$\displaystyle\begin{array}{rcl} [\bar{U}^{\top }\bar{U}]_{ ii}& =& d_{i}I_{2},\qquad i = 1,\ldots,n; \\{} [\bar{U}^{\top }\bar{U}]_{ ij}& =& -R_{ij},\qquad (i,j) \in \mathcal{E}; \\{} [\bar{U}^{\top }\bar{U}]_{ ij}& =& -R_{ji}^{\top },\qquad (j,i) \in \mathcal{E}; \\ {}[\bar{U}^{\top }\bar{U}]_{ ij}& =& 0_{2\times 2},\qquad \text{otherwise}.{}\end{array}$$

(62)

where d _i is the degree (number of neighbours) of node i. Combining (61) and (62) we get the following structure for $\bar{Q}$:

$$\displaystyle\begin{array}{rcl} [\bar{Q}]_{ii}& =& \beta _{i}I_{2},\qquad i = 1,\ldots,n; \\ {}[\bar{Q}]_{ij}& =& -R_{ij},\qquad (i,j) \in \mathcal{E}; \\ {}[\bar{Q}]_{ij}& =& -R_{ji}^{\top },\qquad (j,i) \in \mathcal{E}; \\ {}[\bar{Q}]_{ij}& =& 0_{2\times 2},\qquad \text{otherwise}.{}\end{array}$$

(63)

where we defined $\beta _{i}\doteq d_{i} +\sum _{j\in \mathcal{N}_{ i}^{\text{out}}}\|\varDelta _{ij}\|_{2}^{2}$. Clearly, $\bar{Q}$ has blocks in α S O(2) and the diagonal blocks are nonnegative multiples of I ₂.

Now, it only remains to inspect the structure of $\bar{A}^{\top }\bar{D}$. The matrix $\bar{A}^{\top }\bar{D}$ has the following structure:

$$\displaystyle\begin{array}{rcl} [\bar{A}^{\top }\bar{D}]_{ ii}& =& \sum _{j\in \mathcal{N}_{ i}^{\text{out}}}D_{ij},\qquad i = 1,\ldots,n; \\ {}[\bar{A}^{\top }\bar{D}]_{ ij}& =& -D_{ji},\qquad (j,i) \in \mathcal{E}; \\ {}[\bar{A}^{\top }\bar{D}]_{ ij}& =& 0_{2\times 2},\qquad \text{otherwise}. {}\end{array}$$

(64)

Note that $\sum _{j\in \mathcal{N}_{ i}^{\text{out}}}D_{ij}$ is the sum of matrices in α S O(2), hence it also belongs to α S O(2). Therefore, also all blocks of $\bar{A}^{\top }\bar{D}$ are in α S O(2), thus concluding the proof.

Proof of Proposition 5: Cost in the Complex Domain

Let us prove the equivalence between the complex cost and its real counterpart, as stated in Proposition 5.

We first observe that the dot product between two 2-vectors $x_{1},x_{2} \in \mathbb{R}^{2}$, can be written in terms of their complex representation $\tilde{x}_{1}\doteq x_{1}^{\vee }$, and $\tilde{x}_{2}\doteq x_{2}^{\vee }$, as follows:

$$\displaystyle{ x_{1}^{\top }x_{ 2} = \frac{\tilde{x}_{1}^{{\ast}}\tilde{x}_{2} +\tilde{ x}_{1}\tilde{x}_{2}^{{\ast}}} {2} }$$

(65)

Moreover, we know that the action of a matrix Z ∈ α S O(2) can be written as the product of complex numbers, see (18).

Combining (65) and (18) we get:

$$\displaystyle\begin{array}{rcl} x_{1}^{\top }Zx_{ 2}& \sim & \frac{\tilde{x}_{1}^{{\ast}}\;\tilde{z}\;\tilde{x}_{2} +\tilde{ x}_{1}\;\tilde{z}^{{\ast}}\;\tilde{x}_{2}^{{\ast}}} {2} {}\end{array}$$

(66)

where $\tilde{z} = Z^{\vee }$. Furthermore, when Z is multiple of the identity matrix, it easy to see that z = Z ^∨ is actually a real number, and Eq. (66) becomes:

$$\displaystyle\begin{array}{rcl} x_{1}^{\top }Zx_{ 1}& \sim & \tilde{x}_{1}^{{\ast}}\;z\;\tilde{x}_{ 1}{}\end{array}$$

(67)

With the machinery introduced so far, we are ready to rewrite the cost x ^⊤ Wx in complex form. Since W is symmetric, the product becomes:

$$\displaystyle\begin{array}{rcl} x^{\top }Wx =\sum _{ i=1}^{2n-1}\left [x_{ i}^{\top }[W]_{ ii}x_{i} +\sum _{ j=i+1}^{2n-1}2\;x_{ i}^{\top }[W]_{ ij}x_{j}\right ]& &{}\end{array}$$

(68)

Using the fact that [W] _ii is a multiple of the identity matrix, $\tilde{W}_{ii}\doteq[W]_{ii}^{\vee }\in \mathbb{R}^{}$, and using (67) we conclude $x_{i}^{\top }[W]_{ii}x_{i} =\tilde{ x}_{i}^{{\ast}}\tilde{W}_{ii}\tilde{x}_{i}$. Moreover, defining $\tilde{W}_{ij}\doteq[W]_{ij}^{\vee }$ (these will be complex numbers, in general), and using (66), Eq. (68) becomes:

$$\displaystyle\begin{array}{rcl} x^{\top }Wx& =& \sum _{ i=1}^{2n-1}\left [\tilde{x}_{ i}^{{\ast}}\tilde{W}_{ ii}\tilde{x}_{i} +\sum _{ j=i+1}^{2n-1}(\tilde{x}_{ i}^{{\ast}}\tilde{W}_{ ij}\tilde{x}_{j} +\tilde{ x}_{i}\tilde{W}_{ij}^{{\ast}}\tilde{x}_{ j}^{{\ast}})\right ] \\ & =& \sum _{i=1}^{2n-1}\left [\tilde{x}_{ i}^{{\ast}}\tilde{W}_{ ii}\tilde{x}_{i} +\sum _{j\neq i}\tilde{x}_{i}^{{\ast}}\tilde{W}_{ ij}\tilde{x}_{j}\right ] =\tilde{ x}^{{\ast}}\tilde{W}\tilde{x} {}\end{array}$$

(69)

where we completed the lower triangular part of $\tilde{W}$ as $\tilde{W}_{ji} =\tilde{ W}_{ij}^{{\ast}}$.

Proof of Proposition 6: Zero Eigenvalues in $\tilde{W}$

Let us denote with N ₀ the number of zero eigenvalues of the pose graph matrix $\tilde{W}$. N ₀ can be written in terms of the dimension of the matrix ($\tilde{W} \in \mathbb{C}^{(2n-1)\times (2n-1)}$) and the rank of the matrix:

$$\displaystyle{ N_{0} = (2n - 1) -\mbox{ rank}(\tilde{W}) }$$

(70)

Now, recalling the factorization of $\tilde{W}$ given in (25), we note that:

$$\displaystyle{ \mbox{ rank}(\tilde{W}) = \mbox{ rank}\left (\left [\begin{array}{cc} A&\tilde{D}\\ 0 & \tilde{U} \end{array} \right ]\right ) = \mbox{ rank}(A)+\mbox{ rank}(\tilde{U}) }$$

(71)

where the second relation follows from the upper triangular structure of the matrix. Now, we know from [68, Sect. 19.3] that the anchored incidence matrix A, obtained by removing a row from the the incidence matrix of a connected graph, is full rank:

$$\displaystyle{ \mbox{ rank}(A) = n - 1 }$$

(72)

Therefore:

$$\displaystyle{ N_{0} = n -\mbox{ rank}(\tilde{U}) }$$

(73)

Now, since we recognized that $\tilde{U}$ is the complex incidence matrix of a unit gain graph (Lemma 1), we can use the result of Lemma 2.3 in [61], which says that:

$$\displaystyle{ \mbox{ rank}(\tilde{U}) = n - b, }$$

(74)

where b is the number of connected components in the graph that are balanced. Since we are working on a connected graph (Assumption 1), b can be either one (balanced graph or tree), or zero otherwise. Using (73) and (74), we obtain N ₀ = b, which implies that N ₀ = 1 for balanced graphs or trees, or N ₀ = 0, otherwise.

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

Recall that any Hermitian matrix has real eigenvalues, and possibly complex eigenvectors. Let $\mu \in \mathbb{R}^{}$ be an eigenvalue of $\tilde{W}$, associated with an eigenvector $\tilde{v} \in \mathbb{C}^{2n-1}$, i.e.,

$$\displaystyle\begin{array}{rcl} \tilde{W}\tilde{v}& =& \mu \tilde{v}{}\end{array}$$

(75)

From Eq. (75) we have, for i = 1, …, 2n − 1,

$$\displaystyle\begin{array}{rcl} \sum _{j=1}^{2n-1}\tilde{W}_{ ij}\tilde{v}_{j} =\mu \tilde{ v}_{i} \Leftrightarrow \sum _{j=1}^{2n-1}[W]_{ ij}v_{j} =\mu v_{i}& &{}\end{array}$$

(76)

where v _i is such that $v_{i}^{\vee } =\tilde{ v}_{i}$. Since Eq. (76) holds for all i = 1, …, 2n − 1, it can be written in compact form as:

$$\displaystyle{ Wv =\mu v }$$

(77)

hence v is an eigenvector of the real anchored pose graph matrix W, associated with the eigenvalue μ. This proves that any eigenvalue of $\tilde{W}$ is also an eigenvalue of W.

To prove that the eigenvalue μ is actually repeated twice in W, consider now Eq. (75) and multiply both members by the complex number $\mathrm{e}^{\text{j} \frac{\pi }{ 2} }$:

$$\displaystyle\begin{array}{rcl} \tilde{W}\tilde{v}\mathrm{e}^{\text{j} \frac{\pi } { 2} }& =& \mu \tilde{v}\mathrm{e}^{\text{j} \frac{\pi } {2} }{}\end{array}$$

(78)

For i = 1, …, 2n − 1, we have:

$$\displaystyle\begin{array}{rcl} \sum _{j=1}^{2n-1}\tilde{W}_{ ij}^{{\ast}}\tilde{v}_{ j}\mathrm{e}^{\text{j} \frac{\pi } {2} } =\mu \tilde{ v}_{i}\mathrm{e}^{\text{j} \frac{\pi } { 2} } \Leftrightarrow \sum _{j=1}^{2n-1}[W]_{ij}w_{j} =\mu w_{i}& &{}\end{array}$$

(79)

where w _i is such that $w_{i}^{\vee } =\tilde{ v}_{j}\mathrm{e}^{\text{j} \frac{\pi } { 2} }$. Since Eq. (79) holds for all i = 1, …, 2n − 1, it can be written in compact form as:

$$\displaystyle{ Ww =\mu w }$$

(80)

hence also w is an eigenvector of W associated with the eigenvalue μ.

Now it only remains to demonstrate that v and w are linearly independent. One can readily check that, if $\tilde{v}_{i}$ is in the form $\tilde{v}_{i} =\eta _{i}\mathrm{e}^{\text{j}\theta _{i}}$, then

$$\displaystyle{ v_{i} =\eta _{i}\left [\begin{array}{c} \cos (\theta _{i}) \\ \sin (\theta _{i}) \end{array} \right ]. }$$

(81)

Moreover, observing that $\tilde{v}_{j}\mathrm{e}^{\text{j} \frac{\pi } { 2} } =\eta _{i}\mathrm{e}^{\text{j}(\theta _{i}+\pi /2)}$, then

$$\displaystyle{ w_{i} =\eta _{i}\left [\begin{array}{c} \cos (\theta _{i} +\pi /2) \\ \sin (\theta _{i} +\pi /2) \end{array} \right ] =\eta _{i}\left [\begin{array}{c} -\sin (\theta _{i}) \\ \cos (\theta _{i}) \end{array} \right ] }$$

(82)

From (81) and (82) is it easy to see that v ^⊤ w = 0, thus v, w are orthogonal, hence independent. To each eigenvalue μ of $\tilde{W}$ there thus correspond an identical eigenvalue of W, of geometric multiplicity at least two. Since $\tilde{W}$ has 2n − 1 eigenvalues and W has 2(2n − 1) eigenvalues, we conclude that to each eigenvalue μ of $\tilde{W}$ there correspond exactly two eigenvalues of W in μ. The previous proof also shows how the set of orthogonal eigenvectors of W is related to the set of eigenvectors of $\tilde{W}$.

Proof of Theorem 1: Primal-dual Optimal Pairs

We prove that, given $\lambda \in \mathbb{R}^{n}$, if an $\tilde{x}_{\lambda } \in \mathcal{X}(\lambda )$ is primal feasible, then $\tilde{x}_{\lambda }$ is primal optimal; moreover, λ is dual optimal, and the duality gap is zero.

By weak duality we know that for any λ:

$$\displaystyle{ \mathcal{L}(x_{\lambda },\lambda ) \leq f^{\star } }$$

(83)

However, if x _λ is primal feasible, by optimality of f ^⋆, it must also hold

$$\displaystyle{ f^{\star } \leq f(x_{\lambda }) }$$

(84)

Now we observe that for a feasible x _λ , the terms in the Lagrangian associated to the constraints disappear and $\mathcal{L}(x_{\lambda },\lambda ) = f(x_{\lambda })$. Using the latter equality and the inequalities (83) and (84) we get:

$$\displaystyle{ f^{\star } \leq f(x_{\lambda }) = \mathcal{L}(x_{\lambda },\lambda ) \leq f^{\star } }$$

(85)

which implies f(x _λ ) = f ^⋆, i.e., x _λ is primal optimal.

Further, we have that

$$\displaystyle{d^{\star } \geq \mathop{\mathrm{min}}\limits _{ x}\mathcal{L}(x,\lambda ) = \mathcal{L}(x_{\lambda },\lambda ) = f(x_{\lambda }) = f^{\star },}$$

which, combined with weak duality (d ^⋆ ≤ f ^⋆), implies that d ^⋆ = f ^⋆ and that λ attains the dual optimal value.

Numerical Data for the Toy Examples in Sect. 6

Ground truth nodes poses, written as x _i  = [p _i ^⊤, θ _i ]:

$$\displaystyle{ \begin{array}{ccccccc} x_{1} & =&[& 0.0000 & - 5.0000& 0.2451 &] \\ x_{2} & =&[& 4.7553 & - 1.5451& - 0.4496&] \\ x_{3} & =&[& 2.9389 & 4.0451 & 0.7361 &] \\ x_{4} & =&[& - 2.9389& 4.0451 & 0.3699 &] \\ x_{5} & =&[& - 4.7553& - 1.5451& - 1.7225&]\end{array} }$$

(86)

Relative measurements, for each edge (i, j), written as (i, j): [Δ _ij ^⊤, θ _ij ]:

$$\displaystyle{ \begin{array}{ccccccc} (1,2)&:&[& 4.6606 & 1.2177 & 2.8186 &] \\ (2,3)&:&[& - 4.4199& 4.8043 & 0.1519 &] \\ (3,4)&:&[& - 4.1169& 4.9322 & 0.5638 &] \\ (4,5)&:&[& - 3.6351& - 5.0908& - 0.5855&] \\ (5,1)&:&[& 3.4744 & 5.9425 & 2.5775 &]\end{array} }$$

(87)