Cluster-based Penalty Scaling for Robust Pose Graph Optimization

Abstract

Robust pose graph optimization is essential for reliable pose estimation in Simultaneous Localization and Mapping (SLAM) system. Due to the nature of loop closures, even one spurious measurement could trick the SLAM estimator and severely distort the mapping results. Existing methods to avoid this problem mostly focus on ensuring local measurement consistency by evaluating measurements independently, often requiring parameters that are difficult to tune. This letter proposes a cluster-based penalty scaling (CPS) method to ensure both the local and global consistency by first evaluating the edge quality locally, and then integrating this information into the optimization formulation.