Exploiting Chordal Sparsity for Globally Optimal Estimation with Factor Graphs
This work provides a more robust and efficient way to achieve globally optimal state estimation for robotics applications, addressing safety concerns arising from local minima in existing solvers.
This paper introduces a method to automatically construct and solve convex semidefinite programming (SDP) relaxations for factor graphs within the GTSAM framework. By exploiting chordal sparsity through Bayes tree constructions, the authors achieve significant speedup in solver time for globally optimal state estimation problems, demonstrating favorable scaling compared to local solvers in 3D pose-graph SLAM and 2D localization.
Robust and efficient state estimation is crucial for perception, navigation, and control in robotics. State estimation problems are conveniently modeled using the factor-graph framework as enabled by modern software packages such as GTSAM or g2o. However, the standard solvers included in such frameworks are local and may converge to poor local minima, posing significant safety concerns. Conversely, techniques based on convex relaxations have been shown to provide a means of globally solving or certifying many state estimation problems. However, these relaxations 1) often require substantial effort to formulate, and 2) may incur significantly higher cost compared to efficient local solvers, as they require solving a large semidefinite program (SDP). In this work, we address both shortcomings by 1) creating a new procedure within the GTSAM framework for automatically constructing convex SDP relaxations for any factor graphs with common factor and variable types, and by 2) exploiting the Bayes tree constructions native to GTSAM to decompose the SDP problem, leading to significant speedup in solver time for chordally sparse problems. We demonstrate the favorable scaling of this structure-exploiting global estimator compared to standard local solvers for two case studies: A 3D pose-graph SLAM problem with a ring factor graph and a 2D localization problem with a chain factor graph. The software framework is available at https://github.com/borglab/gtsam.