ICP using pcl library