[OCTREE] Fix the visualization of the octree cubes.

tools/octree_viewer.cpp

@@ -60,9 +60,13 @@ class OctreeViewer
 {
 public:
   OctreeViewer (std::string &filename, double resolution) :
-    viz ("Octree visualizator"), cloud (new pcl::PointCloud<pcl::PointXYZ>()),
-        displayCloud (new pcl::PointCloud<pcl::PointXYZ>()), octree (resolution), displayCubes(false),
-        showPointsWithCubes (false), wireframe (true)
+    viz ("Octree visualizator"),
+    cloud (new pcl::PointCloud<pcl::PointXYZ>()),
+    displayCloud (new pcl::PointCloud<pcl::PointXYZ>()),
+    cloudVoxel (new pcl::PointCloud<pcl::PointXYZ>()),
+    octree (resolution), displayCubes(false),
+    showPointsWithCubes (false),
+    wireframe (true)
   {
 
     //try to load the cloud
@@ -115,6 +119,8 @@ class OctreeViewer
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
   //displayed_cloud
   pcl::PointCloud<pcl::PointXYZ>::Ptr displayCloud;
+  //displayed_cloud
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudVoxel;
   //octree
   pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ> octree;
   //level
@@ -218,7 +224,7 @@ class OctreeViewer
 
     viz.removeShape("level_t2");
     sprintf(level, "Voxel size: %.4fm [%lu voxels]", sqrt(octree.getVoxelSquaredSideLen(displayedDepth)),
-            displayCloud->points.size());
+            cloudVoxel->points.size());
     viz.addText(level, 0, 30, 1.0, 0.0, 0.0, "level_t2");
 
     viz.removeShape("org_t");
@@ -235,7 +241,7 @@ class OctreeViewer
     clearView();
 
     //prevent the display of too many cubes
-    bool displayCubeLegend = displayCubes && static_cast<int> (displayCloud->points.size ()) <= MAX_DISPLAYED_CUBES;
+    bool displayCubeLegend = displayCubes && static_cast<int> (cloudVoxel->points.size ()) <= MAX_DISPLAYED_CUBES;
 
     showLegend(displayCubeLegend);
 
@@ -281,11 +287,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<vtkCubeSource> wk_cubeSource = vtkSmartPointer<vtkCubeSource>::New ();
 
@@ -341,10 +347,12 @@ class OctreeViewer
   void extractPointsAtLevel(int depth)
   {
     displayCloud->points.clear();
+    cloudVoxel->points.clear();
 
     pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::Iterator tree_it;
     pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::Iterator tree_it_end = octree.end();
 
+    pcl::PointXYZ pt_voxel_center;
     pcl::PointXYZ pt_centroid;
     std::cout << "===== Extracting data at depth " << depth << "... " << std::flush;
     double start = pcl::getTime ();
@@ -355,6 +363,15 @@ class OctreeViewer
       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);
+
+      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<pcl::PointXYZ>::LeafNode* container = static_cast<pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::LeafNode*> (tree_it.getCurrentOctreeNode ());