Exactly Sparse Delayed State Filter based robust SLAM with Stereo Vision

This paper presents a new SLAM solution based on Exactly Sparse Delayed State Filter and data association by a novel robust stereo vision image registration constrained geometrically with odometry inputs. The proposed constrained image registration algorithm is robust to high uncertainty of odometry inputs and high percentage of data association outliers and achieves fast convergence. Extended Information Filter (i.e. the dual of the Extended Kalman Filter) is used to estimate the mobile robot's pose history, because information matrix then exhibits exactly sparse structure with Markov process model which yields efficient filtering. Computationally most demanding operations - mean and covariance recovery that are needed in data association and filter motion update - can be recovered in linear time by solving system of sparse equations. Although our solution is tested on 2D stereo vision based SLAM benchmarking problem taken from Rawseeds database, it can solve general 3D SLAM problem.