Implemented closest correspondence with kdtree