Point Cloud Library (PCL)  1.9.0-dev
marching_cubes.hpp
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 
45 //////////////////////////////////////////////////////////////////////////////////////////////
46 template <typename PointNT>
48 {
49 }
50 
51 //////////////////////////////////////////////////////////////////////////////////////////////
52 template <typename PointNT> void
54 {
55  PointNT max_pt, min_pt;
56  pcl::getMinMax3D (*input_, min_pt, max_pt);
57 
58  lower_boundary_ = min_pt.getArray3fMap ();
59  upper_boundary_ = max_pt.getArray3fMap ();
60 
61  const Eigen::Array3f size3_extend = 0.5f * percentage_extend_grid_
62  * (upper_boundary_ - lower_boundary_);
63 
64  lower_boundary_ -= size3_extend;
65  upper_boundary_ += size3_extend;
66 }
67 
68 
69 //////////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointNT> void
72  Eigen::Vector3f &p2,
73  float val_p1,
74  float val_p2,
75  Eigen::Vector3f &output)
76 {
77  const float mu = (iso_level_ - val_p1) / (val_p2 - val_p1);
78  output = p1 + mu * (p2 - p1);
79 }
80 
81 
82 //////////////////////////////////////////////////////////////////////////////////////////////
83 template <typename PointNT> void
84 pcl::MarchingCubes<PointNT>::createSurface (const std::vector<float> &leaf_node,
85  const Eigen::Vector3i &index_3d,
87 {
88  int cubeindex = 0;
89  if (leaf_node[0] < iso_level_) cubeindex |= 1;
90  if (leaf_node[1] < iso_level_) cubeindex |= 2;
91  if (leaf_node[2] < iso_level_) cubeindex |= 4;
92  if (leaf_node[3] < iso_level_) cubeindex |= 8;
93  if (leaf_node[4] < iso_level_) cubeindex |= 16;
94  if (leaf_node[5] < iso_level_) cubeindex |= 32;
95  if (leaf_node[6] < iso_level_) cubeindex |= 64;
96  if (leaf_node[7] < iso_level_) cubeindex |= 128;
97 
98  // Cube is entirely in/out of the surface
99  if (edgeTable[cubeindex] == 0)
100  return;
101 
102  const Eigen::Vector3f center = lower_boundary_
103  + size_voxel_ * index_3d.cast<float> ().array ();
104 
105  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > p;
106  p.resize (8);
107  for (int i = 0; i < 8; ++i)
108  {
109  Eigen::Vector3f point = center;
110  if (i & 0x4)
111  point[1] = static_cast<float> (center[1] + size_voxel_[1]);
112 
113  if (i & 0x2)
114  point[2] = static_cast<float> (center[2] + size_voxel_[2]);
115 
116  if ((i & 0x1) ^ ((i >> 1) & 0x1))
117  point[0] = static_cast<float> (center[0] + size_voxel_[0]);
118 
119  p[i] = point;
120  }
121 
122  // Find the vertices where the surface intersects the cube
123  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > vertex_list;
124  vertex_list.resize (12);
125  if (edgeTable[cubeindex] & 1)
126  interpolateEdge (p[0], p[1], leaf_node[0], leaf_node[1], vertex_list[0]);
127  if (edgeTable[cubeindex] & 2)
128  interpolateEdge (p[1], p[2], leaf_node[1], leaf_node[2], vertex_list[1]);
129  if (edgeTable[cubeindex] & 4)
130  interpolateEdge (p[2], p[3], leaf_node[2], leaf_node[3], vertex_list[2]);
131  if (edgeTable[cubeindex] & 8)
132  interpolateEdge (p[3], p[0], leaf_node[3], leaf_node[0], vertex_list[3]);
133  if (edgeTable[cubeindex] & 16)
134  interpolateEdge (p[4], p[5], leaf_node[4], leaf_node[5], vertex_list[4]);
135  if (edgeTable[cubeindex] & 32)
136  interpolateEdge (p[5], p[6], leaf_node[5], leaf_node[6], vertex_list[5]);
137  if (edgeTable[cubeindex] & 64)
138  interpolateEdge (p[6], p[7], leaf_node[6], leaf_node[7], vertex_list[6]);
139  if (edgeTable[cubeindex] & 128)
140  interpolateEdge (p[7], p[4], leaf_node[7], leaf_node[4], vertex_list[7]);
141  if (edgeTable[cubeindex] & 256)
142  interpolateEdge (p[0], p[4], leaf_node[0], leaf_node[4], vertex_list[8]);
143  if (edgeTable[cubeindex] & 512)
144  interpolateEdge (p[1], p[5], leaf_node[1], leaf_node[5], vertex_list[9]);
145  if (edgeTable[cubeindex] & 1024)
146  interpolateEdge (p[2], p[6], leaf_node[2], leaf_node[6], vertex_list[10]);
147  if (edgeTable[cubeindex] & 2048)
148  interpolateEdge (p[3], p[7], leaf_node[3], leaf_node[7], vertex_list[11]);
149 
150  // Create the triangle
151  for (int i = 0; triTable[cubeindex][i] != -1; i += 3)
152  {
153  PointNT p1, p2, p3;
154  p1.getVector3fMap () = vertex_list[triTable[cubeindex][i]];
155  cloud.push_back (p1);
156  p2.getVector3fMap () = vertex_list[triTable[cubeindex][i+1]];
157  cloud.push_back (p2);
158  p3.getVector3fMap () = vertex_list[triTable[cubeindex][i+2]];
159  cloud.push_back (p3);
160  }
161 }
162 
163 
164 //////////////////////////////////////////////////////////////////////////////////////////////
165 template <typename PointNT> void
167  Eigen::Vector3i &index3d)
168 {
169  leaf.resize (8);
170 
171  leaf[0] = getGridValue (index3d);
172  leaf[1] = getGridValue (index3d + Eigen::Vector3i (1, 0, 0));
173  leaf[2] = getGridValue (index3d + Eigen::Vector3i (1, 0, 1));
174  leaf[3] = getGridValue (index3d + Eigen::Vector3i (0, 0, 1));
175  leaf[4] = getGridValue (index3d + Eigen::Vector3i (0, 1, 0));
176  leaf[5] = getGridValue (index3d + Eigen::Vector3i (1, 1, 0));
177  leaf[6] = getGridValue (index3d + Eigen::Vector3i (1, 1, 1));
178  leaf[7] = getGridValue (index3d + Eigen::Vector3i (0, 1, 1));
179 
180  for (int i = 0; i < 8; ++i)
181  {
182  if (std::isnan (leaf[i]))
183  {
184  leaf.clear ();
185  break;
186  }
187  }
188 }
189 
190 
191 //////////////////////////////////////////////////////////////////////////////////////////////
192 template <typename PointNT> float
194 {
195  /// TODO what to return?
196  if (pos[0] < 0 || pos[0] >= res_x_)
197  return -1.0f;
198  if (pos[1] < 0 || pos[1] >= res_y_)
199  return -1.0f;
200  if (pos[2] < 0 || pos[2] >= res_z_)
201  return -1.0f;
202 
203  return grid_[pos[0]*res_y_*res_z_ + pos[1]*res_z_ + pos[2]];
204 }
205 
206 
207 //////////////////////////////////////////////////////////////////////////////////////////////
208 template <typename PointNT> void
210 {
212 
213  performReconstruction (points, output.polygons);
214 
215  pcl::toPCLPointCloud2 (points, output.cloud);
216 }
217 
218 
219 //////////////////////////////////////////////////////////////////////////////////////////////
220 template <typename PointNT> void
222  std::vector<pcl::Vertices> &polygons)
223 {
224  if (!(iso_level_ >= 0 && iso_level_ < 1))
225  {
226  PCL_ERROR ("[pcl::%s::performReconstruction] Invalid iso level %f! Please use a number between 0 and 1.\n",
227  getClassName ().c_str (), iso_level_);
228  points.width = points.height = 0;
229  points.points.clear ();
230  polygons.clear ();
231  return;
232  }
233 
234  // the point cloud really generated from Marching Cubes, prev intermediate_cloud_
235  pcl::PointCloud<PointNT> intermediate_cloud;
236 
237  // Create grid
238  grid_ = std::vector<float> (res_x_*res_y_*res_z_, NAN);
239 
240  // Populate tree
241  tree_->setInputCloud (input_);
242 
243  // Compute bounding box and voxel size
244  getBoundingBox ();
245  size_voxel_ = (upper_boundary_ - lower_boundary_)
246  * Eigen::Array3f (res_x_, res_y_, res_z_).inverse ();
247 
248  // Transform the point cloud into a voxel grid
249  // This needs to be implemented in a child class
250  voxelizeData ();
251 
252  // preallocate memory assuming a hull. suppose 6 point per voxel
253  double size_reserve = std::min((double) intermediate_cloud.points.max_size (),
254  2.0 * 6.0 * (double) (res_y_*res_z_ + res_x_*res_z_ + res_x_*res_y_));
255  intermediate_cloud.reserve ((size_t) size_reserve);
256 
257  for (int x = 1; x < res_x_-1; ++x)
258  for (int y = 1; y < res_y_-1; ++y)
259  for (int z = 1; z < res_z_-1; ++z)
260  {
261  Eigen::Vector3i index_3d (x, y, z);
262  std::vector<float> leaf_node;
263  getNeighborList1D (leaf_node, index_3d);
264  if (!leaf_node.empty ())
265  createSurface (leaf_node, index_3d, intermediate_cloud);
266  }
267 
268  points.swap (intermediate_cloud);
269 
270  polygons.resize (points.size () / 3);
271  for (size_t i = 0; i < polygons.size (); ++i)
272  {
273  pcl::Vertices v;
274  v.vertices.resize (3);
275  for (int j = 0; j < 3; ++j)
276  v.vertices[j] = static_cast<int> (i) * 3 + j;
277  polygons[i] = v;
278  }
279 }
280 
281 #define PCL_INSTANTIATE_MarchingCubes(T) template class PCL_EXPORTS pcl::MarchingCubes<T>;
282 
283 #endif // PCL_SURFACE_IMPL_MARCHING_CUBES_H_
284 
size_t size() const
Definition: point_cloud.h:448
void reserve(size_t n)
Definition: point_cloud.h:449
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
std::vector< uint32_t > vertices
Definition: Vertices.h:19
void getNeighborList1D(std::vector< float > &leaf, Eigen::Vector3i &index3d)
Method that returns the scalar values of the neighbors of a given 3D position in the grid...
void getBoundingBox()
Get the bounding box for the input data points.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:480
virtual void performReconstruction(pcl::PolygonMesh &output)
Extract the surface.
void createSurface(const std::vector< float > &leaf_node, const Eigen::Vector3i &index_3d, pcl::PointCloud< PointNT > &cloud)
Calculate out the corresponding polygons in the leaf node.
void interpolateEdge(Eigen::Vector3f &p1, Eigen::Vector3f &p2, float val_p1, float val_p2, Eigen::Vector3f &output)
Interpolate along the voxel edge.
virtual ~MarchingCubes()
Destructor.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
Definition: Vertices.h:14
const unsigned int edgeTable[256]
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
void swap(PointCloud< PointT > &rhs)
Swap a point cloud with another cloud.
Definition: point_cloud.h:563
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud...
Definition: common.hpp:242
std::vector< ::pcl::Vertices > polygons
Definition: PolygonMesh.h:24
::pcl::PCLPointCloud2 cloud
Definition: PolygonMesh.h:22
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
Definition: conversions.h:242
const int triTable[256][16]
virtual float getGridValue(Eigen::Vector3i pos)
Method that returns the scalar value at the given grid position.