Robust optimization with credibility factor for graph-based SLAM

Graph-based Simultaneous Localization and Mapping (SLAM) refers to formulate SLAM by a graph model whose nodes represent poses of the robot and whose edges represent constraints relating the poses, then solve an error minimization problem to find the configuration of the poses that best fits with the constraints. One problem of state-of-the-art SLAM algorithms is that they rely on all the poses and constraints with the same credibility and do not fully exploit the different confidence levels of different poses and constraints. This paper proposes a novel formulation that involves the credibility factor of the poses and constraints into the graph model. The proposed optimization model that updates the graphical model with switch variables and credibility factors, which removes wrong loop closures and increases the overall accuracy by keeping the poses with higher credibility factor more stable and the poses with lower credibility factor more elastic. The results of several experiments conducted on large scale synthetic and real datasets are provided to demonstrate the credibility and effectiveness of the method.