Exploiting Chordal Sparsity for Globally Optimal Estimation with Factor Graphs

2026-05-28

AI Summary

This work addresses the challenge in factor graph–based state estimation where local optimization methods often converge to suboptimal solutions, while existing globally optimal approaches based on convex relaxation are hindered by modeling complexity and high computational cost. To overcome these limitations, the paper proposes an efficient method for globally optimal estimation that automatically constructs a semidefinite programming (SDP)–based convex relaxation within general factor graphs. The approach innovatively leverages the Bayes tree structure from the GTSAM framework together with chordal sparsity to decompose and solve the SDP problem efficiently. Experimental results on 3D pose-graph SLAM and 2D localization benchmarks demonstrate that the proposed method achieves global optimality while significantly outperforming conventional local solvers in both scalability and computational efficiency.

Abstract

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.

Problem

• state estimation
• factor graphs
• global optimization
• convex relaxation
• chordal sparsity

Innovation

• chordal sparsity
• convex relaxation
• factor graphs
• semidefinite programming
• Bayes tree