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 rototranslation), 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 glob- ally 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.

Planar Pose Graph Optimization: Duality, Optimal Solutions, and Verification / Carlone, Luca; Calafiore, Giuseppe Carlo; Tommolillo, Carlo; Dellaert, Frank. - In: IEEE TRANSACTIONS ON ROBOTICS. - ISSN 1552-3098. - STAMPA. - 32:3(2016), pp. 545-565. [10.1109/TRO.2016.2544304]

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

CALAFIORE, Giuseppe Carlo;TOMMOLILLO, CARLO;
2016

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 rototranslation), 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 glob- ally 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.
File in questo prodotto:
File Dimensione Formato  
Planar pose graph optimization_Transaction on Robotics.pdf

non disponibili

Tipologia: 2a Post-print versione editoriale / Version of Record
Licenza: Non Pubblico - Accesso privato/ristretto
Dimensione 1.68 MB
Formato Adobe PDF
1.68 MB Adobe PDF   Visualizza/Apri   Richiedi una copia
Pubblicazioni consigliate

I documenti in IRIS sono protetti da copyright e tutti i diritti sono riservati, salvo diversa indicazione.

Utilizza questo identificativo per citare o creare un link a questo documento: https://hdl.handle.net/11583/2643832
 Attenzione

Attenzione! I dati visualizzati non sono stati sottoposti a validazione da parte dell'ateneo