Prepare for icp refinement and 3D reconstruction