Function TEST

test/search/test_kdtree.cpp:72–123

Test for KdTree nearestKSearch */

}

/* Test for KdTree nearestKSearch */TEST (PCL, KdTree_nearestKSearch)
{
  pcl::search::KdTree<PointXYZ> kdtree;
  kdtree.setInputCloud (cloud.makeShared ());
  PointXYZ test_point (0.01f, 0.01f, 0.01f);
  unsigned int no_of_neighbors = 20;
  std::multimap<float, int> sorted_brute_force_result;
  for (std::size_t i = 0; i < cloud.size (); ++i)
  {
    float distance = euclideanDistance (cloud[i], test_point);
    sorted_brute_force_result.insert (std::make_pair (distance, static_cast<int> (i)));
  }
  float max_dist = 0.0f;
  unsigned int counter = 0;
  for (auto it = sorted_brute_force_result.begin (); it != sorted_brute_force_result.end ()
      && counter < no_of_neighbors; ++it)
  {
    max_dist = std::max (max_dist, it->first);
    ++counter;
  }

  pcl::Indices k_indices;
  k_indices.resize (no_of_neighbors);
  std::vector<float> k_distances;
  k_distances.resize (no_of_neighbors);

  kdtree.nearestKSearch (test_point, no_of_neighbors, k_indices, k_distances);

  //if (k_indices.size () != no_of_neighbors)  std::cerr << "Found "<<k_indices.size ()<<" instead of "<<no_of_neighbors<<" neighbors.\n";
  EXPECT_EQ (k_indices.size (), no_of_neighbors);

  // Check if all found neighbors have distance smaller than max_dist
  for (const auto &k_index : k_indices)
  {
    const PointXYZ& point = cloud[k_index];
    bool ok = euclideanDistance (test_point, point) <= max_dist;
    if (!ok)
    ok = (std::abs (euclideanDistance (test_point, point)) - max_dist) <= 1e-6;
    //if (!ok)  std::cerr << k_indices[i] << " is not correct...\n";
    //else      std::cerr << k_indices[i] << " is correct...\n";
    EXPECT_TRUE (ok);
  }

  ScopeTime scopeTime ("FLANN nearestKSearch");
  {
    pcl::search::KdTree<PointXYZ> kdtree;
    //kdtree.initSearchDS ();
    kdtree.setInputCloud (cloud_big.makeShared ());
    for (const auto &point : cloud_big.points)
    kdtree.nearestKSearch (point, no_of_neighbors, k_indices, k_distances);
  }
}


/* Test the templated NN search (for different query point types) */