diff --git a/tools/octree_viewer.cpp b/tools/octree_viewer.cpp index fe7186cd127..5b53100057b 100644 --- a/tools/octree_viewer.cpp +++ b/tools/octree_viewer.cpp @@ -42,6 +42,7 @@ #include #include +#include #include #include "boost.h" @@ -60,9 +61,16 @@ class OctreeViewer { public: OctreeViewer (std::string &filename, double resolution) : - viz ("Octree visualizator"), cloud (new pcl::PointCloud()), - displayCloud (new pcl::PointCloud()), octree (resolution), displayCubes(false), - showPointsWithCubes (false), wireframe (true) + viz ("Octree visualizator"), + cloud (new pcl::PointCloud()), + displayCloud (new pcl::PointCloud()), + cloudVoxel (new pcl::PointCloud()), + octree (resolution), + wireframe (true), + show_cubes_ (true), + show_centroids_ (false), + show_original_points_ (false), + point_size_ (1.0) { //try to load the cloud @@ -73,18 +81,24 @@ class OctreeViewer viz.registerKeyboardCallback(&OctreeViewer::keyboardEventOccurred, *this, 0); //key legends - viz.addText("Keys:", 0, 170, 0.0, 1.0, 0.0, "keys_t"); - viz.addText("a -> Increment displayed depth", 10, 155, 0.0, 1.0, 0.0, "key_a_t"); - viz.addText("z -> Decrement displayed depth", 10, 140, 0.0, 1.0, 0.0, "key_z_t"); - viz.addText("d -> Toggle Point/Cube representation", 10, 125, 0.0, 1.0, 0.0, "key_d_t"); - viz.addText("x -> Show/Hide original cloud", 10, 110, 0.0, 1.0, 0.0, "key_x_t"); - viz.addText("s/w -> Surface/Wireframe representation", 10, 95, 0.0, 1.0, 0.0, "key_sw_t"); + viz.addText ("Keys:", 0, 170, 0.0, 1.0, 0.0, "keys_t"); + viz.addText ("a -> Increment displayed depth", 10, 155, 0.0, 1.0, 0.0, "key_a_t"); + viz.addText ("z -> Decrement displayed depth", 10, 140, 0.0, 1.0, 0.0, "key_z_t"); + viz.addText ("v -> Toggle octree cubes representation", 10, 125, 0.0, 1.0, 0.0, "key_v_t"); + viz.addText ("b -> Toggle centroid points representation", 10, 110, 0.0, 1.0, 0.0, "key_b_t"); + viz.addText ("n -> Toggle original point cloud representation", 10, 95, 0.0, 1.0, 0.0, "key_n_t"); //set current level to half the maximum one displayedDepth = static_cast (floor (octree.getTreeDepth() / 2.0)); if (displayedDepth == 0) displayedDepth = 1; + // assign point cloud to octree + octree.setInputCloud (cloud); + + // add points from cloud to octree + octree.addPointsFromInputCloud (); + //show octree at default depth extractPointsAtLevel(displayedDepth); @@ -108,49 +122,68 @@ class OctreeViewer pcl::PointCloud::Ptr cloud; //displayed_cloud pcl::PointCloud::Ptr displayCloud; + // cloud which contains the voxel center + pcl::PointCloud::Ptr cloudVoxel; //octree pcl::octree::OctreePointCloudVoxelCentroid octree; //level int displayedDepth; - //bool to decide if we display points or cubes - bool displayCubes, showPointsWithCubes, wireframe; + //bool to decide what should be display + bool wireframe; + bool show_cubes_, show_centroids_, show_original_points_; + float point_size_; //======================================================== /* \brief Callback to interact with the keyboard * */ - void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *) + void keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event, void *) { - if (event.getKeySym() == "a" && event.keyDown()) + if (event.getKeySym () == "a" && event.keyDown ()) + { + IncrementLevel (); + } + else if (event.getKeySym () == "z" && event.keyDown ()) { - IncrementLevel(); + DecrementLevel (); } - else if (event.getKeySym() == "z" && event.keyDown()) + else if (event.getKeySym () == "v" && event.keyDown ()) { - DecrementLevel(); + show_cubes_ = !show_cubes_; + update (); } - else if (event.getKeySym() == "d" && event.keyDown()) + else if (event.getKeySym () == "b" && event.keyDown ()) { - displayCubes = !displayCubes; - update(); + show_centroids_ = !show_centroids_; + update (); } - else if (event.getKeySym() == "x" && event.keyDown()) + else if (event.getKeySym () == "n" && event.keyDown ()) { - showPointsWithCubes = !showPointsWithCubes; - update(); + show_original_points_ = !show_original_points_; + update (); } - else if (event.getKeySym() == "w" && event.keyDown()) + else if (event.getKeySym () == "w" && event.keyDown ()) { - if(!wireframe) - wireframe=true; - update(); + if (!wireframe) + wireframe = true; + update (); } - else if (event.getKeySym() == "s" && event.keyDown()) + else if (event.getKeySym () == "s" && event.keyDown ()) { - if(wireframe) - wireframe=false; - update(); + if (wireframe) + wireframe = false; + update (); + } + else if ((event.getKeyCode () == '-') && event.keyDown ()) + { + point_size_ = std::max(1.0f, point_size_ * (1 / 2.0f)); + update (); + } + else if ((event.getKeyCode () == '+') && event.keyDown ()) + { + point_size_ *= 2.0f; + update (); } } @@ -197,26 +230,30 @@ class OctreeViewer /* \brief Helper function that draw info for the user on the viewer * */ - void showLegend(bool showCubes) + void showLegend () { 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"); + sprintf (dataDisplay, "Displaying octree cubes: %s", (show_cubes_) ? ("True") : ("False")); + viz.removeShape ("disp_octree_cubes"); + viz.addText (dataDisplay, 0, 75, 1.0, 0.0, 0.0, "disp_octree_cubes"); + + sprintf (dataDisplay, "Displaying centroids voxel: %s", (show_centroids_) ? ("True") : ("False")); + viz.removeShape ("disp_centroids_voxel"); + viz.addText (dataDisplay, 0, 60, 1.0, 0.0, 0.0, "disp_centroids_voxel"); + + sprintf (dataDisplay, "Displaying original point cloud: %s", (show_original_points_) ? ("True") : ("False")); + viz.removeShape ("disp_original_points"); + viz.addText (dataDisplay, 0, 45, 1.0, 0.0, 0.0, "disp_original_points"); 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"); + sprintf (level, "Displayed depth is %d on %d", displayedDepth, octree.getTreeDepth()); + viz.removeShape ("level_t1"); + viz.addText (level, 0, 30, 1.0, 0.0, 0.0, "level_t1"); + + viz.removeShape ("level_t2"); + sprintf (level, "Voxel size: %.4fm [%lu voxels]", std::sqrt (octree.getVoxelSquaredSideLen (displayedDepth)), + cloudVoxel->points.size ()); + viz.addText (level, 0, 15, 1.0, 0.0, 0.0, "level_t2"); } /* \brief Visual update. Create visualizations and add them to the viewer @@ -225,29 +262,30 @@ class OctreeViewer void update() { //remove existing shapes from visualizer - clearView(); + clearView (); - //prevent the display of too many cubes - bool displayCubeLegend = displayCubes && static_cast (displayCloud->points.size ()) <= MAX_DISPLAYED_CUBES; + showLegend (); - showLegend(displayCubeLegend); - - if (displayCubeLegend) + if (show_cubes_) { //show octree as cubes - showCubes(sqrt(octree.getVoxelSquaredSideLen(displayedDepth))); - if (showPointsWithCubes) - { - //add original cloud in visualizer - pcl::visualization::PointCloudColorHandlerGenericField color_handler(cloud, "z"); - viz.addPointCloud(cloud, color_handler, "cloud"); - } + showCubes (std::sqrt (octree.getVoxelSquaredSideLen (displayedDepth))); } - else + + if (show_centroids_) { - //add current cloud in visualizer - pcl::visualization::PointCloudColorHandlerGenericField color_handler(displayCloud,"z"); - viz.addPointCloud(displayCloud, color_handler, "cloud"); + //show centroid points + pcl::visualization::PointCloudColorHandlerGenericField color_handler (cloudVoxel, "x"); + viz.addPointCloud (cloudVoxel, color_handler, "cloud_centroid"); + viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size_, "cloud_centroid"); + } + + if (show_original_points_) + { + //show origin point cloud + pcl::visualization::PointCloudColorHandlerGenericField color_handler (cloud, "z"); + viz.addPointCloud (cloud, color_handler, "cloud"); + viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size_, "cloud"); } } @@ -257,11 +295,12 @@ class OctreeViewer void clearView() { //remove cubes if any - vtkRenderer *renderer = viz.getRenderWindow()->GetRenderers()->GetFirstRenderer(); - while (renderer->GetActors()->GetNumberOfItems() > 0) - renderer->RemoveActor(renderer->GetActors()->GetLastActor()); + vtkRenderer *renderer = viz.getRenderWindow ()->GetRenderers ()->GetFirstRenderer (); + while (renderer->GetActors ()->GetNumberOfItems () > 0) + renderer->RemoveActor (renderer->GetActors ()->GetLastActor ()); //remove point clouds if any - viz.removePointCloud("cloud"); + viz.removePointCloud ("cloud"); + viz.removePointCloud ("cloud_centroid"); } @@ -274,11 +313,11 @@ class OctreeViewer // Create every cubes to be displayed double s = voxelSideLen / 2.0; - for (size_t i = 0; i < displayCloud->points.size (); i++) + for (size_t i = 0; i < cloudVoxel->points.size (); i++) { - double x = displayCloud->points[i].x; - double y = displayCloud->points[i].y; - double z = displayCloud->points[i].z; + double x = cloudVoxel->points[i].x; + double y = cloudVoxel->points[i].y; + double z = cloudVoxel->points[i].z; vtkSmartPointer wk_cubeSource = vtkSmartPointer::New (); @@ -309,12 +348,13 @@ class OctreeViewer multiActor->GetProperty ()->SetColor (1.0, 1.0, 1.0); multiActor->GetProperty ()->SetAmbient (1.0); - multiActor->GetProperty ()->SetLineWidth (2); + multiActor->GetProperty ()->SetLineWidth (1); + multiActor->GetProperty ()->EdgeVisibilityOn (); + multiActor->GetProperty ()->SetOpacity (1.0); + if (wireframe) { multiActor->GetProperty ()->SetRepresentationToWireframe (); - multiActor->GetProperty ()->EdgeVisibilityOn (); - multiActor->GetProperty ()->SetOpacity (1.0); } else { @@ -334,26 +374,56 @@ class OctreeViewer void extractPointsAtLevel(int depth) { displayCloud->points.clear(); + cloudVoxel->points.clear(); pcl::octree::OctreePointCloudVoxelCentroid::Iterator tree_it; pcl::octree::OctreePointCloudVoxelCentroid::Iterator tree_it_end = octree.end(); - pcl::PointXYZ pt; + pcl::PointXYZ pt_voxel_center; + pcl::PointXYZ pt_centroid; 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) { + // If the iterator is not at the right depth, continue if (tree_it.getCurrentOctreeDepth () != depth) continue; + // Compute the point at the center of the voxel which represents the current OctreeNode Eigen::Vector3f voxel_min, voxel_max; - octree.getVoxelBounds(tree_it, voxel_min, voxel_max); + octree.getVoxelBounds (tree_it, voxel_min, voxel_max); + + pt_voxel_center.x = (voxel_min.x () + voxel_max.x ()) / 2.0f; + pt_voxel_center.y = (voxel_min.y () + voxel_max.y ()) / 2.0f; + pt_voxel_center.z = (voxel_min.z () + voxel_max.z ()) / 2.0f; + cloudVoxel->points.push_back (pt_voxel_center); + + // If the asked depth is the depth of the octree, retrieve the centroid at this LeafNode + if (octree.getTreeDepth () == depth) + { + pcl::octree::OctreePointCloudVoxelCentroid::LeafNode* container = static_cast::LeafNode*> (tree_it.getCurrentOctreeNode ()); + + container->getContainer ().getCentroid (pt_centroid); + } + // Else, compute the centroid of the LeafNode under the current BranchNode + else + { + // Retrieve every centroid under the current BranchNode + pcl::octree::OctreeKey dummy_key; + pcl::PointCloud::VectorType voxelCentroids; + octree.getVoxelCentroidsRecursive (static_cast::BranchNode*> (*tree_it), dummy_key, voxelCentroids); + + // Iterate over the leafs to compute the centroid of all of them + pcl::CentroidPoint centroid; + for (int j = 0; j < voxelCentroids.size (); ++j) + { + centroid.add (voxelCentroids[j]); + } + centroid.get (pt_centroid); + } - 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); + displayCloud->points.push_back (pt_centroid); } double end = pcl::getTime ();