Pose Graph Optimization in the Complex Domain : Duality , Optimal Solutions , and Verification

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 a global optimal solution. In this paper, we show that Lagrangian duality allows computing a globally optimal solution, and enables to certify optimality of a given estimate. Our first contribution is to frame PGO in the complex domain. This makes analysis easier and allows drawing connections with existing literature on unit gain graphs. The second contribution is to formulate and analyze the properties of the Lagrangian 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 leverage duality to devise and algorithm that computes the optimal solution when the penalized matrix has a single eigenvalue in zero. We also propose a suboptimal variant when the eigenvalues in zero are multiple. Finally, we show that duality provides computational tools to verify if a given estimate (e.g., computed using iterative solvers) is globally optimal. We conclude the paper with an extensive 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 has a single eigenvalue in zero, hence our approach allows computing (or verifying) the optimal solution.