ICP algo with pre-processing of point cloud for faster registration.