Fused 2D/3D position tracking for robust SLAM on mobile robots

This paper presents a robust 3D position tracking and map generation algorithm that combines data from a Laser Rangefinder and a RGB-D camera. Our algorithm is capable of running in real time on resource constrained robots. The novelty of this paper is the fusing of 2D-ICP from a Laser Rangefinder directly into a 3D position tracking, map building and SLAM system. Our approach takes advantage of the wide field of view and precision of laser scans to significantly improve the efficiency and accuracy of 3D alignment. We introduce a regularisation term to restrict movement in degrees of freedom that are not constrained by surface geometry. With efficiency in mind, we mirror a 2D local map based Graph SLAM approach in 3D to reliably detect loops if the robot comes near to an area it has previously visited. The mesh of each local map is warped after a graph of local maps is optimised to maintain a globally fused map. We provide experimental results to show that our algorithm is able to generate dense, globally consistent, mesh-based maps on resource constrained robots.