C++ KdTreeFLANN类代码示例

int main (int argc, char** argv)
{
	int nbs=1;
	float rad = 10;
	vector<int> indices(nbs);//temp vector to hold indices of the nearest neighbours found
	vector<float> sqr_distances(nbs);//temp vector to hold the distances to the nearest neighbours
	int total_num_found=0;
	int total_sociable=0;

	PointCloud<PointT>::Ptr scan1(new PointCloud<PointT>);
	PointCloud<PointT>::Ptr scan2(new PointCloud<PointT>);
	io::loadPCDFile<PointT>(argv[1], *scan1);
	io::loadPCDFile<PointT>(argv[2], *scan2);

	KdTreeFLANN<PointT> kdtree;
	kdtree.setInputCloud(scan2);
	float distance=0;
	int num_sociable=0;

	for(int j=0; j<scan1->points.size(); j++)
	{
		int num_found = kdtree.nearestKSearch((scan1->points)[j], 1, indices, sqr_distances);
		if(num_found > 0)
		{
			num_sociable++;
			distance += sqr_distances[0];
		}
	}
	float rmsdistance = sqrt((float)distance/num_sociable);
	cout << "RMS distance: " << rmsdistance << endl;
}

double
computeRmsE (const PointCloud<PointType>::ConstPtr &model, const PointCloud<PointType>::ConstPtr &scene, const Eigen::Matrix4f &rototranslation)
{
  PointCloud<PointType> transformed_model;
  transformPointCloud (*model, transformed_model, rototranslation);

  KdTreeFLANN<PointType> tree;
  tree.setInputCloud (scene);

  double sqr_norm_sum = 0;
  int found_points = 0;

  vector<int> neigh_indices (1);
  vector<float> neigh_sqr_dists (1);
  for (size_t i = 0; i < transformed_model.size (); ++i)
  {

    int found_neighs = tree.nearestKSearch (transformed_model.at (i), 1, neigh_indices, neigh_sqr_dists);
    if(found_neighs == 1)
    {
      ++found_points;
      sqr_norm_sum += static_cast<double> (neigh_sqr_dists[0]);
    }
  }

  if (found_points > 0)
    return sqrt (sqr_norm_sum / double (transformed_model.size ()));

  return numeric_limits<double>::max ();
}

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);
    }
  }
}

bool isInStableArea(std::vector<XYZNormalCloud::Ptr> &clouds,
                    std::vector< KdTreeFLANN<PointNormalT>::Ptr > &trees,
                    PointNormalT searchPoint,
                    std::vector<int> neighborhood_scale)
{


    if(!(clouds.size() == trees.size() && trees.size() == neighborhood_scale.size())) {
        std::cout << "The sizes of the vectors does not agree." << endl;
        return false;
    }
    std::vector<float> curvature_variances;
    std::vector<float> curvature_means;

    for(int i = 0; i < clouds.size();i++) {
        //Looks for (neighborhhod_scale[i]) neighbours of pointSearch at point cloud i
        KdTreeFLANN<PointNormalT> kdtree = *(trees.at(i));
        vector<int> pointIdxNKNSearch(neighborhood_scale[i]);
        vector<float> pointNKNSquaredDistance(neighborhood_scale[i]);

        int neighbours = kdtree.nearestKSearch (searchPoint, neighborhood_scale[i], pointIdxNKNSearch, pointNKNSquaredDistance);

        if(neighbours > 0) { //If there is a vicinity....
            //warns if the nbeighbourhood is smaller than expected
            if(neighbours < neighborhood_scale[i])
                std::cout << "Less points then expected..." << std::endl;


            //copy the curvature values of each neighbour point and calculates its mean and variance..
            std::vector<float> local_curvatures(neighbours);
            for(int j = 0; j < pointIdxNKNSearch.size();j++) {
                int pointIdx = pointIdxNKNSearch.at(j);
                local_curvatures.push_back(clouds.at(i)->at(pointIdx).curvature);
            }


            float mean,var;
            calculate_statistics(local_curvatures,mean,var);
            curvature_means.push_back(mean); curvature_variances.push_back(var);
        }
    }


    float mean,var;
    calculate_statistics(curvature_variances,mean,var);

    return false;
}

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]);
    }
  }
}