Point Cloud Library (PCL)  1.14.1-dev
poisson.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #ifndef PCL_SURFACE_IMPL_POISSON_H_
41 #define PCL_SURFACE_IMPL_POISSON_H_
42 
43 #include <pcl/surface/poisson.h>
44 #include <pcl/common/common.h>
45 #include <pcl/common/vector_average.h>
46 #include <pcl/Vertices.h>
47 
48 #include <pcl/surface/3rdparty/poisson4/octree_poisson.h>
49 #include <pcl/surface/3rdparty/poisson4/sparse_matrix.h>
50 #include <pcl/surface/3rdparty/poisson4/function_data.h>
51 #include <pcl/surface/3rdparty/poisson4/ppolynomial.h>
52 #include <pcl/surface/3rdparty/poisson4/multi_grid_octree_data.h>
53 #include <pcl/surface/3rdparty/poisson4/geometry.h>
54 
55 #define MEMORY_ALLOCATOR_BLOCK_SIZE 1<<12
56 
57 #include <cstdarg>
58 
59 using namespace pcl;
60 
61 //////////////////////////////////////////////////////////////////////////////////////////////
62 template <typename PointNT>
64 
65 //////////////////////////////////////////////////////////////////////////////////////////////
66 template <typename PointNT>
68 
69 //////////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointNT> void
72 {
73  if (threads == 0)
74 #ifdef _OPENMP
75  threads_ = omp_get_num_procs();
76 #else
77  threads_ = 1;
78 #endif
79  else
80  threads_ = threads;
81 }
82 
83 //////////////////////////////////////////////////////////////////////////////////////////////
84 template <typename PointNT> template <int Degree> void
87  float &scale)
88 {
89  pcl::poisson::Real iso_value = 0;
92 
93 
94  tree.threads = threads_;
95  center.coords[0] = center.coords[1] = center.coords[2] = 0;
96 
97 
98  if (solver_divide_ < min_depth_)
99  {
100  PCL_WARN ("[pcl::Poisson] solver_divide_ must be at least as large as min_depth_: %d >= %d\n", solver_divide_, min_depth_);
101  solver_divide_ = min_depth_;
102  }
103  if (iso_divide_< min_depth_)
104  {
105  PCL_WARN ("[pcl::Poisson] iso_divide_ must be at least as large as min_depth_: %d >= %d\n", iso_divide_, min_depth_);
106  iso_divide_ = min_depth_;
107  }
108 
109  pcl::poisson::TreeOctNode::SetAllocator (MEMORY_ALLOCATOR_BLOCK_SIZE);
110 
111  kernel_depth_ = depth_ - 2;
112 
113  tree.setBSplineData (depth_, static_cast<pcl::poisson::Real>(1.0 / (1 << depth_)), true);
114 
115  tree.maxMemoryUsage = 0;
116 
117 
118  int point_count = tree.template setTree<PointNT> (input_, depth_, min_depth_, kernel_depth_, samples_per_node_,
119  scale_, center, scale, confidence_, point_weight_, !non_adaptive_weights_);
120 
121  tree.ClipTree ();
122  tree.finalize ();
123  tree.RefineBoundary (iso_divide_);
124 
125  PCL_DEBUG ("Input Points: %d\n" , point_count );
126  PCL_DEBUG ("Leaves/Nodes: %d/%d\n" , tree.tree.leaves() , tree.tree.nodes() );
127 
128  tree.maxMemoryUsage = 0;
129  tree.SetLaplacianConstraints ();
130 
131  tree.maxMemoryUsage = 0;
132  tree.LaplacianMatrixIteration (solver_divide_, show_residual_, min_iterations_, solver_accuracy_);
133 
134  iso_value = tree.GetIsoValue ();
135 
136  tree.GetMCIsoTriangles (iso_value, iso_divide_, &mesh, 0, 1, manifold_, output_polygons_);
137 }
138 
139 
140 //////////////////////////////////////////////////////////////////////////////////////////////
141 template <typename PointNT> void
143 {
146  float scale = 1.0f;
147 
148  switch (degree_)
149  {
150  case 1:
151  {
152  execute<1> (mesh, center, scale);
153  break;
154  }
155  case 2:
156  {
157  execute<2> (mesh, center, scale);
158  break;
159  }
160  case 3:
161  {
162  execute<3> (mesh, center, scale);
163  break;
164  }
165  case 4:
166  {
167  execute<4> (mesh, center, scale);
168  break;
169  }
170  case 5:
171  {
172  execute<5> (mesh, center, scale);
173  break;
174  }
175  default:
176  {
177  PCL_ERROR (stderr, "Degree %d not supported\n", degree_);
178  }
179  }
180 
181  // Write output PolygonMesh
183  cloud.resize (static_cast<int>(mesh.outOfCorePointCount () + mesh.inCorePoints.size ()));
185  for (int i = 0; i < static_cast<int>(mesh.inCorePoints.size ()); i++)
186  {
187  p = mesh.inCorePoints[i];
188  cloud[i].x = p.coords[0]*scale+center.coords[0];
189  cloud[i].y = p.coords[1]*scale+center.coords[1];
190  cloud[i].z = p.coords[2]*scale+center.coords[2];
191  }
192  for (int i = static_cast<int>(mesh.inCorePoints.size ()); i < static_cast<int>(mesh.outOfCorePointCount () + mesh.inCorePoints.size ()); i++)
193  {
194  mesh.nextOutOfCorePoint (p);
195  cloud[i].x = p.coords[0]*scale+center.coords[0];
196  cloud[i].y = p.coords[1]*scale+center.coords[1];
197  cloud[i].z = p.coords[2]*scale+center.coords[2];
198  }
199  pcl::toPCLPointCloud2 (cloud, output.cloud);
200  output.polygons.resize (mesh.polygonCount ());
201 
202  // Write faces
203  std::vector<poisson::CoredVertexIndex> polygon;
204  for (int p_i = 0; p_i < mesh.polygonCount (); p_i++)
205  {
206  pcl::Vertices v;
207  mesh.nextPolygon (polygon);
208  v.vertices.resize (polygon.size ());
209 
210  for (int i = 0; i < static_cast<int> (polygon.size ()); ++i)
211  if (polygon[i].inCore )
212  v.vertices[i] = polygon[i].idx;
213  else
214  v.vertices[i] = polygon[i].idx + static_cast<int>(mesh.inCorePoints.size ());
215 
216  output.polygons[p_i] = v;
217  }
218 }
219 
220 //////////////////////////////////////////////////////////////////////////////////////////////
221 template <typename PointNT> void
223  std::vector<pcl::Vertices> &polygons)
224 {
227  float scale = 1.0f;
228 
229  switch (degree_)
230  {
231  case 1:
232  {
233  execute<1> (mesh, center, scale);
234  break;
235  }
236  case 2:
237  {
238  execute<2> (mesh, center, scale);
239  break;
240  }
241  case 3:
242  {
243  execute<3> (mesh, center, scale);
244  break;
245  }
246  case 4:
247  {
248  execute<4> (mesh, center, scale);
249  break;
250  }
251  case 5:
252  {
253  execute<5> (mesh, center, scale);
254  break;
255  }
256  default:
257  {
258  PCL_ERROR (stderr, "Degree %d not supported\n", degree_);
259  }
260  }
261 
262  // Write output PolygonMesh
263  // Write vertices
264  points.resize (static_cast<int>(mesh.outOfCorePointCount () + mesh.inCorePoints.size ()));
266  for (int i = 0; i < static_cast<int>(mesh.inCorePoints.size ()); i++)
267  {
268  p = mesh.inCorePoints[i];
269  points[i].x = p.coords[0]*scale+center.coords[0];
270  points[i].y = p.coords[1]*scale+center.coords[1];
271  points[i].z = p.coords[2]*scale+center.coords[2];
272  }
273  for (int i = static_cast<int>(mesh.inCorePoints.size()); i < static_cast<int>(mesh.outOfCorePointCount() + mesh.inCorePoints.size ()); i++)
274  {
275  mesh.nextOutOfCorePoint (p);
276  points[i].x = p.coords[0]*scale+center.coords[0];
277  points[i].y = p.coords[1]*scale+center.coords[1];
278  points[i].z = p.coords[2]*scale+center.coords[2];
279  }
280 
281  polygons.resize (mesh.polygonCount ());
282 
283  // Write faces
284  std::vector<poisson::CoredVertexIndex> polygon;
285  for (int p_i = 0; p_i < mesh.polygonCount (); p_i++)
286  {
287  pcl::Vertices v;
288  mesh.nextPolygon (polygon);
289  v.vertices.resize (polygon.size ());
290 
291  for (int i = 0; i < static_cast<int> (polygon.size ()); ++i)
292  if (polygon[i].inCore )
293  v.vertices[i] = polygon[i].idx;
294  else
295  v.vertices[i] = polygon[i].idx + static_cast<int>(mesh.inCorePoints.size ());
296 
297  polygons[p_i] = v;
298  }
299 }
300 
301 
302 #define PCL_INSTANTIATE_Poisson(T) template class PCL_EXPORTS pcl::Poisson<T>;
303 
304 #endif // PCL_SURFACE_IMPL_POISSON_H_
305 
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
The Poisson surface reconstruction algorithm.
Definition: poisson.h:62
void setThreads(int threads)
Set the number of threads to use.
Definition: poisson.hpp:71
Poisson()
Constructor that sets all the parameters to working default values.
void performReconstruction(pcl::PolygonMesh &output) override
Create the surface.
Definition: poisson.hpp:142
~Poisson() override
Destructor.
std::vector< Point3D< float > > inCorePoints
Definition: geometry.h:202
int nextPolygon(std::vector< CoredVertexIndex > &vertices)
int nextOutOfCorePoint(Point3D< float > &p)
int leaves(void) const
void RefineBoundary(int subdivisionDepth)
void setBSplineData(int maxDepth, Real normalSmooth=-1, bool reflectBoundary=false)
int LaplacianMatrixIteration(int subdivideDepth, bool showResidual, int minIters, double accuracy)
Define standard C methods and C++ classes that are common to all methods.
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg, bool padding)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
Definition: conversions.h:372
std::vector< ::pcl::Vertices > polygons
Definition: PolygonMesh.h:22
::pcl::PCLPointCloud2 cloud
Definition: PolygonMesh.h:20
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
Definition: Vertices.h:15
Indices vertices
Definition: Vertices.h:18