marching_cubes.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  */
00035 
00036 #ifndef PCL_SURFACE_IMPL_MARCHING_CUBES_H_
00037 #define PCL_SURFACE_IMPL_MARCHING_CUBES_H_
00038 
00039 #include <pcl/surface/marching_cubes.h>
00040 #include <pcl/common/common.h>
00041 #include <pcl/common/vector_average.h>
00042 #include <pcl/Vertices.h>
00043 #include <pcl/kdtree/kdtree_flann.h>
00044 
00046 template <typename PointNT>
00047 pcl::MarchingCubes<PointNT>::MarchingCubes () 
00048 : min_p_ (), max_p_ (), percentage_extend_grid_ (), iso_level_ ()
00049 {
00050 }
00051 
00053 template <typename PointNT>
00054 pcl::MarchingCubes<PointNT>::~MarchingCubes ()
00055 {
00056 }
00057 
00059 template <typename PointNT> void
00060 pcl::MarchingCubes<PointNT>::getBoundingBox ()
00061 {
00062   pcl::getMinMax3D (*input_, min_p_, max_p_);
00063 
00064   min_p_ -= (max_p_ - min_p_) * percentage_extend_grid_/2;
00065   max_p_ += (max_p_ - min_p_) * percentage_extend_grid_/2;
00066 
00067   Eigen::Vector4f bounding_box_size = max_p_ - min_p_;
00068 
00069   bounding_box_size = max_p_ - min_p_;
00070   PCL_DEBUG ("[pcl::MarchingCubesHoppe::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n",
00071              bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ());
00072   double max_size =
00073       (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()),
00074           bounding_box_size.z ());
00075   (void)max_size;
00076   // ????
00077   //  data_size_ = static_cast<uint64_t> (max_size / leaf_size_);
00078   PCL_DEBUG ("[pcl::MarchingCubesHoppe::getBoundingBox] Lower left point is [%f, %f, %f]\n",
00079              min_p_.x (), min_p_.y (), min_p_.z ());
00080   PCL_DEBUG ("[pcl::MarchingCubesHoppe::getBoundingBox] Upper left point is [%f, %f, %f]\n",
00081              max_p_.x (), max_p_.y (), max_p_.z ());
00082 }
00083 
00084 
00086 template <typename PointNT> void
00087 pcl::MarchingCubes<PointNT>::interpolateEdge (Eigen::Vector3f &p1,
00088                                               Eigen::Vector3f &p2,
00089                                               float val_p1,
00090                                               float val_p2,
00091                                               Eigen::Vector3f &output)
00092 {
00093   float mu = (iso_level_ - val_p1) / (val_p2-val_p1);
00094   output = p1 + mu * (p2 - p1);
00095 }
00096 
00097 
00099 template <typename PointNT> void
00100 pcl::MarchingCubes<PointNT>::createSurface (std::vector<float> &leaf_node,
00101                                             Eigen::Vector3i &index_3d,
00102                                             pcl::PointCloud<PointNT> &cloud)
00103 {
00104   int cubeindex = 0;
00105   Eigen::Vector3f vertex_list[12];
00106   if (leaf_node[0] < iso_level_) cubeindex |= 1;
00107   if (leaf_node[1] < iso_level_) cubeindex |= 2;
00108   if (leaf_node[2] < iso_level_) cubeindex |= 4;
00109   if (leaf_node[3] < iso_level_) cubeindex |= 8;
00110   if (leaf_node[4] < iso_level_) cubeindex |= 16;
00111   if (leaf_node[5] < iso_level_) cubeindex |= 32;
00112   if (leaf_node[6] < iso_level_) cubeindex |= 64;
00113   if (leaf_node[7] < iso_level_) cubeindex |= 128;
00114 
00115   // Cube is entirely in/out of the surface
00116   if (edgeTable[cubeindex] == 0)
00117     return;
00118 
00119   //Eigen::Vector4f index_3df (index_3d[0], index_3d[1], index_3d[2], 0.0f);
00120   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_);
00121   center[0] = min_p_[0] + (max_p_[0] - min_p_[0]) * float (index_3d[0]) / float (res_x_);
00122   center[1] = min_p_[1] + (max_p_[1] - min_p_[1]) * float (index_3d[1]) / float (res_y_);
00123   center[2] = min_p_[2] + (max_p_[2] - min_p_[2]) * float (index_3d[2]) / float (res_z_);
00124 
00125   std::vector<Eigen::Vector3f> p;
00126   p.resize (8);
00127   for (int i = 0; i < 8; ++i)
00128   {
00129     Eigen::Vector3f point = center;
00130     if(i & 0x4)
00131       point[1] = static_cast<float> (center[1] + (max_p_[1] - min_p_[1]) / float (res_y_));
00132 
00133     if(i & 0x2)
00134       point[2] = static_cast<float> (center[2] + (max_p_[2] - min_p_[2]) / float (res_z_));
00135 
00136     if((i & 0x1) ^ ((i >> 1) & 0x1))
00137       point[0] = static_cast<float> (center[0] + (max_p_[0] - min_p_[0]) / float (res_x_));
00138 
00139     p[i] = point;
00140   }
00141 
00142 
00143   // Find the vertices where the surface intersects the cube
00144   if (edgeTable[cubeindex] & 1)
00145     interpolateEdge (p[0], p[1], leaf_node[0], leaf_node[1], vertex_list[0]);
00146   if (edgeTable[cubeindex] & 2)
00147     interpolateEdge (p[1], p[2], leaf_node[1], leaf_node[2], vertex_list[1]);
00148   if (edgeTable[cubeindex] & 4)
00149     interpolateEdge (p[2], p[3], leaf_node[2], leaf_node[3], vertex_list[2]);
00150   if (edgeTable[cubeindex] & 8)
00151     interpolateEdge (p[3], p[0], leaf_node[3], leaf_node[0], vertex_list[3]);
00152   if (edgeTable[cubeindex] & 16)
00153     interpolateEdge (p[4], p[5], leaf_node[4], leaf_node[5], vertex_list[4]);
00154   if (edgeTable[cubeindex] & 32)
00155     interpolateEdge (p[5], p[6], leaf_node[5], leaf_node[6], vertex_list[5]);
00156   if (edgeTable[cubeindex] & 64)
00157     interpolateEdge (p[6], p[7], leaf_node[6], leaf_node[7], vertex_list[6]);
00158   if (edgeTable[cubeindex] & 128)
00159     interpolateEdge (p[7], p[4], leaf_node[7], leaf_node[4], vertex_list[7]);
00160   if (edgeTable[cubeindex] & 256)
00161     interpolateEdge (p[0], p[4], leaf_node[0], leaf_node[4], vertex_list[8]);
00162   if (edgeTable[cubeindex] & 512)
00163     interpolateEdge (p[1], p[5], leaf_node[1], leaf_node[5], vertex_list[9]);
00164   if (edgeTable[cubeindex] & 1024)
00165     interpolateEdge (p[2], p[6], leaf_node[2], leaf_node[6], vertex_list[10]);
00166   if (edgeTable[cubeindex] & 2048)
00167     interpolateEdge (p[3], p[7], leaf_node[3], leaf_node[7], vertex_list[11]);
00168 
00169   // Create the triangle
00170   for (int i = 0; triTable[cubeindex][i] != -1; i+=3)
00171   {
00172     PointNT p1,p2,p3;
00173     p1.x = vertex_list[triTable[cubeindex][i  ]][0];
00174     p1.y = vertex_list[triTable[cubeindex][i  ]][1];
00175     p1.z = vertex_list[triTable[cubeindex][i  ]][2];
00176     cloud.push_back (p1);
00177     p2.x = vertex_list[triTable[cubeindex][i+1]][0];
00178     p2.y = vertex_list[triTable[cubeindex][i+1]][1];
00179     p2.z = vertex_list[triTable[cubeindex][i+1]][2];
00180     cloud.push_back (p2);
00181     p3.x = vertex_list[triTable[cubeindex][i+2]][0];
00182     p3.y = vertex_list[triTable[cubeindex][i+2]][1];
00183     p3.z = vertex_list[triTable[cubeindex][i+2]][2];
00184     cloud.push_back (p3);
00185   }
00186 }
00187 
00188 
00190 template <typename PointNT> void
00191 pcl::MarchingCubes<PointNT>::getNeighborList1D (std::vector<float> &leaf,
00192                                                 Eigen::Vector3i &index3d)
00193 {
00194   leaf = std::vector<float> (8, 0.0f);
00195 
00196   leaf[0] = getGridValue (index3d);
00197   leaf[1] = getGridValue (index3d + Eigen::Vector3i (1, 0, 0));
00198   leaf[2] = getGridValue (index3d + Eigen::Vector3i (1, 0, 1));
00199   leaf[3] = getGridValue (index3d + Eigen::Vector3i (0, 0, 1));
00200   leaf[4] = getGridValue (index3d + Eigen::Vector3i (0, 1, 0));
00201   leaf[5] = getGridValue (index3d + Eigen::Vector3i (1, 1, 0));
00202   leaf[6] = getGridValue (index3d + Eigen::Vector3i (1, 1, 1));
00203   leaf[7] = getGridValue (index3d + Eigen::Vector3i (0, 1, 1));
00204 }
00205 
00206 
00208 template <typename PointNT> float
00209 pcl::MarchingCubes<PointNT>::getGridValue (Eigen::Vector3i pos)
00210 {
00212   if (pos[0] < 0 || pos[0] >= res_x_)
00213     return -1.0f;
00214   if (pos[1] < 0 || pos[1] >= res_y_)
00215     return -1.0f;
00216   if (pos[2] < 0 || pos[2] >= res_z_)
00217     return -1.0f;
00218 
00219   return grid_[pos[0]*res_y_*res_z_ + pos[1]*res_z_ + pos[2]];
00220 }
00221 
00222 
00224 template <typename PointNT> void
00225 pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PolygonMesh &output)
00226 {
00227   if (!(iso_level_ >= 0 && iso_level_ < 1))
00228   {
00229     PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", getClassName ().c_str (), iso_level_);
00230     output.cloud.width = output.cloud.height = 0;
00231     output.cloud.data.clear ();
00232     output.polygons.clear ();
00233     return;
00234   }
00235 
00236   // Create grid
00237   grid_ = std::vector<float> (res_x_*res_y_*res_z_, 0.0f);
00238 
00239   // Populate tree
00240   tree_->setInputCloud (input_);
00241 
00242   getBoundingBox ();
00243 
00244   // Transform the point cloud into a voxel grid
00245   // This needs to be implemented in a child class
00246   voxelizeData ();
00247 
00248 
00249 
00250   // Run the actual marching cubes algorithm, store it into a point cloud,
00251   // and copy the point cloud + connectivity into output
00252   pcl::PointCloud<PointNT> cloud;
00253 
00254   for (int x = 1; x < res_x_-1; ++x)
00255     for (int y = 1; y < res_y_-1; ++y)
00256       for (int z = 1; z < res_z_-1; ++z)
00257       {
00258         Eigen::Vector3i index_3d (x, y, z);
00259         std::vector<float> leaf_node;
00260         getNeighborList1D (leaf_node, index_3d);
00261         createSurface (leaf_node, index_3d, cloud);
00262       }
00263   pcl::toPCLPointCloud2 (cloud, output.cloud);
00264 
00265   output.polygons.resize (cloud.size () / 3);
00266   for (size_t i = 0; i < output.polygons.size (); ++i)
00267   {
00268     pcl::Vertices v;
00269     v.vertices.resize (3);
00270     for (int j = 0; j < 3; ++j)
00271       v.vertices[j] = static_cast<int> (i) * 3 + j;
00272     output.polygons[i] = v;
00273   }
00274 }
00275 
00276 
00278 template <typename PointNT> void
00279 pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PointCloud<PointNT> &points,
00280                                                     std::vector<pcl::Vertices> &polygons)
00281 {
00282   if (!(iso_level_ >= 0 && iso_level_ < 1))
00283   {
00284     PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", getClassName ().c_str (), iso_level_);
00285     points.width = points.height = 0;
00286     points.points.clear ();
00287     polygons.clear ();
00288     return;
00289   }
00290 
00291   // Create grid
00292   grid_ = std::vector<float> (res_x_*res_y_*res_z_, 0.0f);
00293 
00294   // Populate tree
00295   tree_->setInputCloud (input_);
00296 
00297   getBoundingBox ();
00298 
00299   // Transform the point cloud into a voxel grid
00300   // This needs to be implemented in a child class
00301   voxelizeData ();
00302 
00303   // Run the actual marching cubes algorithm, store it into a point cloud,
00304   // and copy the point cloud + connectivity into output
00305   points.clear ();
00306   for (int x = 1; x < res_x_-1; ++x)
00307     for (int y = 1; y < res_y_-1; ++y)
00308       for (int z = 1; z < res_z_-1; ++z)
00309       {
00310         Eigen::Vector3i index_3d (x, y, z);
00311         std::vector<float> leaf_node;
00312         getNeighborList1D (leaf_node, index_3d);
00313         createSurface (leaf_node, index_3d, points);
00314       }
00315 
00316   polygons.resize (points.size () / 3);
00317   for (size_t i = 0; i < polygons.size (); ++i)
00318   {
00319     pcl::Vertices v;
00320     v.vertices.resize (3);
00321     for (int j = 0; j < 3; ++j)
00322       v.vertices[j] = static_cast<int> (i) * 3 + j;
00323     polygons[i] = v;
00324   }
00325 }
00326 
00327 
00328 
00329 #define PCL_INSTANTIATE_MarchingCubes(T) template class PCL_EXPORTS pcl::MarchingCubes<T>;
00330 
00331 #endif    // PCL_SURFACE_IMPL_MARCHING_CUBES_H_
00332 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:25:25