FastSLAM with Stereo Vision

We consider the problem of performing simultaneous localization and mapping (SLAM) with a stereo vision sensor, where image features are matched and triangulated for use as landmarks. We explain how we obtain landmark measurements from image features, and describe them with a Gaussian noise model for use with a Rao-Blackwellized particle filter-based SLAM algorithm called FastSLAM. This algorithm uses particles to describe uncertainty in robot pose, and Gaussian distributions to describe landmark position estimates. Simulation and experimental results indicate that FastSLAM is well suited for visionbased SLAM, because of an inherent robustness to landmark mismatches, and we achieve accuracies that are comparable to other state-of-the-art systems.