Point Cloud Library (PCL)  1.3.1
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 {}
00049 
00051 template <typename PointNT>
00052 pcl::MarchingCubes<PointNT>::~MarchingCubes ()
00053 {
00054 
00055 }
00056 
00058 template <typename PointNT> void
00059 pcl::MarchingCubes<PointNT>::getBoundingBox ()
00060 {
00061   pcl::getMinMax3D (*input_, min_p_, max_p_);
00062 
00063   Eigen::Vector4f bounding_box_size = max_p_ - min_p_;
00064 
00065   bounding_box_size = max_p_ - min_p_;
00066   PCL_DEBUG ("[pcl::MarchingCubes::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n",
00067       bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ());
00068   double max_size =
00069     (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()),
00070                 bounding_box_size.z ());
00071 
00072   data_size_ = max_size / leaf_size_;
00073   PCL_DEBUG ("[pcl::MarchingCubes::getBoundingBox] Lower left point is [%f, %f, %f]\n",
00074       min_p_.x (), min_p_.y (), min_p_.z ());
00075   PCL_DEBUG ("[pcl::MarchingCubes::getBoundingBox] Upper left point is [%f, %f, %f]\n",
00076       max_p_.x (), max_p_.y (), max_p_.z ());
00077   PCL_DEBUG ("[pcl::MarchingCubes::getBoundingBox] Padding size: %d\n", padding_size_);
00078   PCL_DEBUG ("[pcl::MarchingCubes::getBoundingBox] Leaf size: %f\n", leaf_size_);
00079 
00080   gaussian_scale_ = pow ((padding_size_+1) * leaf_size_ / 2.0, 2.0);
00081 }
00082 
00084 template <typename PointNT> void
00085 pcl::MarchingCubes<PointNT>::interpolateEdge(float iso_level, Eigen::Vector3f &p1, Eigen::Vector3f &p2, float val_p1, float val_p2, Eigen::Vector3f &output)
00086 {
00087   if(iso_level - val_p1 < 0.00001)
00088     output = p1;
00089   if(iso_level - val_p2 < 0.00001)
00090     output = p2;
00091   if(val_p1 - val_p2 < 0.00001)
00092     output = p1;
00093 
00094   float mu = (iso_level - val_p1) / (val_p2-val_p1);
00095   output[0] = p1[0] + mu * (p2[0] - p1[0]);
00096   output[1] = p1[1] + mu * (p2[1] - p1[1]);
00097   output[2] = p1[2] + mu * (p2[2] - p1[2]);
00098 }
00099 
00101 template <typename PointNT> void
00102 pcl::MarchingCubes<PointNT>::createSurface (Leaf leaf_node, Eigen::Vector3i &index_3d, pcl::PointCloud<pcl::PointXYZ> &cloud, float iso_level)
00103 {
00104   int cubeindex = 0;
00105   Eigen::Vector3f vertex_list[12];
00106   if (leaf_node.vertex[0] < iso_level) cubeindex |= 1;
00107   if (leaf_node.vertex[1] < iso_level) cubeindex |= 2;
00108   if (leaf_node.vertex[2] < iso_level) cubeindex |= 4;
00109   if (leaf_node.vertex[3] < iso_level) cubeindex |= 8;
00110   if (leaf_node.vertex[4] < iso_level) cubeindex |= 16;
00111   if (leaf_node.vertex[5] < iso_level) cubeindex |= 32;
00112   if (leaf_node.vertex[6] < iso_level) cubeindex |= 64;
00113   if (leaf_node.vertex[7] < iso_level) cubeindex |= 128;
00114 
00115   // Cube is entirely in/out of the surface
00116   if (edgeTable[cubeindex] == 0)
00117      return;
00118 
00119   std::vector<Eigen::Vector3f> p;
00120   p.resize(8);
00121   Eigen::Vector4f center;
00122   getCellCenterFromIndex(index_3d, center);
00123   for( int i = 0; i < 8; ++i )
00124   {
00125     Eigen::Vector3f point;
00126     if(i & 0x4)
00127       point[1] = center[1] + leaf_size_/2;
00128     else
00129       point[1] = center[1] - leaf_size_/2;
00130 
00131     if(i & 0x2)
00132       point[2] = center[2] + leaf_size_/2;
00133     else
00134       point[2] = center[2] - leaf_size_/2;
00135 
00136     if((i & 0x1) ^ ((i >> 1) & 0x1))
00137       point[0] = center[0] + leaf_size_/2;
00138     else
00139       point[0] = center[0] - leaf_size_/2;
00140 
00141     p[i] = point;
00142   }
00143 
00144   // Find the vertices where the surface intersects the cube
00145   if (edgeTable[cubeindex] & 1)
00146     interpolateEdge(iso_level,p[0],p[1],leaf_node.vertex[0],leaf_node.vertex[1], vertex_list[0]);
00147   if (edgeTable[cubeindex] & 2)
00148     interpolateEdge(iso_level,p[1],p[2],leaf_node.vertex[1],leaf_node.vertex[2], vertex_list[1]);
00149   if (edgeTable[cubeindex] & 4)
00150     interpolateEdge(iso_level,p[2],p[3],leaf_node.vertex[2],leaf_node.vertex[3], vertex_list[2]);
00151   if (edgeTable[cubeindex] & 8)
00152     interpolateEdge(iso_level,p[3],p[0],leaf_node.vertex[3],leaf_node.vertex[0], vertex_list[3]);
00153   if (edgeTable[cubeindex] & 16)
00154     interpolateEdge(iso_level,p[4],p[5],leaf_node.vertex[4],leaf_node.vertex[5], vertex_list[4]);
00155   if (edgeTable[cubeindex] & 32)
00156     interpolateEdge(iso_level,p[5],p[6],leaf_node.vertex[5],leaf_node.vertex[6], vertex_list[5]);
00157   if (edgeTable[cubeindex] & 64)
00158     interpolateEdge(iso_level,p[6],p[7],leaf_node.vertex[6],leaf_node.vertex[7], vertex_list[6]);
00159   if (edgeTable[cubeindex] & 128)
00160     interpolateEdge(iso_level,p[7],p[4],leaf_node.vertex[7],leaf_node.vertex[4], vertex_list[7]);
00161   if (edgeTable[cubeindex] & 256)
00162     interpolateEdge(iso_level,p[0],p[4],leaf_node.vertex[0],leaf_node.vertex[4], vertex_list[8]);
00163   if (edgeTable[cubeindex] & 512)
00164     interpolateEdge(iso_level,p[1],p[5],leaf_node.vertex[1],leaf_node.vertex[5], vertex_list[9]);
00165   if (edgeTable[cubeindex] & 1024)
00166     interpolateEdge(iso_level,p[2],p[6],leaf_node.vertex[2],leaf_node.vertex[6], vertex_list[10]);
00167   if (edgeTable[cubeindex] & 2048)
00168     interpolateEdge(iso_level,p[3],p[7],leaf_node.vertex[3],leaf_node.vertex[7], vertex_list[11]);
00169 
00170   // Create the triangle
00171   for (int i=0;triTable[cubeindex][i]!=-1;i+=3) {
00172     pcl::PointXYZ 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 
00189 template <typename PointNT> void
00190 pcl::MarchingCubes<PointNT>::getNeighborList1D (Leaf leaf, Eigen::Vector3i &index3d, HashMap &neighbor_list)
00191 {
00192   for (int x = -1; x < 2; ++x)
00193   {
00194     for (int y = -1; y < 2; ++y)
00195     {
00196       for (int z = -1; z < 2; ++z)
00197       {
00198         if(x == 0 && y == 0 && z == 0)
00199           continue;
00200 
00201         if(index3d[0] == 0 && x == -1)
00202           continue;
00203         if(index3d[1] == 0 && y == -1)
00204           continue;
00205         if(index3d[2] == 0 && z == -1)
00206           continue;
00207 
00208         Eigen::Vector3i neighbor_index;
00209         neighbor_index[0] = index3d[0] + x;
00210         neighbor_index[1] = index3d[1] + y;
00211         neighbor_index[2] = index3d[2] + z;
00212         Leaf neighbor_leaf;
00213 
00214         if( (x == 0 || x == 1) &&
00215             (y == 0 || y == 1) &&
00216             (z == 0 || z == 1)
00217           )
00218         {
00219           neighbor_leaf.vertex[0] = leaf.vertex[0];
00220         }
00221         if( (x == 0 || x == -1) &&
00222             (y == 0 || y == 1) &&
00223             (z == 0 || z == 1)
00224           )
00225         {
00226           neighbor_leaf.vertex[1] = leaf.vertex[1];
00227         }
00228         if( (x == 0 || x == -1) &&
00229             (y == 0 || y == 1) &&
00230             (z == 0 || z == -1)
00231           )
00232         {
00233           neighbor_leaf.vertex[2] = leaf.vertex[2];
00234         }
00235         if( (x == 0 || x == 1) &&
00236             (y == 0 || y == 1) &&
00237             (z == 0 || z == -1)
00238           )
00239         {
00240           neighbor_leaf.vertex[3] = leaf.vertex[3];
00241         }
00242 
00243         if( (x == 0 || x == 1) &&
00244             (y == 0 || y == -1) &&
00245             (z == 0 || z == 1)
00246           )
00247         {
00248           neighbor_leaf.vertex[4] = leaf.vertex[4];
00249         }
00250         if( (x == 0 || x == -1) &&
00251             (y == 0 || y == -1) &&
00252             (z == 0 || z == 1)
00253           )
00254         {
00255           neighbor_leaf.vertex[5] = leaf.vertex[5];
00256         }
00257         if( (x == 0 || x == -1) &&
00258             (y == 0 || y == -1) &&
00259             (z == 0 || z == -1)
00260           )
00261         {
00262           neighbor_leaf.vertex[6] = leaf.vertex[6];
00263         }
00264         if( (x == 0 || x == 1) &&
00265             (y == 0 || y == -1) &&
00266             (z == 0 || z == -1)
00267           )
00268         {
00269           neighbor_leaf.vertex[7] = leaf.vertex[7];
00270         }
00271 
00272         neighbor_list[getIndexIn1D(neighbor_index)] = neighbor_leaf;
00273       }
00274     }
00275   }
00276 }
00277 
00279 template <typename PointNT> void
00280 pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PolygonMesh &output)
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     output.cloud.width = output.cloud.height = 0;
00286     output.cloud.data.clear ();
00287     output.polygons.clear ();
00288     return;
00289   }
00290   if( leaf_size_ <=0 )
00291   {
00292     PCL_ERROR ("[pcl::%s::performReconstruction] Invalid leaf size %f! Please use a number greater than 0.\n", getClassName ().c_str (), leaf_size_);
00293     output.cloud.width = output.cloud.height = 0;
00294     output.cloud.data.clear ();
00295     output.polygons.clear ();
00296     return;
00297     
00298   }
00299 
00300   getBoundingBox();
00301 
00302   // transform the point cloud into a voxel grid
00303   // this needs to be implemented in a child class
00304   voxelizeData();
00305 
00306   // run the actual marching cubes algorithm, store it into a point cloud,
00307   // and copy the point cloud + connectivity into output
00308   pcl::PointCloud<pcl::PointXYZ> cloud;
00309   BOOST_FOREACH (typename HashMap::value_type entry, cell_hash_map_)
00310   {
00311     Eigen::Vector3i leaf_index;
00312     getIndexIn3D (entry.first, leaf_index);
00313     createSurface (entry.second, leaf_index, cloud, iso_level_);
00314   }
00315   pcl::toROSMsg (cloud, output.cloud);
00316   output.polygons.resize (cloud.size () / 3);
00317 
00318   for (size_t i = 0; i < output.polygons.size (); ++i)
00319   {
00320     pcl::Vertices v;
00321     v.vertices.resize (3);
00322     for (int j = 0; j < 3; ++j)
00323       v.vertices[j] = i*3+j;
00324      output.polygons[i] = v;
00325   }
00326 }
00327 
00328 #define PCL_INSTANTIATE_MarchingCubes(T) template class PCL_EXPORTS pcl::MarchingCubes<T>;
00329 
00330 #endif    // PCL_SURFACE_IMPL_MARCHING_CUBES_H_
00331 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines