3D FastSLAM algorithm with Kinect sensor

The FastSLAM is a fundamental algorithm for autonomous mobile robots Simultaneous Localization and Mapping (SLAM) problem. Until now, FastSLAM has been implemented in two-dimensional environment case and grid map is popular choice for constructing the map. This paper presents a new FastSLAM system to estimate the robot trajectory and reconstruct three-dimensional environments. This 3D FastSLAM algorithm uses both Rao-Blackwellized particle filtering and voxel map. Each scan of 3D range sensor provides accurate measurements likelihood using binary Bayes filter. We implemented the hardware system based on the Pioneer 2-DX platform equipped with one Microsoft Kinect sensor. The proposed method can be applied with any 3D range sensors and experimental results show that the proposed method builds a 3D OctoMap and estimates the robot´s pose accurately.