iSPCG: Incremental Subgraph-Preconditioned Conjugate Gradient Method for Online SLAM with Many Loop-Closures

Abstract

We propose a novel method to solve online SLAM problems with many loop-closures on the basis of two state- of-the-art SLAM methods, iSAM and SPCG. We first use iSAM to solve a sparse sub-problem to obtain an approximate solution. When the error grows larger than a threshold or the optimal solution is requested, we use subgraph-preconditioned conjugate gradient method to solve the original problem where the subgraph preconditioner and initial estimate are provided by iSAM. Finally we use the optimal solution from SPCG to regularize iSAM in the next steps. The proposed method is consistent, efficient and can find the optimal solution. We apply this method to solve large simulated and real SLAM problems, and obtain promising results.