Avoid phantom surfces in marching cubes hoppe

surface/include/pcl/surface/impl/marching_cubes.hpp

@@ -45,7 +45,7 @@
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT>
 pcl::MarchingCubes<PointNT>::MarchingCubes () 
-: min_p_ (), max_p_ (), percentage_extend_grid_ (), iso_level_ ()
+: min_p_ (), max_p_ (), percentage_extend_grid_ (), iso_level_ (), dist_ignore_ (-1.0f)
 {
 }
 
@@ -90,15 +90,15 @@ pcl::MarchingCubes<PointNT>::interpolateEdge (Eigen::Vector3f &p1,
                                               float val_p2,
                                               Eigen::Vector3f &output)
 {
-  float mu = (iso_level_ - val_p1) / (val_p2-val_p1);
+  float mu = (iso_level_ - val_p1) / (val_p2 - val_p1);
   output = p1 + mu * (p2 - p1);
 }
 
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT> void
-pcl::MarchingCubes<PointNT>::createSurface (std::vector<float> &leaf_node,
-                                            Eigen::Vector3i &index_3d,
+pcl::MarchingCubes<PointNT>::createSurface (const std::vector<float> &leaf_node,
+                                            const Eigen::Vector3i &index_3d,
                                             pcl::PointCloud<PointNT> &cloud)
 {
   int cubeindex = 0;
@@ -191,7 +191,7 @@ template <typename PointNT> void
 pcl::MarchingCubes<PointNT>::getNeighborList1D (std::vector<float> &leaf,
                                                 Eigen::Vector3i &index3d)
 {
-  leaf = std::vector<float> (8, 0.0f);
+  leaf.resize(8);
 
   leaf[0] = getGridValue (index3d);
   leaf[1] = getGridValue (index3d + Eigen::Vector3i (1, 0, 0));
@@ -201,6 +201,15 @@ pcl::MarchingCubes<PointNT>::getNeighborList1D (std::vector<float> &leaf,
   leaf[5] = getGridValue (index3d + Eigen::Vector3i (1, 1, 0));
   leaf[6] = getGridValue (index3d + Eigen::Vector3i (1, 1, 1));
   leaf[7] = getGridValue (index3d + Eigen::Vector3i (0, 1, 1));
+
+  for(int i = 0; i < 8; ++i)
+  {
+    if(std::isnan(leaf[i]))
+    {
+      leaf.clear();
+      break;
+    }
+  }
 }
 
 
@@ -224,53 +233,11 @@ pcl::MarchingCubes<PointNT>::getGridValue (Eigen::Vector3i pos)
 template <typename PointNT> void
 pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PolygonMesh &output)
 {
-  if (!(iso_level_ >= 0 && iso_level_ < 1))
-  {
-    PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", getClassName ().c_str (), iso_level_);
-    output.cloud.width = output.cloud.height = 0;
-    output.cloud.data.clear ();
-    output.polygons.clear ();
-    return;
-  }
-
-  // Create grid
-  grid_ = std::vector<float> (res_x_*res_y_*res_z_, 0.0f);
-
-  // Populate tree
-  tree_->setInputCloud (input_);
-
-  getBoundingBox ();
+  pcl::PointCloud<PointNT> points;
 
-  // Transform the point cloud into a voxel grid
-  // This needs to be implemented in a child class
-  voxelizeData ();
-
-
-
-  // Run the actual marching cubes algorithm, store it into a point cloud,
-  // and copy the point cloud + connectivity into output
-  pcl::PointCloud<PointNT> cloud;
+  performReconstruction (points, output.polygons);
 
-  for (int x = 1; x < res_x_-1; ++x)
-    for (int y = 1; y < res_y_-1; ++y)
-      for (int z = 1; z < res_z_-1; ++z)
-      {
-        Eigen::Vector3i index_3d (x, y, z);
-        std::vector<float> leaf_node;
-        getNeighborList1D (leaf_node, index_3d);
-        createSurface (leaf_node, index_3d, cloud);
-      }
-  pcl::toPCLPointCloud2 (cloud, output.cloud);
-
-  output.polygons.resize (cloud.size () / 3);
-  for (size_t i = 0; i < output.polygons.size (); ++i)
-  {
-    pcl::Vertices v;
-    v.vertices.resize (3);
-    for (int j = 0; j < 3; ++j)
-      v.vertices[j] = static_cast<int> (i) * 3 + j;
-    output.polygons[i] = v;
-  }
+  pcl::toPCLPointCloud2(points, output.cloud);
 }
 
 
@@ -281,15 +248,19 @@ pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PointCloud<PointNT> &po
 {
   if (!(iso_level_ >= 0 && iso_level_ < 1))
   {
-    PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", getClassName ().c_str (), iso_level_);
+    PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", 
+        getClassName ().c_str (), iso_level_);
     points.width = points.height = 0;
     points.points.clear ();
     polygons.clear ();
     return;
   }
 
+  // the point cloud really generated from Marching Cubes, prev intermediate_cloud_
+  pcl::PointCloud<PointNT> intermediate_cloud;
+
   // Create grid
-  grid_ = std::vector<float> (res_x_*res_y_*res_z_, 0.0f);
+  grid_ = std::vector<float> (res_x_*res_y_*res_z_, NAN);
 
   // Populate tree
   tree_->setInputCloud (input_);
@@ -302,17 +273,24 @@ pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PointCloud<PointNT> &po
 
   // Run the actual marching cubes algorithm, store it into a point cloud,
   // and copy the point cloud + connectivity into output
-  points.clear ();
+  intermediate_cloud.clear();
+
+  // preallocate memory assuming a hull. suppose 6 point per voxel
+  intermediate_cloud.reserve((size_t)(res_y_*res_z_ + res_x_*res_z_ + res_x_*res_y_) * 2 * 6);
+
   for (int x = 1; x < res_x_-1; ++x)
     for (int y = 1; y < res_y_-1; ++y)
       for (int z = 1; z < res_z_-1; ++z)
       {
         Eigen::Vector3i index_3d (x, y, z);
         std::vector<float> leaf_node;
         getNeighborList1D (leaf_node, index_3d);
-        createSurface (leaf_node, index_3d, points);
+        if(!leaf_node.empty())
+          createSurface (leaf_node, index_3d, intermediate_cloud);
       }
 
+  points.swap(intermediate_cloud);
+
   polygons.resize (points.size () / 3);
   for (size_t i = 0; i < polygons.size (); ++i)
   {
@@ -324,8 +302,6 @@ pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PointCloud<PointNT> &po
   }
 }
 
-
-
 #define PCL_INSTANTIATE_MarchingCubes(T) template class PCL_EXPORTS pcl::MarchingCubes<T>;
 
 #endif    // PCL_SURFACE_IMPL_MARCHING_CUBES_H_

surface/include/pcl/surface/impl/marching_cubes_hoppe.hpp

@@ -60,26 +60,42 @@ pcl::MarchingCubesHoppe<PointNT>::~MarchingCubesHoppe ()
 template <typename PointNT> void
 pcl::MarchingCubesHoppe<PointNT>::voxelizeData ()
 {
+  const bool is_far_ignored = dist_ignore_ > 0.0f;
+
+  const Eigen::Vector3f min_p = min_p_.segment(0, 3);
+  const Eigen::Vector3f delta = ( max_p_ - min_p_ ).segment(0, 3).cwiseQuotient(
+        Eigen::Vector3f((float)res_x_, (float)res_y_, (float)res_z_) );
+
   for (int x = 0; x < res_x_; ++x)
+  {
+    const int y_start = x * res_y_ * res_z_;
+
     for (int y = 0; y < res_y_; ++y)
+    {
+      const int z_start = y_start + y * res_z_;
+
       for (int z = 0; z < res_z_; ++z)
       {
-        std::vector<int> nn_indices;
-        std::vector<float> nn_sqr_dists;
-
-        Eigen::Vector3f point;
-        point[0] = min_p_[0] + (max_p_[0] - min_p_[0]) * float (x) / float (res_x_);
-        point[1] = min_p_[1] + (max_p_[1] - min_p_[1]) * float (y) / float (res_y_);
-        point[2] = min_p_[2] + (max_p_[2] - min_p_[2]) * float (z) / float (res_z_);
-
+        std::vector<int> nn_indices(1, 0);
+        std::vector<float> nn_sqr_dists(1, 0.0f);
+        const Eigen::Vector3f point = min_p + delta.cwiseProduct(Eigen::Vector3f(x, y, z));
         PointNT p;
+
         p.getVector3fMap () = point;
 
         tree_->nearestKSearch (p, 1, nn_indices, nn_sqr_dists);
 
-        grid_[x * res_y_*res_z_ + y * res_z_ + z] = input_->points[nn_indices[0]].getNormalVector3fMap ().dot (
-            point - input_->points[nn_indices[0]].getVector3fMap ());
+        if( !is_far_ignored || nn_sqr_dists[0] < dist_ignore_ )
+        {
+          const Eigen::Vector3f normal = input_->points[nn_indices[0]].getNormalVector3fMap ();
+
+          if(normal.norm() > 0.5f)
+            grid_[z_start + z] = normal.dot (
+                point - input_->points[nn_indices[0]].getVector3fMap ());
+        }
       }
+    }
+  }
 }
 
 

surface/include/pcl/surface/marching_cubes.h

@@ -430,6 +430,15 @@ namespace pcl
       getPercentageExtendGrid ()
       { return percentage_extend_grid_; }
 
+      /** \brief Method that sets the parameter that defines the distance to ignore the distance to ignore
+        * a grid
+        * \param[in] threshold of distance. if it is negative, then calculate in all grids; otherwise
+        * ignore grids with distance to point cloud(to nearest point) larger than distIgnore
+        */
+      inline void
+      setDistanceIgnore (float distIgnore)
+      { dist_ignore_ = distIgnore; }
+
     protected:
       /** \brief The data structure storing the 3D grid */
       std::vector<float> grid_;
@@ -447,6 +456,11 @@ namespace pcl
       /** \brief The iso level to be extracted. */
       float iso_level_;
 
+      /** \brief ignore the distance function
+       * if it is negative
+       * or distance between voxel centroid and point are larger that it. */
+      float dist_ignore_;
+
       /** \brief Convert the point cloud into voxel data. */
       virtual void
       voxelizeData () = 0;
@@ -468,8 +482,8 @@ namespace pcl
         * \param cloud point cloud to store the vertices of the polygon
        */
       void
-      createSurface (std::vector<float> &leaf_node,
-                     Eigen::Vector3i &index_3d,
+      createSurface (const std::vector<float> &leaf_node,
+                     const Eigen::Vector3i &index_3d,
                      pcl::PointCloud<PointNT> &cloud);
 
       /** \brief Get the bounding box for the input data points. */

surface/include/pcl/surface/marching_cubes_hoppe.h

@@ -63,6 +63,7 @@ namespace pcl
       using MarchingCubes<PointNT>::res_z_;
       using MarchingCubes<PointNT>::min_p_;
       using MarchingCubes<PointNT>::max_p_;
+      using MarchingCubes<PointNT>::dist_ignore_;
 
       typedef typename pcl::PointCloud<PointNT>::Ptr PointCloudPtr;