Avoid phantom surfces in marching cubes hoppe

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

@@ -42,13 +42,6 @@
 #include <pcl/Vertices.h>
 #include <pcl/kdtree/kdtree_flann.h>
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointNT>
-pcl::MarchingCubes<PointNT>::MarchingCubes () 
-: min_p_ (), max_p_ (), percentage_extend_grid_ (), iso_level_ ()
-{
-}
-
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT>
 pcl::MarchingCubes<PointNT>::~MarchingCubes ()
@@ -59,26 +52,17 @@ pcl::MarchingCubes<PointNT>::~MarchingCubes ()
 template <typename PointNT> void
 pcl::MarchingCubes<PointNT>::getBoundingBox ()
 {
-  pcl::getMinMax3D (*input_, min_p_, max_p_);
-
-  min_p_ -= (max_p_ - min_p_) * percentage_extend_grid_/2;
-  max_p_ += (max_p_ - min_p_) * percentage_extend_grid_/2;
-
-  Eigen::Vector4f bounding_box_size = max_p_ - min_p_;
-
-  bounding_box_size = max_p_ - min_p_;
-  PCL_DEBUG ("[pcl::MarchingCubesHoppe::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n",
-             bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ());
-  double max_size =
-      (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()),
-          bounding_box_size.z ());
-  (void)max_size;
-  // ????
-  //  data_size_ = static_cast<uint64_t> (max_size / leaf_size_);
-  PCL_DEBUG ("[pcl::MarchingCubesHoppe::getBoundingBox] Lower left point is [%f, %f, %f]\n",
-             min_p_.x (), min_p_.y (), min_p_.z ());
-  PCL_DEBUG ("[pcl::MarchingCubesHoppe::getBoundingBox] Upper left point is [%f, %f, %f]\n",
-             max_p_.x (), max_p_.y (), max_p_.z ());
+  PointNT max_pt, min_pt;
+  pcl::getMinMax3D (*input_, min_pt, max_pt);
+
+  lower_boundary_ = min_pt.getVector3fMap ().array ();
+  upper_boundary_ = max_pt.getVector3fMap ().array ();
+
+  const Eigen::Array3f size3_extend = 0.5f * percentage_extend_grid_ 
+    * (upper_boundary_ - lower_boundary_);
+
+  lower_boundary_ -= size3_extend;
+  upper_boundary_ += size3_extend;
 }
 
 
@@ -90,15 +74,15 @@ pcl::MarchingCubes<PointNT>::interpolateEdge (Eigen::Vector3f &p1,
                                               float val_p2,
                                               Eigen::Vector3f &output)
 {
-  float mu = (iso_level_ - val_p1) / (val_p2-val_p1);
+  const 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;
@@ -116,30 +100,26 @@ pcl::MarchingCubes<PointNT>::createSurface (std::vector<float> &leaf_node,
   if (edgeTable[cubeindex] == 0)
     return;
 
-  //Eigen::Vector4f index_3df (index_3d[0], index_3d[1], index_3d[2], 0.0f);
-  Eigen::Vector3f center;// TODO coeff wise product = min_p_ + Eigen::Vector4f (1.0f/res_x_, 1.0f/res_y_, 1.0f/res_z_) * index_3df * (max_p_ - min_p_);
-  center[0] = min_p_[0] + (max_p_[0] - min_p_[0]) * float (index_3d[0]) / float (res_x_);
-  center[1] = min_p_[1] + (max_p_[1] - min_p_[1]) * float (index_3d[1]) / float (res_y_);
-  center[2] = min_p_[2] + (max_p_[2] - min_p_[2]) * float (index_3d[2]) / float (res_z_);
+  const Eigen::Vector3f center = lower_boundary_ 
+    + size_voxel_ * index_3d.cast<float> ().array ();
 
   std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > p;
   p.resize (8);
   for (int i = 0; i < 8; ++i)
   {
     Eigen::Vector3f point = center;
     if(i & 0x4)
-      point[1] = static_cast<float> (center[1] + (max_p_[1] - min_p_[1]) / float (res_y_));
+      point[1] = static_cast<float> (center[1] + size_voxel_[1]);
 
     if(i & 0x2)
-      point[2] = static_cast<float> (center[2] + (max_p_[2] - min_p_[2]) / float (res_z_));
+      point[2] = static_cast<float> (center[2] + size_voxel_[2]);
 
     if((i & 0x1) ^ ((i >> 1) & 0x1))
-      point[0] = static_cast<float> (center[0] + (max_p_[0] - min_p_[0]) / float (res_x_));
+      point[0] = static_cast<float> (center[0] + size_voxel_[0]);
 
     p[i] = point;
   }
 
-
   // Find the vertices where the surface intersects the cube
   if (edgeTable[cubeindex] & 1)
     interpolateEdge (p[0], p[1], leaf_node[0], leaf_node[1], vertex_list[0]);
@@ -191,7 +171,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 +181,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 +213,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_);
+  pcl::PointCloud<PointNT> points;
 
-  getBoundingBox ();
+  performReconstruction (points, output.polygons);
 
-  // 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;
-
-  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,38 +228,50 @@ 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_);
 
+  // Compute bounding box and voxel size
   getBoundingBox ();
+  size_voxel_ = (upper_boundary_ - lower_boundary_) 
+    * Eigen::Array3f (res_x_, res_y_, res_z_).inverse ();
 
   // 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
-  points.clear ();
+  // preallocate memory assuming a hull. suppose 6 point per voxel
+  double size_reserve = std::min((double) intermediate_cloud.points.max_size (),
+      2.0 * 6.0 * (double) (res_y_*res_z_ + res_x_*res_z_ + res_x_*res_y_));
+  intermediate_cloud.reserve ((size_t) size_reserve);
+
   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 +283,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

@@ -42,13 +42,6 @@
 #include <pcl/Vertices.h>
 #include <pcl/kdtree/kdtree_flann.h>
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointNT>
-pcl::MarchingCubesHoppe<PointNT>::MarchingCubesHoppe ()
-  : MarchingCubes<PointNT> ()
-{
-}
-
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT>
 pcl::MarchingCubesHoppe<PointNT>::~MarchingCubesHoppe ()
@@ -60,26 +53,38 @@ pcl::MarchingCubesHoppe<PointNT>::~MarchingCubesHoppe ()
 template <typename PointNT> void
 pcl::MarchingCubesHoppe<PointNT>::voxelizeData ()
 {
+  const bool is_far_ignored = dist_ignore_ > 0.0f;
+
   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 = (lower_boundary_ + size_voxel_ * Eigen::Array3f (x, y, z)).matrix ();
         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(!std::isnan (normal(0)) && normal.norm () > 0.5f)
+            grid_[z_start + z] = normal.dot (
+                point - input_->points[nn_indices[0]].getVector3fMap ());
+        }
       }
+    }
+  }
 }
 
 

surface/include/pcl/surface/impl/marching_cubes_rbf.hpp

@@ -45,14 +45,6 @@
 #include <pcl/Vertices.h>
 #include <pcl/kdtree/kdtree_flann.h>
 
-//////////////////////////////////////////////////////////////////////////////////////////////
-template <typename PointNT>
-pcl::MarchingCubesRBF<PointNT>::MarchingCubesRBF ()
-  : MarchingCubes<PointNT> (),
-    off_surface_epsilon_ (0.1f)
-{
-}
-
 //////////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointNT>
 pcl::MarchingCubesRBF<PointNT>::~MarchingCubesRBF ()
@@ -64,7 +56,7 @@ template <typename PointNT> void
 pcl::MarchingCubesRBF<PointNT>::voxelizeData ()
 {
   // Initialize data structures
-  unsigned int N = static_cast<unsigned int> (input_->size ());
+  const unsigned int N = static_cast<unsigned int> (input_->size ());
   Eigen::MatrixXd M (2*N, 2*N),
                   d (2*N, 1);
 
@@ -103,10 +95,9 @@ pcl::MarchingCubesRBF<PointNT>::voxelizeData ()
     for (int y = 0; y < res_y_; ++y)
       for (int z = 0; z < res_z_; ++z)
       {
-        Eigen::Vector3d 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_);
+        const Eigen::Vector3f point_f = (size_voxel_ * Eigen::Array3f (x, y, z) 
+            + lower_boundary_).matrix ();
+        const Eigen::Vector3d point = point_f.cast<double> ();
 
         double f = 0.0;
         std::vector<double>::const_iterator w_it (weights.begin());