Planar Pose Graph Optimization: Duality, Optimal Solutions, and Verification

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 computation of a global optimal solution. In this paper, we show that Lagrangian duality allows computing a globally optimal solution under conditions that are satisfied in most robotics applications and enables to certify optimality of a given estimate. Our first contribution is to frame planar 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 zero eigenvalues of the penalized pose graph matrix. We prove that if this matrix has a single zero eigenvalue, then 1) the duality gap is zero, 2) the primal PGO problem has a unique solution (up to an arbitrary roto-translation), and 3) 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 zero eigenvalue. We also propose a suboptimal variant when the zero eigenvalues 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 zero eigenvalue; hence, our approach allows computing (or verifying) the optimal solution.