3D map construction using heterogeneous robots

This paper presents a novel method to construct a complete 3D map that includes all surfaces (ceiling, wall, and furniture tops, etc.) in indoor environments. A team of four robots, including three ground robots and one wall-climbing robot is deployed in a tetrahedron configuration that satisfies the perspective three point (P3P) problem. P3P problem is to estimate the pose of a perspective camera on the wall-climbing robot viewing three ground robots, which will produce up to four solutions using Grunert´s algorithm while only one of them is genuine. We propose a probabilistic Bayesian algorithm that identifies the unique solution of the P3P problem using the mobility of the camera. Based on this technique, we introduce an intra-robot localization method to determine the geometric relationship among four robots. Each ground robot is equipped with a rotary laser range finder (LRF), a pan-tilt-zoom camera, and a LED cluster. The wall-climbing robot is fitted with a LRF, a perspective camera, and a motion sensor. Through the vision sensors, the robots obtain their relative poses by solving the P3P problem. Through the LRF on each robot, 4 laser point cloud maps are produced from each robot´s point of view. With the information of relative poses of the multiple robots and the calibration data of each LRF and camera pair, the 4 partial maps are fused to acquire a complete 3D map that is rich with information of all surfaces. Our approach outperforms the traditional range image fusion algorithms in terms of time complexity and is suitable for real-time implementation. Real experiments verified the effectiveness of the method.