Hybrid Inference Optimization for robust pose graph estimation

In this paper we introduce a new optimization algorithm for networks of switched nonlinear objectives and apply this to the important problem of pose graph estimation for robot localization and mapping. The key insight is to replace the linear solver typically used in Gauss-Newton style methods with hybrid inference over switched discrete/continuous linear Gaussian networks. Since exact inference in these networks is known to be NP-hard, we also propose an approximate inference algorithm for the linearized hybrid networks based on message passing. We apply the new algorithm to the problem of robust pose graph estimation in the presence of incorrect loop closures and compare against three recently published approaches to the same problem. Evaluation is performed on ten sequences from two different datasets and shows that our approach performs substantially better than the state of the art.