/* \brief Extracts all the points of depth = level from the octree
*
*/
void extractPointsAtLevel(int depth)
{
displayCloud->points.clear();
pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::Iterator tree_it;
pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::Iterator tree_it_end = octree.end();
pcl::PointXYZ pt;
std::cout << "===== Extracting data at depth " << depth << "... " << std::flush;
double start = pcl::getTime ();
for (tree_it = octree.begin(depth); tree_it!=tree_it_end; ++tree_it)
{
Eigen::Vector3f voxel_min, voxel_max;
octree.getVoxelBounds(tree_it, voxel_min, voxel_max);
pt.x = (voxel_min.x() + voxel_max.x()) / 2.0f;
pt.y = (voxel_min.y() + voxel_max.y()) / 2.0f;
pt.z = (voxel_min.z() + voxel_max.z()) / 2.0f;
displayCloud->points.push_back(pt);
}
double end = pcl::getTime ();
printf("%lu pts, %.4gs. %.4gs./pt. =====\n", displayCloud->points.size (), end - start,
(end - start) / static_cast<double> (displayCloud->points.size ()));
update();
}
/* \brief Extracts all the points of depth = level from the octree
*
*/
void extractPointsAtLevel(int depth)
{
displayCloud->points.clear();
pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::Iterator tree_it(octree);
pcl::PointXYZ pt;
std::cout << "===== Extracting data at depth " << depth << "... " << std::flush;
double start = pcl::getTime ();
while (*tree_it++)
{
if (static_cast<int> (tree_it.getCurrentOctreeDepth ()) != depth)
continue;
Eigen::Vector3f voxel_min, voxel_max;
octree.getVoxelBounds(tree_it, voxel_min, voxel_max);
pt.x = (voxel_min.x() + voxel_max.x()) / 2.0f;
pt.y = (voxel_min.y() + voxel_max.y()) / 2.0f;
pt.z = (voxel_min.z() + voxel_max.z()) / 2.0f;
displayCloud->points.push_back(pt);
//we are already the desired depth, there is no reason to go deeper.
tree_it.skipChildVoxels();
}
double end = pcl::getTime ();
printf("%zu pts, %.4gs. %.4gs./pt. =====\n", displayCloud->points.size (), end - start,
(end - start) / static_cast<double> (displayCloud->points.size ()));
update();
}