Lagrangian Duality in 3D SLAM: Verification Techniques and Optimal solutions


L. Carlone, D. Rosen, G. Calafiore, J. Leonard, F. Dellaert, "Lagrangian duality in 3D SLAM: verification techniques and optimal solutions", IROS, 2015.




AbstractState-of-the-art techniques for Simultaneous Localization and Mapping resort to iterative nonlinear optimization to compute an estimate for robot poses. These techniques often work well in practice, but they do not provide guarantees on the quality of the estimate. This paper shows that Lagrangian duality is a powerful tool to assess the quality of a given candi- date solution. Our contribution is threefold. First, we discuss a revisited SLAM formulation. We show that this formulation is probabilistically grounded and has the advantage of leading to an optimization problem with quadratic objective. The second contribution is the derivation of the Lagrange dual problem. The dual SLAM problem is a (convex) semidefinite program, and can be solved reliably and globally by off-the-shelf solvers. The third contribution is to discuss the relation between the original and the dual SLAM problem. We show that from the dual problem, one can evaluate the quality (i.e., the suboptimality gap) of a candidate SLAM solution, and eventually provide a certificate of optimality. Moreover, when the duality gap is zero, one can compute a guaranteed optimal SLAM solution from the dual problem, circumventing non-convex optimization. We present extensive (real and simulated) experiments supporting our claims and discuss practical relevance and open problems. 






Supplementary material



  • Datasets
    • The datasets used in the experimental section of this work can be downloaded here
    • For the datasets: sphere, sphere-a, garage, cubicle, and rim, we substituted the original covariances with isotropic ones.