Merge pull request #4973 from tin1254/fix_mls_voxel_grid_dilation

Fix mls voxel grid hashing out of bound

Author: mvieth

surface/include/pcl/surface/impl/mls.hpp

@@ -403,7 +403,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::performUpsampling (PointCloudOut &
   {
     corresponding_input_indices_.reset (new PointIndices);
 
-    MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_);
+    MLSVoxelGrid voxel_grid (input_, indices_, voxel_size_, dilation_iteration_num_);
     for (int iteration = 0; iteration < dilation_iteration_num_; ++iteration)
       voxel_grid.dilate ();
 
@@ -805,15 +805,18 @@ pcl::MLSResult::computeMLSSurface (const pcl::PointCloud<PointT> &cloud,
 template <typename PointInT, typename PointOutT>
 pcl::MovingLeastSquares<PointInT, PointOutT>::MLSVoxelGrid::MLSVoxelGrid (PointCloudInConstPtr& cloud,
                                                                           IndicesPtr &indices,
-                                                                          float voxel_size) :
+                                                                          float voxel_size,
+                                                                          int dilation_iteration_num) :
   voxel_grid_ (), data_size_ (), voxel_size_ (voxel_size)
 {
   pcl::getMinMax3D (*cloud, *indices, bounding_min_, bounding_max_);
+  bounding_min_ -= Eigen::Vector4f::Constant(voxel_size_ * (dilation_iteration_num + 1));
+  bounding_max_ += Eigen::Vector4f::Constant(voxel_size_ * (dilation_iteration_num + 1));
 
   Eigen::Vector4f bounding_box_size = bounding_max_ - bounding_min_;
   const double max_size = (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()), bounding_box_size.z ());
   // Put initial cloud in voxel grid
-  data_size_ = static_cast<std::uint64_t> (1.5 * max_size / voxel_size_);
+  data_size_ = static_cast<std::uint64_t> (std::ceil(max_size / voxel_size_));
   for (std::size_t i = 0; i < indices->size (); ++i)
     if (std::isfinite ((*cloud)[(*indices)[i]].x))
     {

surface/include/pcl/surface/mls.h

@@ -591,7 +591,8 @@ namespace pcl
 
           MLSVoxelGrid (PointCloudInConstPtr& cloud,
                         IndicesPtr &indices,
-                        float voxel_size);
+                        float voxel_size,
+                        int dilation_iteration_num);
 
           void
           dilate ();

test/surface/test_moving_least_squares.cpp

@@ -44,6 +44,7 @@
 #include <pcl/io/vtk_io.h>
 #include <pcl/features/normal_3d.h>
 #include <pcl/surface/mls.h>
+#include <pcl/common/common.h> // getMinMax3D
 
 using namespace pcl;
 using namespace pcl::io;
@@ -127,17 +128,29 @@ TEST (PCL, MovingLeastSquares)
 //  EXPECT_NEAR ((*mls_normals)[10].curvature, 0.019003, 1e-3);
 //  EXPECT_EQ (mls_normals->size (), 457);
 
-
+  const float voxel_size = 0.005f;
+  const int num_dilations = 5;
   mls_upsampling.setUpsamplingMethod (MovingLeastSquares<PointXYZ, PointNormal>::VOXEL_GRID_DILATION);
-  mls_upsampling.setDilationIterations (5);
-  mls_upsampling.setDilationVoxelSize (0.005f);
+  mls_upsampling.setDilationIterations (num_dilations);
+  mls_upsampling.setDilationVoxelSize (voxel_size);
   mls_normals->clear ();
   mls_upsampling.process (*mls_normals);
-  EXPECT_NEAR ((*mls_normals)[10].x, -0.070005938410758972, 2e-3);
-  EXPECT_NEAR ((*mls_normals)[10].y, 0.028887597844004631, 2e-3);
+  EXPECT_NEAR ((*mls_normals)[10].x, -0.086348414421081543, 2e-3);
+  EXPECT_NEAR ((*mls_normals)[10].y, 0.080920584499835968, 2e-3);
   EXPECT_NEAR ((*mls_normals)[10].z, 0.01788550429046154, 2e-3);
   EXPECT_NEAR ((*mls_normals)[10].curvature, 0.107273, 1e-1);
-  EXPECT_NEAR (double (mls_normals->size ()), 29394, 2);
+  EXPECT_NEAR (double (mls_normals->size ()), 25483, 2);
+  
+  // Check the boundary
+  Eigen::Vector4f original_min_pt, original_max_pt, min_pt, max_pt;
+  pcl::getMinMax3D(*cloud, original_min_pt, original_max_pt);
+  pcl::getMinMax3D(*mls_normals, min_pt, max_pt);
+  EXPECT_TRUE(
+      (original_min_pt.array() - (num_dilations + 3) * voxel_size <= min_pt.array())
+          .all());
+  EXPECT_TRUE(
+      (max_pt.array() <= original_max_pt.array() + (num_dilations + 4) * voxel_size)
+          .all());
 }
 
 #ifdef _OPENMP