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)
 {
 }
 
@@ -97,8 +97,8 @@ pcl::MarchingCubes<PointNT>::interpolateEdge (Eigen::Vector3f &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;
+    }
+  }
 }
 
 
@@ -233,36 +242,12 @@ pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PolygonMesh &output)
     return;
   }
 
-  // Create grid
-  grid_ = std::vector<float> (res_x_*res_y_*res_z_, 0.0f);
-
-  // Populate tree
-  tree_->setInputCloud (input_);
-
-  getBoundingBox ();
-
-  // Transform the point cloud into a voxel grid
-  // This needs to be implemented in a child class
-  voxelizeData ();
-
+  // real part of marching cube
+  performReconstructionProc();
 
+  pcl::toPCLPointCloud2 (intermediate_cloud_, output.cloud);
 
-  // Run the actual marching cubes algorithm, store it into a point cloud,
-  // and copy the point cloud + connectivity into output
-  pcl::PointCloud<PointNT> cloud;
-
-  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);
+  output.polygons.resize (intermediate_cloud_.size () / 3);
   for (size_t i = 0; i < output.polygons.size (); ++i)
   {
     pcl::Vertices v;
@@ -288,8 +273,28 @@ pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PointCloud<PointNT> &po
     return;
   }
 
+  // real part of marching cube
+  performReconstructionProc();
+
+  points.swap(intermediate_cloud_);
+
+  polygons.resize (points.size () / 3);
+  for (size_t i = 0; i < 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;
+    polygons[i] = v;
+  }
+}
+
+template <typename PointNT> void
+pcl::MarchingCubes<PointNT>::performReconstructionProc ()
+{
   // Create grid
-  grid_ = std::vector<float> (res_x_*res_y_*res_z_, 0.0f);
+  // 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,30 +307,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_);
       }
-
-  polygons.resize (points.size () / 3);
-  for (size_t i = 0; i < 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;
-    polygons[i] = v;
-  }
 }
 
 
-
 #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,46 @@ pcl::MarchingCubesHoppe<PointNT>::~MarchingCubesHoppe ()
 template <typename PointNT> void
 pcl::MarchingCubesHoppe<PointNT>::voxelizeData ()
 {
+  const bool is_far_ignored = dist_ignore_ > 0.0f;
+
+  std::vector<int> nn_indices(1, 0);
+  std::vector<float> nn_sqr_dists(1, 0.0f);
+  Eigen::Vector3f point;
+  PointNT p;
+  const Eigen::Vector4f delta = ( max_p_ - min_p_ ).cwiseQuotient( Eigen::Vector4f(
+      float (res_x_),  float (res_y_),  float (res_z_), 1.0f ) );
+
+  Eigen::Vector3f normal;
+
   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_);
+        point[0] = min_p_[0] + delta[0] * float (x);
+        point[1] = min_p_[1] + delta[1] * float (y);
+        point[2] = min_p_[2] + delta[2] * float (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_ )
+        {
+          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,15 @@ 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 the point cloud really generated from Marching Cubes
+       */
+      pcl::PointCloud<PointNT> intermediate_cloud_;
+
       /** \brief Convert the point cloud into voxel data. */
       virtual void
       voxelizeData () = 0;
@@ -468,8 +486,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. */
@@ -508,6 +526,11 @@ namespace pcl
        performReconstruction (pcl::PointCloud<PointNT> &points,
                               std::vector<pcl::Vertices> &polygons);
 
+        /** \brief real marching cube part. called in two performReconstruction-s
+          */
+        virtual void
+        performReconstructionProc ();
+
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   };

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;