Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
marching_cubes.hpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  */
35 
36 #ifndef PCL_SURFACE_IMPL_MARCHING_CUBES_H_
37 #define PCL_SURFACE_IMPL_MARCHING_CUBES_H_
38 
39 #include <pcl/surface/marching_cubes.h>
40 #include <pcl/common/common.h>
41 #include <pcl/common/vector_average.h>
42 #include <pcl/Vertices.h>
43 #include <pcl/kdtree/kdtree_flann.h>
44 
46 template <typename PointNT>
47 pcl::MarchingCubes<PointNT>::MarchingCubes ()
48 : min_p_ (), max_p_ (), percentage_extend_grid_ (), iso_level_ ()
49 {
50 }
51 
53 template <typename PointNT>
54 pcl::MarchingCubes<PointNT>::~MarchingCubes ()
55 {
56 }
57 
59 template <typename PointNT> void
60 pcl::MarchingCubes<PointNT>::getBoundingBox ()
61 {
62  pcl::getMinMax3D (*input_, min_p_, max_p_);
63 
64  min_p_ -= (max_p_ - min_p_) * percentage_extend_grid_/2;
65  max_p_ += (max_p_ - min_p_) * percentage_extend_grid_/2;
66 
67  Eigen::Vector4f bounding_box_size = max_p_ - min_p_;
68 
69  bounding_box_size = max_p_ - min_p_;
70  PCL_DEBUG ("[pcl::MarchingCubesHoppe::getBoundingBox] Size of Bounding Box is [%f, %f, %f]\n",
71  bounding_box_size.x (), bounding_box_size.y (), bounding_box_size.z ());
72  double max_size =
73  (std::max) ((std::max)(bounding_box_size.x (), bounding_box_size.y ()),
74  bounding_box_size.z ());
75 
76  // data_size_ = static_cast<uint64_t> (max_size / leaf_size_);
77  PCL_DEBUG ("[pcl::MarchingCubesHoppe::getBoundingBox] Lower left point is [%f, %f, %f]\n",
78  min_p_.x (), min_p_.y (), min_p_.z ());
79  PCL_DEBUG ("[pcl::MarchingCubesHoppe::getBoundingBox] Upper left point is [%f, %f, %f]\n",
80  max_p_.x (), max_p_.y (), max_p_.z ());
81 }
82 
83 
85 template <typename PointNT> void
86 pcl::MarchingCubes<PointNT>::interpolateEdge (Eigen::Vector3f &p1,
87  Eigen::Vector3f &p2,
88  float val_p1,
89  float val_p2,
90  Eigen::Vector3f &output)
91 {
92  float mu = (iso_level_ - val_p1) / (val_p2-val_p1);
93  output = p1 + mu * (p2 - p1);
94 }
95 
96 
98 template <typename PointNT> void
99 pcl::MarchingCubes<PointNT>::createSurface (std::vector<float> &leaf_node,
100  Eigen::Vector3i &index_3d,
101  pcl::PointCloud<PointNT> &cloud)
102 {
103  int cubeindex = 0;
104  Eigen::Vector3f vertex_list[12];
105  if (leaf_node[0] < iso_level_) cubeindex |= 1;
106  if (leaf_node[1] < iso_level_) cubeindex |= 2;
107  if (leaf_node[2] < iso_level_) cubeindex |= 4;
108  if (leaf_node[3] < iso_level_) cubeindex |= 8;
109  if (leaf_node[4] < iso_level_) cubeindex |= 16;
110  if (leaf_node[5] < iso_level_) cubeindex |= 32;
111  if (leaf_node[6] < iso_level_) cubeindex |= 64;
112  if (leaf_node[7] < iso_level_) cubeindex |= 128;
113 
114  // Cube is entirely in/out of the surface
115  if (edgeTable[cubeindex] == 0)
116  return;
117 
118  //Eigen::Vector4f index_3df (index_3d[0], index_3d[1], index_3d[2], 0.0f);
119  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_);
120  center[0] = min_p_[0] + (max_p_[0] - min_p_[0]) * index_3d[0] / res_x_;
121  center[1] = min_p_[1] + (max_p_[1] - min_p_[1]) * index_3d[1] / res_y_;
122  center[2] = min_p_[2] + (max_p_[2] - min_p_[2]) * index_3d[2] / res_z_;
123 
124  std::vector<Eigen::Vector3f> p;
125  p.resize (8);
126  for (int i = 0; i < 8; ++i)
127  {
128  Eigen::Vector3f point = center;
129  if(i & 0x4)
130  point[1] = static_cast<float> (center[1] + (max_p_[1] - min_p_[1]) / float (res_y_));
131 
132  if(i & 0x2)
133  point[2] = static_cast<float> (center[2] + (max_p_[2] - min_p_[2]) / float (res_z_));
134 
135  if((i & 0x1) ^ ((i >> 1) & 0x1))
136  point[0] = static_cast<float> (center[0] + (max_p_[0] - min_p_[0]) / float (res_x_));
137 
138  p[i] = point;
139  }
140 
141 
142  // Find the vertices where the surface intersects the cube
143  if (edgeTable[cubeindex] & 1)
144  interpolateEdge (p[0], p[1], leaf_node[0], leaf_node[1], vertex_list[0]);
145  if (edgeTable[cubeindex] & 2)
146  interpolateEdge (p[1], p[2], leaf_node[1], leaf_node[2], vertex_list[1]);
147  if (edgeTable[cubeindex] & 4)
148  interpolateEdge (p[2], p[3], leaf_node[2], leaf_node[3], vertex_list[2]);
149  if (edgeTable[cubeindex] & 8)
150  interpolateEdge (p[3], p[0], leaf_node[3], leaf_node[0], vertex_list[3]);
151  if (edgeTable[cubeindex] & 16)
152  interpolateEdge (p[4], p[5], leaf_node[4], leaf_node[5], vertex_list[4]);
153  if (edgeTable[cubeindex] & 32)
154  interpolateEdge (p[5], p[6], leaf_node[5], leaf_node[6], vertex_list[5]);
155  if (edgeTable[cubeindex] & 64)
156  interpolateEdge (p[6], p[7], leaf_node[6], leaf_node[7], vertex_list[6]);
157  if (edgeTable[cubeindex] & 128)
158  interpolateEdge (p[7], p[4], leaf_node[7], leaf_node[4], vertex_list[7]);
159  if (edgeTable[cubeindex] & 256)
160  interpolateEdge (p[0], p[4], leaf_node[0], leaf_node[4], vertex_list[8]);
161  if (edgeTable[cubeindex] & 512)
162  interpolateEdge (p[1], p[5], leaf_node[1], leaf_node[5], vertex_list[9]);
163  if (edgeTable[cubeindex] & 1024)
164  interpolateEdge (p[2], p[6], leaf_node[2], leaf_node[6], vertex_list[10]);
165  if (edgeTable[cubeindex] & 2048)
166  interpolateEdge (p[3], p[7], leaf_node[3], leaf_node[7], vertex_list[11]);
167 
168  // Create the triangle
169  for (int i = 0; triTable[cubeindex][i] != -1; i+=3)
170  {
171  PointNT p1,p2,p3;
172  p1.x = vertex_list[triTable[cubeindex][i ]][0];
173  p1.y = vertex_list[triTable[cubeindex][i ]][1];
174  p1.z = vertex_list[triTable[cubeindex][i ]][2];
175  cloud.push_back (p1);
176  p2.x = vertex_list[triTable[cubeindex][i+1]][0];
177  p2.y = vertex_list[triTable[cubeindex][i+1]][1];
178  p2.z = vertex_list[triTable[cubeindex][i+1]][2];
179  cloud.push_back (p2);
180  p3.x = vertex_list[triTable[cubeindex][i+2]][0];
181  p3.y = vertex_list[triTable[cubeindex][i+2]][1];
182  p3.z = vertex_list[triTable[cubeindex][i+2]][2];
183  cloud.push_back (p3);
184  }
185 }
186 
187 
189 template <typename PointNT> void
190 pcl::MarchingCubes<PointNT>::getNeighborList1D (std::vector<float> &leaf,
191  Eigen::Vector3i &index3d)
192 {
193  leaf = std::vector<float> (8, 0.0f);
194 
195  leaf[0] = getGridValue (index3d);
196  leaf[1] = getGridValue (index3d + Eigen::Vector3i (1, 0, 0));
197  leaf[2] = getGridValue (index3d + Eigen::Vector3i (1, 0, 1));
198  leaf[3] = getGridValue (index3d + Eigen::Vector3i (0, 0, 1));
199  leaf[4] = getGridValue (index3d + Eigen::Vector3i (0, 1, 0));
200  leaf[5] = getGridValue (index3d + Eigen::Vector3i (1, 1, 0));
201  leaf[6] = getGridValue (index3d + Eigen::Vector3i (1, 1, 1));
202  leaf[7] = getGridValue (index3d + Eigen::Vector3i (0, 1, 1));
203 }
204 
205 
207 template <typename PointNT> float
208 pcl::MarchingCubes<PointNT>::getGridValue (Eigen::Vector3i pos)
209 {
211  if (pos[0] < 0 || pos[0] >= res_x_)
212  return -1.0f;
213  if (pos[1] < 0 || pos[1] >= res_y_)
214  return -1.0f;
215  if (pos[2] < 0 || pos[2] >= res_z_)
216  return -1.0f;
217 
218  return grid_[pos[0]*res_y_*res_z_ + pos[1]*res_z_ + pos[2]];
219 }
220 
221 
223 template <typename PointNT> void
224 pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PolygonMesh &output)
225 {
226  if (!(iso_level_ >= 0 && iso_level_ < 1))
227  {
228  PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", getClassName ().c_str (), iso_level_);
229  output.cloud.width = output.cloud.height = 0;
230  output.cloud.data.clear ();
231  output.polygons.clear ();
232  return;
233  }
234 
235  // Create grid
236  grid_ = std::vector<float> (res_x_*res_y_*res_z_, 0.0f);
237 
238  // Populate tree
239  tree_->setInputCloud (input_);
240 
241  getBoundingBox ();
242 
243  // Transform the point cloud into a voxel grid
244  // This needs to be implemented in a child class
245  voxelizeData ();
246 
247 
248 
249  // Run the actual marching cubes algorithm, store it into a point cloud,
250  // and copy the point cloud + connectivity into output
251  pcl::PointCloud<PointNT> cloud;
252 
253  for (int x = 1; x < res_x_-1; ++x)
254  for (int y = 1; y < res_y_-1; ++y)
255  for (int z = 1; z < res_z_-1; ++z)
256  {
257  Eigen::Vector3i index_3d (x, y, z);
258  std::vector<float> leaf_node;
259  getNeighborList1D (leaf_node, index_3d);
260  createSurface (leaf_node, index_3d, cloud);
261  }
262  pcl::toROSMsg (cloud, output.cloud);
263 
264  output.polygons.resize (cloud.size () / 3);
265  for (size_t i = 0; i < output.polygons.size (); ++i)
266  {
267  pcl::Vertices v;
268  v.vertices.resize (3);
269  for (int j = 0; j < 3; ++j)
270  v.vertices[j] = static_cast<int> (i) * 3 + j;
271  output.polygons[i] = v;
272  }
273 }
274 
275 
277 template <typename PointNT> void
278 pcl::MarchingCubes<PointNT>::performReconstruction (pcl::PointCloud<PointNT> &points,
279  std::vector<pcl::Vertices> &polygons)
280 {
281  if (!(iso_level_ >= 0 && iso_level_ < 1))
282  {
283  PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n", getClassName ().c_str (), iso_level_);
284  points.width = points.height = 0;
285  points.points.clear ();
286  polygons.clear ();
287  return;
288  }
289 
290  // Create grid
291  grid_ = std::vector<float> (res_x_*res_y_*res_z_, 0.0f);
292 
293  // Populate tree
294  tree_->setInputCloud (input_);
295 
296  getBoundingBox ();
297 
298  // Transform the point cloud into a voxel grid
299  // This needs to be implemented in a child class
300  voxelizeData ();
301 
302  // Run the actual marching cubes algorithm, store it into a point cloud,
303  // and copy the point cloud + connectivity into output
304  points.clear ();
305  for (int x = 1; x < res_x_-1; ++x)
306  for (int y = 1; y < res_y_-1; ++y)
307  for (int z = 1; z < res_z_-1; ++z)
308  {
309  Eigen::Vector3i index_3d (x, y, z);
310  std::vector<float> leaf_node;
311  getNeighborList1D (leaf_node, index_3d);
312  createSurface (leaf_node, index_3d, points);
313  }
314 
315  polygons.resize (points.size () / 3);
316  for (size_t i = 0; i < polygons.size (); ++i)
317  {
318  pcl::Vertices v;
319  v.vertices.resize (3);
320  for (int j = 0; j < 3; ++j)
321  v.vertices[j] = static_cast<int> (i) * 3 + j;
322  polygons[i] = v;
323  }
324 }
325 
326 
327 
328 #define PCL_INSTANTIATE_MarchingCubes(T) template class PCL_EXPORTS pcl::MarchingCubes<T>;
329 
330 #endif // PCL_SURFACE_IMPL_MARCHING_CUBES_H_
331 
Generated on Sat Aug 24 2013 17:28:41 for Point Cloud Library (PCL) by   doxygen 1.8.4