Implementing Robust M-Estimators with Certifiable Factor Graph Optimization

Zhexin Xu; Hanna Jiamei Zhang; Helena Calatrava; Pau Closas; David M. Rosen

cs.ROMar 21, 2026

Implementing Robust M-Estimators with Certifiable Factor Graph Optimization

Authors: Zhexin Xu, Hanna Jiamei Zhang, Helena Calatrava, Pau Closas, David M. Rosen

Organizations: Institute for Experiential Robotics, Northeastern University, 360 Huntington Ave, Boston, MA 02115, USA.

Abstract

Parameter estimation in robotics and computer vision faces formidable challenges from both outlier contamination and nonconvex optimization landscapes. While M-estimation addresses the problem of outliers through robust loss functions, it creates severely nonconvex problems that are difficult to solve globally. Adaptive reweighting schemes provide one particularly appealing strategy for implementing M-estimation in practice: these methods solve a sequence of simpler weighted least squares (WLS) subproblems, enabling both the use of standard least squares solvers and the recovery of higher-quality estimates than simple local search. However, adaptive reweighting still crucially relies upon solving the inner WLS problems effectively, a task that remains challenging in many robotics applications due to the intrinsic nonconvexity of many common parameter spaces (e.g. rotations and poses). In this paper, we show how one can easily implement adaptively reweighted M-estimators with certifiably correct solvers for the inner WLS subproblems using only fast local optimization over smooth manifolds. Our approach exploits recent work on certifiable factor graph optimization to provide global optimality certificates for the inner WLS subproblems while seamlessly integrating into existing factor graph-based software libraries and workflows. Experimental evaluation on pose-graph optimization and landmark SLAM tasks demonstrates that our adaptively reweighted certifiable estimation approach provides higher-quality estimates than alternative local search-based methods, while scaling tractably to realistic problem sizes.

Figures & tables

Fig. 1 : Examples of solutions obtained with GNC-Local (left) and the proposed Certi-GNC framework (right), in the presence of 30 % outlier loop closures (shown in grey) for a) PGO and b) landmark SLAM.

w_{i}\leftarrow\arg\min_{w_{i}\in[0,1]}\,\Phi_{\rho_{\mu}}(w_{i})+w_{i}r_{i}^{2}(\bm{x})

Algorithm 1 GNC for Robust M-estimation [ 10 ]

Fig. 2 : Overview of our Certi-GNC framework: Certi-FGO [ 11 ] provides 1) certifiable global inner solves (i.e. non-linear weighted least squares (WLS) problem) within a graduated non-convexity (GNC) framework. The control parameter $\mu$ defines convex surrogates $\rho_{\mu}(r_{i})$ of the target non-convex robust loss, yielding a sequence of WLS problems with 2) weights $w_{i}\in[0,1]$ updated via. closed form solve from residuals $r_{i}$ . 3) A truncated least squares (TLS) robust loss function is used here: $\mu$ is increased to gradually recover non-convexity, with $\mu\!\to\!\infty$ recovering original truncation.

Fig. 3 : Illustration of the Certi-FGO framework showing the transformation from constrained to unconstrained optimization leveraging underlying sparse factor graph structure. The BM variable

\mathbf{Y}

is partitioned into

K

block rows

\mathbf{Y}_{i}

corresponding to variables

\bm{x}_{i}

in the factor graph. The preserved block structure is apparent in this BM form of the factor graph MLE ( 9 ), in that each constraint block

(\mathbf{A}_{\ell})_{i,i}

acts locally on a single block

\mathbf{Y}_{i}

, defining individual submanifolds

\mathcal{M}_{i}^{(p)}

. Sparse separable structure is also present in the objective, though not explicitly visualized. The constrained optimization problem (Eq. ( 9 ), left) is reformulated as unconstrained Riemannian optimization over the product manifold

\mathcal{M}^{(p)}=\mathcal{M}_{1}^{(p)}\times\cdots\times\mathcal{M}_{K}^{(p)}

(Eq. ( 11 ), right), enabling efficient solution using standard manifold optimization techniques.

Fig. 4 : Performance of our Certi-GNC framework with random initialization compared with the non-certifiable baseline GNC-Local given a good initialization from odometry and a random one. Columns (left to right): RMSE-ATE (translation), RMSE-ATE (rotation), and solution time. Rows (top to bottom): a) PGO problems from Intel dataset and b) landmark SLAM from Trees dataset [ 25 ] .

Fig. 5 : A single Monte Carlo trial on the Intel PGO dataset with outlier rates of $0$ %, $10$ %, $20$ %, and $30$ %, showing (top) the Riemannian-staircase termination rank (a proxy for computational effort) and (bottom) the stagewise optimality gap of the Certi-FGO solve at each GNC iteration. In the $0$ % outlier case, GNC weights converge in the first iteration and the optimality gap is $10^{-12}$ as per Remark 1 , the terminal level is visualized with a dashed line. All trials start at level 2 with random initialization; the trials with outlier terminated at GNC iteration 13.

Robust Estimation

Robust estimation infers reliable parameters and states despite outliers, heavy-tailed noise, data contamination, or model uncertainty.

See all 83 papers

Mar 1, 2026

Certifiable Factor Graph Optimization

We show that the factor graph and certifiable estimation paradigms, which have thus far been treated as essentially independent in the literature, can be naturally synthesized into a unified framework for certifiable factor graph optimization that combines the ease of use of the former with the strong performance guarantees of the latter. The key insight enabling our synthesis is that the core mathematical constructions used to develop certifiable estimators (Shor's relaxation and Burer-Monteiro factorization) inherit a factor graph structure from the original problem: applying these transformations to a QCQP-representable estimation task with an associated factor graph model yields a lifted problem with identical factor graph connectivity whose constituent variables and factors are simple one-to-one algebraic transformations (lifts) of those appearing in the original QCQP's factor graph. This correspondence enables the Riemannian Staircase methodology for certifiable estimation to be easily instantiated and deployed using the same mature, highly-performant factor graph libraries and workflows already ubiquitously employed throughout robotics and computer vision. Experimental evaluation on a variety of pose graph optimization, landmark SLAM, and range-aided SLAM benchmarks demonstrates that our certifiable factor graph optimization methodology enables the implementation of certifiable estimators that are functionally equivalent to current state-of-the-art hand-designed, problem-specific methods, while dramatically reducing the required implementation effort from the order of months to hours.

Zhexin Xu, Nikolas R. Sanderson, Hanna Jiamei Zhang +1

Robust Autonomy Lab, Institute for Experiential Robotics, Northeastern University, 360 Huntington Ave, Boston, MA 02115, USA.

Factor Graph Optimization

Ask PDF

May 28, 2026

Exploiting Chordal Sparsity for Globally Optimal Estimation with Factor Graphs

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.

Avinash Subramanian, Connor Holmes, Timothy D. Barfoot +2

College of Computing, Georgia Institute of Technology, Atlanta, GA, USA · Robotics Institute, University of Toronto, Toronto, ON, Canada · Department of Mechanical Engineering, Carnegie Mellon University, Pittsburgh, PA, USA

Semidefinite Programming Factor Graph Optimization Constrained Optimization

Ask PDF

May 9, 2026

Smoothing Out the Edges: Continuous-Time Estimation with Gaussian Process Motion Priors on Factor Graphs

Continuous-time state estimation is gaining in popularity due to its abilities to provide smooth solutions, handle asynchronous sensors, and interpolate between data points. While there are two main paradigms, parametric (e.g., temporal basis functions, splines) and nonparametric (Gaussian processes), the latter has seen less adoption despite its technical advantages and relative ease of implementation. In this article, we seek to rectify this situation by providing a new simplified explanation of GP continuous-time estimation rooted in the language of factor graphs, which have become the de facto estimation paradigm in much of robotics. To simplify onboarding, we also provide three working examples implemented in the popular GTSAM estimation framework.

Connor Holmes, Sven Lilge, Zi Cong Guo +2

University of Toronto · Georgia Institute of Technology

Gaussian Processes Robot State Estimation

Ask PDF

Abstract

Robust Estimation

Explore similar work

Certifiable Factor Graph Optimization

Exploiting Chordal Sparsity for Globally Optimal Estimation with Factor Graphs

Smoothing Out the Edges: Continuous-Time Estimation with Gaussian Process Motion Priors on Factor Graphs