/* \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 Visual update. Create visualizations and add them to the viewer
*
*/
void update()
{
//remove existing shapes from visualizer
clearView();
//prevent the display of too many cubes
bool displayCubeLegend = displayCubes && static_cast<int> (displayCloud->points.size ()) <= MAX_DISPLAYED_CUBES;
showLegend(displayCubeLegend);
if (displayCubeLegend)
{
//show octree as cubes
showCubes(sqrt(octree.getVoxelSquaredSideLen(displayedDepth)));
if (showPointsWithCubes)
{
//add original cloud in visualizer
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(cloud, "z");
viz.addPointCloud(cloud, color_handler, "cloud");
}
}
else
{
//add current cloud in visualizer
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(displayCloud,"z");
viz.addPointCloud(displayCloud, color_handler, "cloud");
}
}
/* \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();
}
/* \brief Helper function to increase the octree display level by one
*
*/
bool IncrementLevel()
{
if (displayedDepth < static_cast<int> (octree.getTreeDepth ()))
{
displayedDepth++;
extractPointsAtLevel(displayedDepth);
return true;
}
else
return false;
}
/* \brief Helper function that draw info for the user on the viewer
*
*/
void showLegend(bool showCubes)
{
char dataDisplay[256];
sprintf(dataDisplay, "Displaying data as %s", (showCubes) ? ("CUBES") : ("POINTS"));
viz.removeShape("disp_t");
viz.addText(dataDisplay, 0, 60, 1.0, 0.0, 0.0, "disp_t");
char level[256];
sprintf(level, "Displayed depth is %d on %d", displayedDepth, octree.getTreeDepth());
viz.removeShape("level_t1");
viz.addText(level, 0, 45, 1.0, 0.0, 0.0, "level_t1");
viz.removeShape("level_t2");
sprintf(level, "Voxel size: %.4fm [%lu voxels]", sqrt(octree.getVoxelSquaredSideLen(displayedDepth)),
displayCloud->points.size());
viz.addText(level, 0, 30, 1.0, 0.0, 0.0, "level_t2");
viz.removeShape("org_t");
if (showPointsWithCubes)
viz.addText("Displaying original cloud", 0, 15, 1.0, 0.0, 0.0, "org_t");
}
/* \brief Helper function that read a pointcloud file (returns false if pbl)
* Also initialize the octree
*
*/
bool loadCloud(std::string &filename)
{
std::cout << "Loading file " << filename.c_str() << std::endl;
//read cloud
if (pcl::io::loadPCDFile(filename, *cloud))
{
std::cerr << "ERROR: Cannot open file " << filename << "! Aborting..." << std::endl;
return false;
}
//remove NaN Points
std::vector<int> nanIndexes;
pcl::removeNaNFromPointCloud(*cloud, *cloud, nanIndexes);
std::cout << "Loaded " << cloud->points.size() << " points" << std::endl;
//create octree structure
octree.setInputCloud(cloud);
//update bounding box automatically
octree.defineBoundingBox();
//add points in the tree
octree.addPointsFromInputCloud();
return true;
}