3D visual SLAM with a Time-of-Flight camera

Location and mapping are fundamental problems for a mobile robot to implement a series of high-level applications. Traditional solutions to Simultaneous Location and Mapping (SLAM) are probabilistic reasoning. This paper proposes an analytic solution to 3D visual SLAM with a Time-of-Flight (TOF) camera. According to the visual registration, 3D visual SLAM problem is decomposed into such steps as environment sensing, data matching, motion estimation, as well as location update and registration of new landmarks. First, TOF range camera enables the robot to capture images of distance and intensity of a scene. Scale-Invariant feature transform (SIFT) algorithms for visual feature extraction is applied to the captured intensity images. These visual features combined with the corresponding distance information give a full measurement of 3D landmarks. Then, the process of data association and match is developed through SIFT and the Iterative Closest Point (ICP) to minimize the relative and global error in SLAM process, while obtaining motion estimation. Finally, based on the visual theory of structure from motion (SFM), an analytic solution to location and mapping is presented to 3D Visual SLAM, instead of conventional probabilistic reasoning. We provide 3D visual SLAM experimental results from simulation and the indoor environment. It turns out that the proposed scheme is feasible.