Point Cloud Library (PCL)  1.8.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 <stdarg.h>
58 #include <string>
59 
60 using namespace pcl;
61 
62 //////////////////////////////////////////////////////////////////////////////////////////////
63 template <typename PointNT>
65  : depth_ (8)
66  , min_depth_ (5)
67  , point_weight_ (4)
68  , scale_ (1.1f)
69  , solver_divide_ (8)
70  , iso_divide_ (8)
71  , samples_per_node_ (1.0)
72  , confidence_ (false)
73  , output_polygons_ (false)
74  , no_reset_samples_ (false)
75  , no_clip_tree_ (false)
76  , manifold_ (true)
77  , refine_ (3)
78  , kernel_depth_ (8)
79  , degree_ (2)
80  , non_adaptive_weights_ (false)
81  , show_residual_ (false)
82  , min_iterations_ (8)
83  , solver_accuracy_ (1e-3f)
84 {
85 }
86 
87 //////////////////////////////////////////////////////////////////////////////////////////////
88 template <typename PointNT>
90 {
91 }
92 
93 //////////////////////////////////////////////////////////////////////////////////////////////
94 template <typename PointNT> template <int Degree> void
97  float &scale)
98 {
99  pcl::poisson::Real iso_value = 0;
102 
103  /// TODO OPENMP stuff
104  // tree.threads = Threads.value;
105  center.coords[0] = center.coords[1] = center.coords[2] = 0;
106 
107 
108  if (solver_divide_ < min_depth_)
109  {
110  PCL_WARN ("[pcl::Poisson] solver_divide_ must be at least as large as min_depth_: %d >= %d\n", solver_divide_, min_depth_);
111  solver_divide_ = min_depth_;
112  }
113  if (iso_divide_< min_depth_)
114  {
115  PCL_WARN ("[pcl::Poisson] iso_divide_ must be at least as large as min_depth_: %d >= %d\n", iso_divide_, min_depth_);
116  iso_divide_ = min_depth_;
117  }
118 
119  pcl::poisson::TreeOctNode::SetAllocator (MEMORY_ALLOCATOR_BLOCK_SIZE);
120 
121  kernel_depth_ = depth_ - 2;
122 
123  tree.setBSplineData (depth_, pcl::poisson::Real (1.0 / (1 << depth_)), true);
124 
125  tree.maxMemoryUsage = 0;
126 
127 
128  int point_count = tree.setTree (input_, depth_, min_depth_, kernel_depth_, samples_per_node_,
129  scale_, center, scale, confidence_, point_weight_, !non_adaptive_weights_);
130 
131  tree.ClipTree ();
132  tree.finalize ();
133  tree.RefineBoundary (iso_divide_);
134 
135  PCL_DEBUG ("Input Points: %d\n" , point_count );
136  PCL_DEBUG ("Leaves/Nodes: %d/%d\n" , tree.tree.leaves() , tree.tree.nodes() );
137 
138  tree.maxMemoryUsage = 0;
139  tree.SetLaplacianConstraints ();
140 
141  tree.maxMemoryUsage = 0;
142  tree.LaplacianMatrixIteration (solver_divide_, show_residual_, min_iterations_, solver_accuracy_);
143 
144  iso_value = tree.GetIsoValue ();
145 
146  tree.GetMCIsoTriangles (iso_value, iso_divide_, &mesh, 0, 1, manifold_, output_polygons_);
147 }
148 
149 
150 //////////////////////////////////////////////////////////////////////////////////////////////
151 template <typename PointNT> void
153 {
156  float scale = 1.0f;
157 
158  switch (degree_)
159  {
160  case 1:
161  {
162  execute<1> (mesh, center, scale);
163  break;
164  }
165  case 2:
166  {
167  execute<2> (mesh, center, scale);
168  break;
169  }
170  case 3:
171  {
172  execute<3> (mesh, center, scale);
173  break;
174  }
175  case 4:
176  {
177  execute<4> (mesh, center, scale);
178  break;
179  }
180  case 5:
181  {
182  execute<5> (mesh, center, scale);
183  break;
184  }
185  default:
186  {
187  PCL_ERROR (stderr, "Degree %d not supported\n", degree_);
188  }
189  }
190 
191  // Write output PolygonMesh
193  cloud.points.resize (int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ()));
195  for (int i = 0; i < int (mesh.inCorePoints.size ()); i++)
196  {
197  p = mesh.inCorePoints[i];
198  cloud.points[i].x = p.coords[0]*scale+center.coords[0];
199  cloud.points[i].y = p.coords[1]*scale+center.coords[1];
200  cloud.points[i].z = p.coords[2]*scale+center.coords[2];
201  }
202  for (int i = int (mesh.inCorePoints.size ()); i < int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ()); i++)
203  {
204  mesh.nextOutOfCorePoint (p);
205  cloud.points[i].x = p.coords[0]*scale+center.coords[0];
206  cloud.points[i].y = p.coords[1]*scale+center.coords[1];
207  cloud.points[i].z = p.coords[2]*scale+center.coords[2];
208  }
209  pcl::toPCLPointCloud2 (cloud, output.cloud);
210  output.polygons.resize (mesh.polygonCount ());
211 
212  // Write faces
213  std::vector<poisson::CoredVertexIndex> polygon;
214  for (int p_i = 0; p_i < mesh.polygonCount (); p_i++)
215  {
216  pcl::Vertices v;
217  mesh.nextPolygon (polygon);
218  v.vertices.resize (polygon.size ());
219 
220  for (int i = 0; i < static_cast<int> (polygon.size ()); ++i)
221  if (polygon[i].inCore )
222  v.vertices[i] = polygon[i].idx;
223  else
224  v.vertices[i] = polygon[i].idx + int (mesh.inCorePoints.size ());
225 
226  output.polygons[p_i] = v;
227  }
228 }
229 
230 //////////////////////////////////////////////////////////////////////////////////////////////
231 template <typename PointNT> void
233  std::vector<pcl::Vertices> &polygons)
234 {
237  float scale = 1.0f;
238 
239  switch (degree_)
240  {
241  case 1:
242  {
243  execute<1> (mesh, center, scale);
244  break;
245  }
246  case 2:
247  {
248  execute<2> (mesh, center, scale);
249  break;
250  }
251  case 3:
252  {
253  execute<3> (mesh, center, scale);
254  break;
255  }
256  case 4:
257  {
258  execute<4> (mesh, center, scale);
259  break;
260  }
261  case 5:
262  {
263  execute<5> (mesh, center, scale);
264  break;
265  }
266  default:
267  {
268  PCL_ERROR (stderr, "Degree %d not supported\n", degree_);
269  }
270  }
271 
272  // Write output PolygonMesh
273  // Write vertices
274  points.resize (int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ()));
276  for (int i = 0; i < int(mesh.inCorePoints.size ()); i++)
277  {
278  p = mesh.inCorePoints[i];
279  points.points[i].x = p.coords[0]*scale+center.coords[0];
280  points.points[i].y = p.coords[1]*scale+center.coords[1];
281  points.points[i].z = p.coords[2]*scale+center.coords[2];
282  }
283  for (int i = int(mesh.inCorePoints.size()); i < int (mesh.outOfCorePointCount() + mesh.inCorePoints.size ()); i++)
284  {
285  mesh.nextOutOfCorePoint (p);
286  points.points[i].x = p.coords[0]*scale+center.coords[0];
287  points.points[i].y = p.coords[1]*scale+center.coords[1];
288  points.points[i].z = p.coords[2]*scale+center.coords[2];
289  }
290 
291  polygons.resize (mesh.polygonCount ());
292 
293  // Write faces
294  std::vector<poisson::CoredVertexIndex> polygon;
295  for (int p_i = 0; p_i < mesh.polygonCount (); p_i++)
296  {
297  pcl::Vertices v;
298  mesh.nextPolygon (polygon);
299  v.vertices.resize (polygon.size ());
300 
301  for (int i = 0; i < static_cast<int> (polygon.size ()); ++i)
302  if (polygon[i].inCore )
303  v.vertices[i] = polygon[i].idx;
304  else
305  v.vertices[i] = polygon[i].idx + int (mesh.inCorePoints.size ());
306 
307  polygons[p_i] = v;
308  }
309 }
310 
311 
312 #define PCL_INSTANTIATE_Poisson(T) template class PCL_EXPORTS pcl::Poisson<T>;
313 
314 #endif // PCL_SURFACE_IMPL_POISSON_H_
315 
void performReconstruction(pcl::PolygonMesh &output)
Create the surface.
Definition: poisson.hpp:152
int nextOutOfCorePoint(Point3D< float > &p)
void RefineBoundary(int subdivisionDepth)
std::vector< uint32_t > vertices
Definition: Vertices.h:19
int setTree(boost::shared_ptr< const pcl::PointCloud< PointNT > > input_, int maxDepth, int minDepth, int kernelDepth, Real samplesPerNode, Real scaleFactor, Point3D< Real > &center, Real &scale, int useConfidence, Real constraintWeight, bool adaptiveWeights)
int LaplacianMatrixIteration(int subdivideDepth, bool showResidual, int minIters, double accuracy)
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
Definition: Vertices.h:14
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
The Poisson surface reconstruction algorithm.
Definition: poisson.h:60
int nextPolygon(std::vector< CoredVertexIndex > &vertices)
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:455
Poisson()
Constructor that sets all the parameters to working default values.
Definition: poisson.hpp:64
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
std::vector< Point3D< float > > inCorePoints
Definition: geometry.h:202
~Poisson()
Destructor.
Definition: poisson.hpp:89
void setBSplineData(int maxDepth, Real normalSmooth=-1, bool reflectBoundary=false)