Merge pull request #5867 from mvieth/voxel_grid_occlusion_estimation

VoxelGridOcclusionEstimation: fix behaviour if sensor origin is inside the voxel grid

filters/include/pcl/filters/impl/voxel_grid_occlusion_estimation.hpp

@@ -62,6 +62,13 @@ pcl::VoxelGridOcclusionEstimation<PointT>::initializeVoxelGrid ()
   // set the sensor origin and sensor orientation
   sensor_origin_ = filtered_cloud_.sensor_origin_;
   sensor_orientation_ = filtered_cloud_.sensor_orientation_;
+
+  Eigen::Vector3i ijk = this->getGridCoordinates(sensor_origin_.x(), sensor_origin_.y(), sensor_origin_.z());
+  // first check whether the sensor origin is within the voxel grid bounding box, then whether it is occupied
+  if((ijk[0]>=min_b_[0] && ijk[1]>=min_b_[1] && ijk[2]>=min_b_[2] && ijk[0]<=max_b_[0] && ijk[1]<=max_b_[1] && ijk[2]<=max_b_[2]) && this->getCentroidIndexAt (ijk) != -1) {
+    PCL_WARN ("[VoxelGridOcclusionEstimation::initializeVoxelGrid] The voxel containing the sensor origin (%g, %g, %g) is marked as occupied. We will instead assume that it is free, to be able to perform the occlusion estimation.\n", sensor_origin_.x(), sensor_origin_.y(), sensor_origin_.z());
+    this->leaf_layout_[((Eigen::Vector4i() << ijk, 0).finished() - min_b_).dot (this->divb_mul_)] = -1;
+  }
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -233,8 +240,7 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayBoxIntersection (const Eigen::Vect
     tmin = tzmin;
   if (tzmax < tmax)
     tmax = tzmax;
-
-  return tmin;
+  return std::max<float>(tmin, 0.0f); // We only want to go in "positive" direction here, not in negative. This is relevant if the origin is inside the bounding box
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////

filters/include/pcl/filters/voxel_grid_occlusion_estimation.h

@@ -47,7 +47,15 @@ namespace pcl
   /** \brief VoxelGrid to estimate occluded space in the scene.
     * The ray traversal algorithm is implemented by the work of 
     * 'John Amanatides and Andrew Woo, A Fast Voxel Traversal Algorithm for Ray Tracing'
-    *
+    * Example code:
+    * \code
+    * pcl::VoxelGridOcclusionEstimation<pcl::PointXYZ> vg;
+    * vg.setInputCloud (input_cloud);
+    * vg.setLeafSize (leaf_x, leaf_y, leaf_z);
+    * vg.initializeVoxelGrid ();
+    * std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > occluded_voxels;
+    * vg.occlusionEstimationAll (occluded_voxels);
+    * \endcode
     * \author Christian Potthast
     * \ingroup filters
     */

tools/voxel_grid_occlusion_estimation.cpp

@@ -43,6 +43,7 @@
 #include <pcl/console/print.h>
 #include <pcl/console/parse.h>
 #include <pcl/console/time.h>
+#include <pcl/visualization/vtk/vtkRenderWindowInteractorFix.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/filters/voxel_grid_occlusion_estimation.h>
 #include <vtkCubeSource.h>
@@ -79,6 +80,7 @@ getCuboid (double minX, double maxX, double minY, double maxY, double minZ, doub
 {
   vtkSmartPointer < vtkCubeSource > cube = vtkSmartPointer<vtkCubeSource>::New ();
   cube->SetBounds (minX, maxX, minY, maxY, minZ, maxZ);
+  cube->Update ();
   return cube->GetOutput ();
 }
 
@@ -99,6 +101,7 @@ getVoxelActors (pcl::PointCloud<pcl::PointXYZ>& voxelCenters,
 
     treeWireframe->AddInputData (getCuboid (x - s, x + s, y - s, y + s, z - s, z + s));
   }
+  treeWireframe->Update ();
 
   vtkSmartPointer < vtkLODActor > treeActor = vtkSmartPointer<vtkLODActor>::New ();
 
@@ -119,6 +122,7 @@ displayBoundingBox (Eigen::Vector3f& min_b, Eigen::Vector3f& max_b,
 {
   vtkSmartPointer < vtkAppendPolyData > treeWireframe = vtkSmartPointer<vtkAppendPolyData>::New ();
   treeWireframe->AddInputData (getCuboid (min_b[0], max_b[0], min_b[1], max_b[1], min_b[2], max_b[2]));
+  treeWireframe->Update();
 
   vtkSmartPointer < vtkActor > treeActor = vtkSmartPointer<vtkActor>::New ();
 
@@ -218,17 +222,19 @@ int main (int argc, char** argv)
     (*occ_centroids)[i] = point;
   }
 
+  // we use the filtered cloud because it contains exactly one point per occupied voxel (avoids drawing the same voxel box multiple times)
+  CloudT filtered_cloud = vg.getFilteredPointCloud();
   CloudT::Ptr cloud_centroids (new CloudT);
-  cloud_centroids->width = input_cloud->size ();
+  cloud_centroids->width = filtered_cloud.size ();
   cloud_centroids->height = 1;
   cloud_centroids->is_dense = false;
-  cloud_centroids->points.resize (input_cloud->size ());
+  cloud_centroids->points.resize (filtered_cloud.size ());
 
-  for (std::size_t i = 0; i < input_cloud->size (); ++i)
+  for (std::size_t i = 0; i < filtered_cloud.size (); ++i)
   {
-    float x = (*input_cloud)[i].x;
-    float y = (*input_cloud)[i].y;
-    float z = (*input_cloud)[i].z;
+    float x = filtered_cloud[i].x;
+    float y = filtered_cloud[i].y;
+    float z = filtered_cloud[i].z;
     Eigen::Vector3i c = vg.getGridCoordinates (x, y, z);
     Eigen::Vector4f xyz = vg.getCentroidCoordinate (c);
     PointT point;
@@ -254,7 +260,7 @@ int main (int argc, char** argv)
     vtkSmartPointer<vtkRenderWindow>::New();
   renderWindow->AddRenderer (renderer);  
   vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor = 
-    vtkSmartPointer<vtkRenderWindowInteractor>::New();
+    vtkSmartPointer<vtkRenderWindowInteractor>::Take (vtkRenderWindowInteractorFixNew ());
   renderWindowInteractor->SetRenderWindow (renderWindow);
 
   // Add the actors to the renderer, set the background and size