KDTree nanoflann integration

Solution to #1936