Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Implementing Robust M-Estimators with Certifiable Factor Graph Optimization

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

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.

Implementing Robust M-Estimators with Certifiable Factor Graph Optimization

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.
Paper Structure (18 sections, 1 theorem, 12 equations, 5 figures, 2 algorithms)

This paper contains 18 sections, 1 theorem, 12 equations, 5 figures, 2 algorithms.

Table of Contents

  1. Introduction
  2. Review of Robust Estimation
  3. Reformulating M-estimation as Outlier Processes
  4. IRLS via Alternating Minimization
  5. GNC as a Continuation Strategy
  6. A Robust Implementation of Certifiable Factor Graph Optimization
  7. Shor's Relaxation of QCQPs
  8. Efficiently Solving the Relaxation via Low-Rank Factorization and Riemannian Staircase
  9. Certifiable Estimation for Factor Graphs
  10. QCQP and Shor's relaxation over Factor Graphs
  11. Burer-Monteiro Factorization over Factor Graphs
  12. Optimality Verification and Saddle-Escape in the Factor Graph Setting
  13. Robustifying Certifiable FGO
  14. Applications and Experiments
  15. Experimental Setup
  16. ...and 3 more sections

Key Result

Theorem 1

(Black--Rangarajan Duality black1996unification) Given a robust loss $\rho(\cdot)$, define $\phi(z) := \rho(\sqrt{z})$. If $\phi(z)$ satisfies $\lim_{z \to 0} \phi'(z) = 1$, $\lim_{z \to \infty} \phi'(z) = 0$, and $\phi"(z) < 0$, then the M-estimation problem in eq:map2mest is equivalent to where $w_i \in [0,1]$ are auxiliary weights, and $\Phi_\rho(w_i)$ is an outlier process induced by $\rho(\cd

Figures (5)

  • Figure 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.
  • Figure 2: Overview of our Certi-GNC framework: Certi-FGOcerti_fgo 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.
  • Figure 3: Illustration of the Certi-FGO framework showing the transformation from constrained to unconstrained optimization leveraging underlying sparse factor graph structure. The 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 form of the factor graph \ref{['equation:bm_fgo']}, 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. \ref{['equation:bm_fgo']}, 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. \ref{['equation: intrinsic_optimization_formulation']}, right), enabling efficient solution using standard manifold optimization techniques.
  • Figure 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 from Trees dataset kaess2012isam2.
  • Figure 5: A single Monte Carlo trial on the Intel 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 iteration. In the $0$% outlier case, weights converge in the first iteration and the optimality gap is $10^{-12}$ as per Remark \ref{['rem:solution-recovery']}, the terminal level is visualized with a dashed line. All trials start at level 2 with random initialization; the trials with outlier terminated at iteration 13.

Theorems & Definitions (5)

  • Theorem 1
  • Remark 1
  • Remark 2
  • Remark 3
  • Remark 4