Modified RigidPointICP to do real ICP