Our research objective is Simultaneous Localization and Mapping (SLAM) in rubble environment. The map construction requires estimation of robot trajectory in 3D space. However, it is hard to estimate it by using odometry or gyro in rubble. In this paper, the authors proposed real-time SLAM based on 3D scan match. 3D camera is used for measurement of 3D shape and its texture in real-time. 3D map and robot trajectory are estimated by combining these 3D scan data. ICP algorithm is used for the matching method. The authors modified ICP algorithm as fast and robust one for real-time 3D map construction.