3D environment reconstruction using modified color ICP algorithm by fusion of a camera and a 3D laser range finder

In this paper, we propose a system which reconstructs the environment with both color and 3D information. We perform extrinsic calibration of a camera and a LRF (laser range finder) to fuse 3D information and color information of objects. We also formularize an equation to measure the result of the calibration. Moreover, we acquire 3D data by rotating 2D LRF with camera, and use ICP (iterative closest point) algorithm to combine data acquired in other places. We use the SIFT (scale invariant feature transform) matching for the initial estimation of ICP algorithm. It offers accurate and stable initial estimation robust to motion change compare to odometry. We also modify the ICP algorithm using color information. Computation time of ICP algorithm can be reduced by using color information.