Toward robust linear SLAM

This paper presents a robust solution to the linear pose-graph SLAM problem based on local submap joining. This algorithm aims to converge toward correct solutions by detecting and eliminating the passive impacts from the failed loop closures. In the linear SLAM problem, the information matrix of each submap becomes non-diagonal due to nonlinear coordinate transformations. It is naive to make a least square operation between two constraints, when there isn´t enough information to make a decision whether outlier loop closures exist. We thereby apply a delayed optimization to process the observations and pass them to the next level submap. To detect the outlier loop closure, we treat each loop closure as a random variable with an additional weight computed according to Expectation Maximization. By investing the feature of information matrix, the corrupted information matrix can be recovered efficiently. Experimental results based on publicly synthetic and real-world datasets show that this robust approach can effectively deal with incorrect loop closures.