An Efficient Global Optimality Certificate for Landmark-Based SLAM

TL;DR

This paper introduces a novel, efficient method to certify global optimality in landmark-based SLAM problems, extending existing pose-graph techniques and integrating with current solvers for improved accuracy and robustness.

Contribution

It develops a graph-theoretic approach to provide a sufficient condition for global optimality in landmark-based SLAM, with a computationally efficient certificate construction.

Findings

The certificate can be computed with linear complexity in the number of landmarks.

The method successfully certifies global optimality on simulated and real-world SLAM datasets.

Integration with SE-Sync enhances the efficiency and reliability of solving landmark-based SLAM.

Abstract

Modern state estimation is often formulated as an optimization problem and solved using efficient local search methods. These methods at best guarantee convergence to local minima, but, in some cases, global optimality can also be certified. Although such global optimality certificates have been well established for 3D \textit{pose-graph optimization}, the details have yet to be worked out for the 3D landmark-based SLAM problem, in which estimated states include both robot poses and map landmarks. In this paper, we address this gap by using a graph-theoretic approach to cast the subproblems of landmark-based SLAM into a form that yields a sufficient condition for global optimality. Efficient methods of computing the optimality certificates for these subproblems exist, but first require the construction of a large data matrix. We show that this matrix can be constructed with complexity that remains linear in the number of landmarks and does not exceed the state-of-the-art computational complexity of one local solver iteration. We demonstrate the efficacy of the certificate on simulated and real-world landmark-based SLAM problems. We also integrate our method into the state-of-the-art SE-Sync pipeline to efficiently solve landmark-based SLAM problems to global optimality. Finally, we study the robustness of the global optimality certificate to measurement noise, taking into consideration the effect of the underlying measurement graph.