点云密度计算

 1 double computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud)
 2 {
 3   double res = 0.0;
 4   int n_points = 0;
 5   int nres;
 6   std::vector<int> indices (2);
 7   std::vector<float> sqr_distances (2);
 8   pcl::search::KdTree<PointType> tree;
 9   tree.setInputCloud (cloud);
10 
11   for (size_t i = 0; i < cloud->size (); ++i)
12   {
13     if (! pcl_isfinite ((*cloud)[i].x))
14     {
15       continue;
16     }
17     //Considering the second neighbor since the first is the point itself.
18     nres = tree.nearestKSearch (i, 2, indices, sqr_distances);
19     if (nres == 2)
20     {
21       res += sqrt (sqr_distances[1]);
22       ++n_points;
23     }
24   }
25   if (n_points != 0)
26   {
27     res /= n_points;
28   }
29   return res;
30 }