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

Certifiable Factor Graph Optimization
Mar 1, 2026Zhexin Xu, Nikolas R. Sanderson, Hanna Jiamei Zhang +1Factor Graph Optimization
Robust Autonomy Lab, Institute for Experiential Robotics, Northeastern University, 360 Huntington Ave, Boston, MA 02115, USA.
Exploiting Chordal Sparsity for Globally Optimal Estimation with Factor Graphs
May 28, 2026Avinash Subramanian, Connor Holmes, Timothy D. Barfoot +2Semidefinite Programming Factor Graph Optimization
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
Smoothing Out the Edges: Continuous-Time Estimation with Gaussian Process Motion Priors on Factor Graphs
May 9, 2026Connor Holmes, Sven Lilge, Zi Cong Guo +2Gaussian Processes Robot State Estimation
University of Toronto · Georgia Institute of Technology

Mar 1, 2026J/K move · Enter open · S save

Certifiable Factor Graph Optimization

Zhexin Xu, Nikolas R. Sanderson, Hanna Jiamei Zhang, David M. Rosen

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

Factor Graph Optimization

Open paper AskPDF arXiv

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.

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