KDtree nanoflann integration
made changes