double kpoAnalyzerThread::computeCloudResolution (const CloudConstPtr &cloud)
{
double res = 0.0;
int n_points = 0;
int nres;
std::vector<int> indices (2);
std::vector<float> sqr_distances (2);
pcl::search::KdTree<PointType> tree;
tree.setInputCloud (cloud);
for (size_t i = 0; i < cloud->size (); ++i)
{
if (! pcl_isfinite ((*cloud)[i].x))
{
continue;
}
//Considering the second neighbor since the first is the point itself.
nres = tree.nearestKSearch (i, 2, indices, sqr_distances);
if (nres == 2)
{
res += sqrt (sqr_distances[1]);
++n_points;
}
}
if (n_points != 0)
{
res /= n_points;
}
return res;
}
void
cloud_cb (const CloudConstPtr& cloud)
{
PCDWriter w;
sprintf (buf, "frame_%06d.pcd", i);
w.writeBinaryCompressed (buf, *cloud);
PCL_INFO ("Wrote a cloud with %zu (%ux%u) points in %s.\n",
cloud->size (), cloud->width, cloud->height, buf);
++i;
}