Ejemplo n.º 1
0
TEST (PCL, KdTreeFLANN_radiusSearch)
{
  KdTreeFLANN<MyPoint> kdtree;
  kdtree.setInputCloud (cloud.makeShared ());
  MyPoint test_point(0.0f, 0.0f, 0.0f);
  double max_dist = 0.15;
  set<int> brute_force_result;
  for (unsigned int i=0; i<cloud.points.size(); ++i)
    if (euclideanDistance(cloud.points[i], test_point) < max_dist)
      brute_force_result.insert(i);
  vector<int> k_indices;
  vector<float> k_distances;
  kdtree.radiusSearch (test_point, max_dist, k_indices, k_distances, 100);
  
  //cout << k_indices.size()<<"=="<<brute_force_result.size()<<"?\n";
  
  for (size_t i = 0; i < k_indices.size (); ++i)
  {
    set<int>::iterator brute_force_result_it = brute_force_result.find (k_indices[i]);
    bool ok = brute_force_result_it != brute_force_result.end ();
    //if (!ok)  cerr << k_indices[i] << " is not correct...\n";
    //else      cerr << k_indices[i] << " is correct...\n";
    EXPECT_EQ (ok, true);
    if (ok)
      brute_force_result.erase (brute_force_result_it);
  }
  //for (set<int>::const_iterator it=brute_force_result.begin(); it!=brute_force_result.end(); ++it)
    //cerr << "FLANN missed "<<*it<<"\n";
  
  bool error = brute_force_result.size () > 0;
  //if (error)  cerr << "Missed too many neighbors!\n";
  EXPECT_EQ (error, false);

  {
    KdTreeFLANN<MyPoint> kdtree;
    kdtree.setInputCloud (cloud_big.makeShared ());
    // preallocate indices and dists arrays
    k_indices.resize(cloud_big.points.size ());
    k_distances.resize(cloud_big.points.size ());

    ScopeTime scopeTime ("FLANN radiusSearch");
    {
      for (size_t i = 0; i < cloud_big.points.size (); ++i)
        kdtree.radiusSearch (cloud_big.points[i], 0.1, k_indices, k_distances);
    }
  }
  
  {
    KdTreeFLANN<MyPoint> kdtree (false);
    kdtree.setInputCloud (cloud_big.makeShared ());
    // preallocate indices and dists arrays
    k_indices.resize(cloud_big.points.size ());
    k_distances.resize(cloud_big.points.size ());
    ScopeTime scopeTime ("FLANN radiusSearch (unsorted results)");
    {
      for (size_t i = 0; i < cloud_big.points.size (); ++i)
        kdtree.radiusSearch (cloud_big.points[i], 0.1, k_indices, k_distances);
    }
  }
}
Ejemplo n.º 2
0
TEST (PCL, KdTreeFLANN_radiusSearchEigen)
{
  KdTreeFLANN<Eigen::MatrixXf> kdtree;
  kdtree.setInputCloud (cloud_eigen.makeShared ());

  kdtree.setInputCloud (cloud_eigen.makeShared ());
  Eigen::VectorXf test_point = Eigen::Vector3f (0.0f, 0.0f, 0.0f);
  double max_dist = 0.15;
  set<int> brute_force_result;
  for (int i = 0; i < cloud_eigen.points.rows (); ++i)
    if ((cloud_eigen.points.row (i) - test_point.transpose ()).norm () < max_dist)
      brute_force_result.insert (i);
  vector<int> k_indices;
  vector<float> k_distances;
  kdtree.radiusSearch (test_point, max_dist, k_indices, k_distances);
  
  for (size_t i = 0; i < k_indices.size (); ++i)
  {
    set<int>::iterator brute_force_result_it = brute_force_result.find (k_indices[i]);
    bool ok = brute_force_result_it != brute_force_result.end ();
    EXPECT_EQ (ok, true);
    if (ok)
      brute_force_result.erase (brute_force_result_it);
  }
  
  bool error = brute_force_result.size () > 0;
  EXPECT_EQ (error, false);

  {
    KdTreeFLANN<Eigen::MatrixXf> kdtree;
    kdtree.setInputCloud (cloud_eigen.makeShared ());
    // preallocate indices and dists arrays
    k_indices.resize (cloud_eigen.points.rows ());
    k_distances.resize (cloud_eigen.points.rows ());

    ScopeTime scopeTime ("FLANN radiusSearch");
    {
      for (int i = 0; i < cloud_eigen.points.rows (); ++i)
        kdtree.radiusSearch (cloud_eigen.points.row (i), 0.1, k_indices, k_distances);
    }
  }
  
  {
    KdTreeFLANN<Eigen::MatrixXf> kdtree (false);
    kdtree.setInputCloud (cloud_eigen.makeShared ());
    // preallocate indices and dists arrays
    k_indices.resize (cloud_eigen.points.rows ());
    k_distances.resize (cloud_eigen.points.rows ());
    ScopeTime scopeTime ("FLANN radiusSearch (unsorted results)");
    {
      for (int i = 0; i < cloud_eigen.points.rows (); ++i)
        kdtree.radiusSearch (cloud_eigen.points.row (i), 0.1, k_indices, k_distances);
    }
  }
}
Ejemplo n.º 3
0
TEST (PCL, KdTreeFLANN_32_vs_64_bit)
{
  KdTreeFLANN<PointXYZ> tree;
  tree.setInputCloud (cloud_in);

  std::vector<std::vector<int> > nn_indices_vector;
  for (size_t i = 0; i < cloud_in->size (); ++i)
    if (isFinite ((*cloud_in)[i]))
    {
      std::vector<int> nn_indices;
      std::vector<float> nn_dists;
      tree.radiusSearch ((*cloud_in)[i], 0.02, nn_indices, nn_dists);

      nn_indices_vector.push_back (nn_indices);
    }



  for (size_t vec_i = 0; vec_i < nn_indices_vector.size (); ++vec_i)
  {
    char str[512];
    sprintf (str, "point_%d", int (vec_i));
    boost::optional<boost::property_tree::ptree&> tree = xml_property_tree.get_child_optional (str);
    if (!tree)
      FAIL ();

    int vec_size = tree.get ().get<int> ("size");
    EXPECT_EQ (vec_size, nn_indices_vector[vec_i].size ());

    for (size_t n_i = 0; n_i < nn_indices_vector[vec_i].size (); ++n_i)
    {
      sprintf (str, "nn_%d", int (n_i));
      int neighbor_index = tree.get ().get<int> (str);
      EXPECT_EQ (neighbor_index, nn_indices_vector[vec_i][n_i]);
    }
  }
}