VoxelGridOcclusionEstimation should always round down to go from coordinates to voxel indices. (#5942)

* VoxelGrid coordinate to index mappoing should always round down

* occlusion estimation: Fix floating point inaccuracies placing entry voxel outside of cloud bounding box

* add VoxelGridOcclusionEstimation test

* Fix incorrect doc

* Fix missing include

* add epsilon to ray entry coordinate in second location as well

Author: themightyoarfish

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

@@ -47,7 +47,7 @@ pcl::VoxelGridOcclusionEstimation<PointT>::initializeVoxelGrid ()
 {
   // initialization set to true
   initialized_ = true;
-  
+
   // create the voxel grid and store the output cloud
   this->filter (filtered_cloud_);
 
@@ -162,13 +162,13 @@ pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimationAll (std::vector<E
           Eigen::Vector4f p = getCentroidCoordinate (ijk);
           Eigen::Vector4f direction = p - sensor_origin_;
           direction.normalize ();
-          
+
           // estimate entry point into the voxel grid
           float tmin = rayBoxIntersection (sensor_origin_, direction);
 
           // ray traversal
           int state = rayTraversal (ijk, sensor_origin_, direction, tmin);
-          
+
           // if voxel is occluded
           if (state == 1)
             occluded_voxels.push_back (ijk);
@@ -179,7 +179,7 @@ pcl::VoxelGridOcclusionEstimation<PointT>::occlusionEstimationAll (std::vector<E
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> float
-pcl::VoxelGridOcclusionEstimation<PointT>::rayBoxIntersection (const Eigen::Vector4f& origin, 
+pcl::VoxelGridOcclusionEstimation<PointT>::rayBoxIntersection (const Eigen::Vector4f& origin,
                                                                const Eigen::Vector4f& direction)
 {
   float tmin, tmax, tymin, tymax, tzmin, tzmax;
@@ -198,7 +198,7 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayBoxIntersection (const Eigen::Vect
   if (direction[1] >= 0)
   {
     tymin = (b_min_[1] - origin[1]) / direction[1];
-    tymax = (b_max_[1] - origin[1]) / direction[1]; 
+    tymax = (b_max_[1] - origin[1]) / direction[1];
   }
   else
   {
@@ -233,7 +233,7 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayBoxIntersection (const Eigen::Vect
   {
     PCL_ERROR ("no intersection with the bounding box \n");
     tmin = -1.0f;
-    return tmin;       
+    return tmin;
   }
 
   if (tzmin > tmin)
@@ -246,15 +246,16 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayBoxIntersection (const Eigen::Vect
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointT> int
 pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (const Eigen::Vector3i& target_voxel,
-                                                         const Eigen::Vector4f& origin, 
+                                                         const Eigen::Vector4f& origin,
                                                          const Eigen::Vector4f& direction,
                                                          const float t_min)
 {
-  // coordinate of the boundary of the voxel grid
-  Eigen::Vector4f start = origin + t_min * direction;
+  // coordinate of the boundary of the voxel grid. To avoid numerical imprecision
+  // causing the start voxel index to be off by one, we add the machine epsilon
+  Eigen::Vector4f start = origin + (t_min > 0.0f ? (t_min + std::numeric_limits<float>::epsilon()) : t_min) * direction;
 
   // i,j,k coordinate of the voxel were the ray enters the voxel grid
-  Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]);
+  Eigen::Vector3i ijk = this->getGridCoordinates(start[0], start[1], start[2]);
 
   // steps in which direction we have to travel in the voxel grid
   int step_x, step_y, step_z;
@@ -296,13 +297,13 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (const Eigen::Vector3i&
   float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0];
   float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1];
   float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2];
-     
+
   float t_delta_x = leaf_size_[0] / static_cast<float> (std::abs (direction[0]));
   float t_delta_y = leaf_size_[1] / static_cast<float> (std::abs (direction[1]));
   float t_delta_z = leaf_size_[2] / static_cast<float> (std::abs (direction[2]));
 
-  while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) && 
-          (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) && 
+  while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
+          (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
           (ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) )
   {
     // check if we reached target voxel
@@ -339,20 +340,20 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (const Eigen::Vector3i&
 template <typename PointT> int
 pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> >& out_ray,
                                                          const Eigen::Vector3i& target_voxel,
-                                                         const Eigen::Vector4f& origin, 
+                                                         const Eigen::Vector4f& origin,
                                                          const Eigen::Vector4f& direction,
                                                          const float t_min)
 {
   // reserve space for the ray vector
   int reserve_size = div_b_.maxCoeff () * div_b_.maxCoeff ();
   out_ray.reserve (reserve_size);
 
-  // coordinate of the boundary of the voxel grid
-  Eigen::Vector4f start = origin + t_min * direction;
+  // coordinate of the boundary of the voxel grid. To avoid numerical imprecision
+  // causing the start voxel index to be off by one, we add the machine epsilon
+  Eigen::Vector4f start = origin + (t_min > 0.0f ? (t_min + std::numeric_limits<float>::epsilon()) : t_min) * direction;
 
   // i,j,k coordinate of the voxel were the ray enters the voxel grid
-  Eigen::Vector3i ijk = getGridCoordinatesRound (start[0], start[1], start[2]);
-  //Eigen::Vector3i ijk = this->getGridCoordinates (start_x, start_y, start_z);
+  Eigen::Vector3i ijk = this->getGridCoordinates (start[0], start[1], start[2]);
 
   // steps in which direction we have to travel in the voxel grid
   int step_x, step_y, step_z;
@@ -394,16 +395,16 @@ pcl::VoxelGridOcclusionEstimation<PointT>::rayTraversal (std::vector<Eigen::Vect
   float t_max_x = t_min + (voxel_max[0] - start[0]) / direction[0];
   float t_max_y = t_min + (voxel_max[1] - start[1]) / direction[1];
   float t_max_z = t_min + (voxel_max[2] - start[2]) / direction[2];
-     
+
   float t_delta_x = leaf_size_[0] / static_cast<float> (std::abs (direction[0]));
   float t_delta_y = leaf_size_[1] / static_cast<float> (std::abs (direction[1]));
   float t_delta_z = leaf_size_[2] / static_cast<float> (std::abs (direction[2]));
 
   // the index of the cloud (-1 if empty)
   int result = 0;
 
-  while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) && 
-          (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) && 
+  while ( (ijk[0] < max_b_[0]+1) && (ijk[0] >= min_b_[0]) &&
+          (ijk[1] < max_b_[1]+1) && (ijk[1] >= min_b_[1]) &&
           (ijk[2] < max_b_[2]+1) && (ijk[2] >= min_b_[2]) )
   {
     // add voxel to ray

filters/include/pcl/filters/voxel_grid_occlusion_estimation.h

@@ -216,7 +216,7 @@ namespace pcl
                     const Eigen::Vector4f& direction,
                     const float t_min);
 
-      /** \brief Returns a rounded value. 
+      /** \brief Returns a value rounded to the nearest integer
         * \param[in] d
         * \return rounded value
         */
@@ -226,8 +226,7 @@ namespace pcl
         return static_cast<float> (std::floor (d + 0.5f));
       }
 
-      // We use round here instead of std::floor due to some numerical issues.
-      /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z). 
+      /** \brief Returns the corresponding (i,j,k) coordinates in the grid of point (x,y,z).
         * \param[in] x the X point coordinate to get the (i, j, k) index at
         * \param[in] y the Y point coordinate to get the (i, j, k) index at
         * \param[in] z the Z point coordinate to get the (i, j, k) index at

test/filters/test_filters.cpp

@@ -48,6 +48,7 @@
 #include <pcl/filters/frustum_culling.h>
 #include <pcl/filters/sampling_surface_normal.h>
 #include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/voxel_grid_occlusion_estimation.h>
 #include <pcl/filters/voxel_grid_covariance.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/filters/project_inliers.h>
@@ -2460,6 +2461,27 @@ TEST (NormalRefinement, Filters)
   EXPECT_LT(err_refined_var, err_est_var);
 }
 
+TEST (VoxelGridOcclusionEstimation, Filters)
+{
+  auto input_cloud = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
+  input_cloud->emplace_back(0.0, 0.0, 0.0);
+  input_cloud->emplace_back(9.9, 9.9, 9.9); // we want a nice bounding box from (0, 0, 0) to (10, 10, 10)
+  input_cloud->sensor_origin_ << -0.1, 0.5, 0.5; // just outside the bounding box. Most rays will enter at voxel (0, 0, 0)
+  pcl::VoxelGridOcclusionEstimation<pcl::PointXYZ> vg;
+  vg.setInputCloud (input_cloud);
+  vg.setLeafSize (1.0, 1.0, 1.0);
+  vg.initializeVoxelGrid ();
+  std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i> > occluded_voxels;
+  vg.occlusionEstimationAll (occluded_voxels);
+  for(std::size_t y=0; y<10; ++y) {
+    for (std::size_t z=0; z<10; ++z) {
+      if(y==9 && z==9) continue; // voxel (9, 9, 9) is occupied by point (9.9, 9.9, 9.9), so it will not be counted as occluded
+      Eigen::Vector3i cell(9, y, z);
+      EXPECT_NE(std::find(occluded_voxels.begin(), occluded_voxels.end(), cell), occluded_voxels.end()); // not equal means it was found
+    }
+  }
+}
+
 /* ---[ */
 int
 main (int argc, char** argv)