Function “findNeighbors” result wrong

nanoflann: tag v1.5.0, commit f1e47f0dbae72c11b008fd0d5b1191d9ba1f29bb system: ubuntu18.04, buildchain: cmake -> make (installed by apt-get) According to the demo, I write a test:

#include <iostream>
#include <nanoflann.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>

using num_t = float;
constexpr int DIM = 3;
using matrix_t = Eigen::Matrix<num_t, DIM, Eigen::Dynamic>;
using kdtree_t = nanoflann::KDTreeEigenMatrixAdaptor<Eigen::Matrix3Xf, 3 /*fixed size*/, nanoflann::metric_L2, false>;

void test() {
  matrix_t src(DIM, 5);
  src << 0, 1, 2, 3, 4,
         1, 1, 1, 1, 0,
         0, 1, 2, 3, 3;

  Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
  //pose.rotate(Eigen::AngleAxisf(1.0f , Eigen::Vector3f::UnitZ()).matrix());
  pose.pretranslate(Eigen::Vector3f(0.1, 0.2, 0.3));

  matrix_t tgt = pose * src;
  std::cout << "src\n" << src << "\ntgt=\n" << tgt << std::endl;

  const size_t        num_results = 1;
  std::vector<size_t> ret_indexes(num_results);
  std::vector<float>  out_dists_sqr(num_results);

  nanoflann::KNNResultSet<float> resultSet(num_results);
  resultSet.init(&ret_indexes[0], &out_dists_sqr[0]);

  std::vector<float> query_pt(3);

  kdtree_t kdtree(3, std::cref(tgt), 10 /* max leaf */);

  for (size_t i = 0; i< src.cols(); ++i) {
    ret_indexes[0] = -1;
    Eigen::Vector3f src_p = src.col(i);
    query_pt[0] = src_p.x(), query_pt[1] = src_p.y(), query_pt[2] = src_p.z();
    bool r = kdtree.index_->findNeighbors(resultSet, &query_pt[0]);
    std::cout << "query succ=" << r  << " [" << src_p.transpose() << "] id="<< ret_indexes[0]  << " [" <<
               kdtree.kdtree_get_pt(ret_indexes[0], 0) << " " << kdtree.kdtree_get_pt(ret_indexes[0], 1) << " " << kdtree.kdtree_get_pt(ret_indexes[0], 2)
               << "]"<< " dis*dis=" << out_dists_sqr[0] << std::endl;
  }
}

int main(int argc, char **argv) {
  test();
}

But the output is:

src
0 1 2 3 4
1 1 1 1 0
0 1 2 3 3
tgt=
0.1 1.1 2.1 3.1 4.1
1.2 1.2 1.2 1.2 0.2
0.3 1.3 2.3 3.3 3.3
query succ=1 [0 1 0] id=0 [0.1 1.2 0.3] dis*dis=0.14
query succ=1 [1 1 1] id=1 [1.1 1.2 1.3] dis*dis=0.14
query succ=1 [2 1 2] id=2 [2.1 1.2 2.3] dis*dis=0.14
query succ=1 [3 1 3] id=18446744073709551615 [0 1.13505e-43 0] dis*dis=0.14
query succ=1 [4 0 3] id=4 [4.1 0.2 3.3] dis*dis=0.14

Obviously something is wrong about "findNeighbors".

jlblancoc / nanoflann

Function “findNeighbors” result wrong #211