Robust Pose Graph Optimization Using Two-stage Initialization and Covariance Matrix Rescaling Algorithm

The back-end module of Simultaneous Localization and Mapping (SLAM) involves solving a nonlinear Pose Graph Optimization (PGO) problem. Typically, back-end optimization algorithms for SLAM require a good initial pose, followed by gradient-based optimization techniques such as Gauss-Newton (GN). However, in environments with outliers, these algorithms often struggle with poor initial values. To address this issue, this paper proposes a robust pose optimization algorithm. First, we begin pose initialization algorithms based on a two-stage least squares method with Tukey's Biweight kernel function. Then, we use covariance matrix rescaling algorithm, which add an adaptive constraint factor to robustly adjust the weight of each measurement, conduct iterative optimization, and verify their robustness and convergence. Experimental evaluations using both synthetic and real-world datasets in 2D and 3D environments demonstrate that this robust method handles outlier loop-closures with greater effectiveness and reliability compared to state-of-art techniques.