Point Cloud Library (PCL)  1.11.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 #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  , threads_(1)
85 {
86 }
87 
88 //////////////////////////////////////////////////////////////////////////////////////////////
89 template <typename PointNT>
91 {
92 }
93 
94 //////////////////////////////////////////////////////////////////////////////////////////////
95 template <typename PointNT> void
97 {
98  if (threads == 0)
99 #ifdef _OPENMP
100  threads_ = omp_get_num_procs();
101 #else
102  threads_ = 1;
103 #endif
104  else
105  threads_ = threads;
106 }
107 
108 //////////////////////////////////////////////////////////////////////////////////////////////
109 template <typename PointNT> template <int Degree> void
111  poisson::Point3D<float> &center,
112  float &scale)
113 {
114  pcl::poisson::Real iso_value = 0;
117 
118 
119  tree.threads = threads_;
120  center.coords[0] = center.coords[1] = center.coords[2] = 0;
121 
122 
123  if (solver_divide_ < min_depth_)
124  {
125  PCL_WARN ("[pcl::Poisson] solver_divide_ must be at least as large as min_depth_: %d >= %d\n", solver_divide_, min_depth_);
126  solver_divide_ = min_depth_;
127  }
128  if (iso_divide_< min_depth_)
129  {
130  PCL_WARN ("[pcl::Poisson] iso_divide_ must be at least as large as min_depth_: %d >= %d\n", iso_divide_, min_depth_);
131  iso_divide_ = min_depth_;
132  }
133 
134  pcl::poisson::TreeOctNode::SetAllocator (MEMORY_ALLOCATOR_BLOCK_SIZE);
135 
136  kernel_depth_ = depth_ - 2;
137 
138  tree.setBSplineData (depth_, pcl::poisson::Real (1.0 / (1 << depth_)), true);
139 
140  tree.maxMemoryUsage = 0;
141 
142 
143  int point_count = tree.template setTree<PointNT> (input_, depth_, min_depth_, kernel_depth_, samples_per_node_,
144  scale_, center, scale, confidence_, point_weight_, !non_adaptive_weights_);
145 
146  tree.ClipTree ();
147  tree.finalize ();
148  tree.RefineBoundary (iso_divide_);
149 
150  PCL_DEBUG ("Input Points: %d\n" , point_count );
151  PCL_DEBUG ("Leaves/Nodes: %d/%d\n" , tree.tree.leaves() , tree.tree.nodes() );
152 
153  tree.maxMemoryUsage = 0;
154  tree.SetLaplacianConstraints ();
155 
156  tree.maxMemoryUsage = 0;
157  tree.LaplacianMatrixIteration (solver_divide_, show_residual_, min_iterations_, solver_accuracy_);
158 
159  iso_value = tree.GetIsoValue ();
160 
161  tree.GetMCIsoTriangles (iso_value, iso_divide_, &mesh, 0, 1, manifold_, output_polygons_);
162 }
163 
164 
165 //////////////////////////////////////////////////////////////////////////////////////////////
166 template <typename PointNT> void
168 {
171  float scale = 1.0f;
172 
173  switch (degree_)
174  {
175  case 1:
176  {
177  execute<1> (mesh, center, scale);
178  break;
179  }
180  case 2:
181  {
182  execute<2> (mesh, center, scale);
183  break;
184  }
185  case 3:
186  {
187  execute<3> (mesh, center, scale);
188  break;
189  }
190  case 4:
191  {
192  execute<4> (mesh, center, scale);
193  break;
194  }
195  case 5:
196  {
197  execute<5> (mesh, center, scale);
198  break;
199  }
200  default:
201  {
202  PCL_ERROR (stderr, "Degree %d not supported\n", degree_);
203  }
204  }
205 
206  // Write output PolygonMesh
208  cloud.resize (int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ()));
210  for (int i = 0; i < int (mesh.inCorePoints.size ()); i++)
211  {
212  p = mesh.inCorePoints[i];
213  cloud[i].x = p.coords[0]*scale+center.coords[0];
214  cloud[i].y = p.coords[1]*scale+center.coords[1];
215  cloud[i].z = p.coords[2]*scale+center.coords[2];
216  }
217  for (int i = int (mesh.inCorePoints.size ()); i < int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ()); i++)
218  {
219  mesh.nextOutOfCorePoint (p);
220  cloud[i].x = p.coords[0]*scale+center.coords[0];
221  cloud[i].y = p.coords[1]*scale+center.coords[1];
222  cloud[i].z = p.coords[2]*scale+center.coords[2];
223  }
224  pcl::toPCLPointCloud2 (cloud, output.cloud);
225  output.polygons.resize (mesh.polygonCount ());
226 
227  // Write faces
228  std::vector<poisson::CoredVertexIndex> polygon;
229  for (int p_i = 0; p_i < mesh.polygonCount (); p_i++)
230  {
231  pcl::Vertices v;
232  mesh.nextPolygon (polygon);
233  v.vertices.resize (polygon.size ());
234 
235  for (int i = 0; i < static_cast<int> (polygon.size ()); ++i)
236  if (polygon[i].inCore )
237  v.vertices[i] = polygon[i].idx;
238  else
239  v.vertices[i] = polygon[i].idx + int (mesh.inCorePoints.size ());
240 
241  output.polygons[p_i] = v;
242  }
243 }
244 
245 //////////////////////////////////////////////////////////////////////////////////////////////
246 template <typename PointNT> void
248  std::vector<pcl::Vertices> &polygons)
249 {
252  float scale = 1.0f;
253 
254  switch (degree_)
255  {
256  case 1:
257  {
258  execute<1> (mesh, center, scale);
259  break;
260  }
261  case 2:
262  {
263  execute<2> (mesh, center, scale);
264  break;
265  }
266  case 3:
267  {
268  execute<3> (mesh, center, scale);
269  break;
270  }
271  case 4:
272  {
273  execute<4> (mesh, center, scale);
274  break;
275  }
276  case 5:
277  {
278  execute<5> (mesh, center, scale);
279  break;
280  }
281  default:
282  {
283  PCL_ERROR (stderr, "Degree %d not supported\n", degree_);
284  }
285  }
286 
287  // Write output PolygonMesh
288  // Write vertices
289  points.resize (int (mesh.outOfCorePointCount () + mesh.inCorePoints.size ()));
291  for (int i = 0; i < int(mesh.inCorePoints.size ()); i++)
292  {
293  p = mesh.inCorePoints[i];
294  points[i].x = p.coords[0]*scale+center.coords[0];
295  points[i].y = p.coords[1]*scale+center.coords[1];
296  points[i].z = p.coords[2]*scale+center.coords[2];
297  }
298  for (int i = int(mesh.inCorePoints.size()); i < int (mesh.outOfCorePointCount() + mesh.inCorePoints.size ()); i++)
299  {
300  mesh.nextOutOfCorePoint (p);
301  points[i].x = p.coords[0]*scale+center.coords[0];
302  points[i].y = p.coords[1]*scale+center.coords[1];
303  points[i].z = p.coords[2]*scale+center.coords[2];
304  }
305 
306  polygons.resize (mesh.polygonCount ());
307 
308  // Write faces
309  std::vector<poisson::CoredVertexIndex> polygon;
310  for (int p_i = 0; p_i < mesh.polygonCount (); p_i++)
311  {
312  pcl::Vertices v;
313  mesh.nextPolygon (polygon);
314  v.vertices.resize (polygon.size ());
315 
316  for (int i = 0; i < static_cast<int> (polygon.size ()); ++i)
317  if (polygon[i].inCore )
318  v.vertices[i] = polygon[i].idx;
319  else
320  v.vertices[i] = polygon[i].idx + int (mesh.inCorePoints.size ());
321 
322  polygons[p_i] = v;
323  }
324 }
325 
326 
327 #define PCL_INSTANTIATE_Poisson(T) template class PCL_EXPORTS pcl::Poisson<T>;
328 
329 #endif // PCL_SURFACE_IMPL_POISSON_H_
330 
pcl::poisson::CoredVectorMeshData
Definition: geometry.h:247
pcl::poisson::Octree::tree
TreeOctNode tree
Definition: multi_grid_octree_data.h:340
pcl::PolygonMesh::cloud
::pcl::PCLPointCloud2 cloud
Definition: PolygonMesh.h:21
pcl
Definition: convolution.h:46
common.h
pcl::poisson::Octree::ClipTree
void ClipTree(void)
Definition: multi_grid_octree_data.hpp:2123
pcl::poisson::CoredVectorMeshData::outOfCorePointCount
int outOfCorePointCount(void)
pcl::Vertices::vertices
Indices vertices
Definition: Vertices.h:19
pcl::poisson::Point3D< float >
pcl::poisson::Octree::threads
int threads
Definition: multi_grid_octree_data.h:335
pcl::Poisson::performReconstruction
void performReconstruction(pcl::PolygonMesh &output) override
Create the surface.
Definition: poisson.hpp:167
pcl::poisson::Octree
Definition: multi_grid_octree_data.h:195
pcl::PointCloud< pcl::PointXYZ >
pcl::poisson::CoredMeshData::inCorePoints
std::vector< Point3D< float > > inCorePoints
Definition: geometry.h:202
pcl::poisson::TreeNodeData::UseIndex
static int UseIndex
Definition: multi_grid_octree_data.h:176
pcl::poisson::Octree::SetLaplacianConstraints
void SetLaplacianConstraints(void)
Definition: multi_grid_octree_data.hpp:2137
pcl::poisson::Octree::RefineBoundary
void RefineBoundary(int subdivisionDepth)
Definition: multi_grid_octree_data.hpp:2381
pcl::PolygonMesh
Definition: PolygonMesh.h:14
pcl::poisson::OctNode< class TreeNodeData, Real >::SetAllocator
static void SetAllocator(int blockSize)
Definition: octree_poisson.hpp:54
pcl::Poisson
The Poisson surface reconstruction algorithm.
Definition: poisson.h:61
pcl::poisson::Octree::finalize
void finalize(void)
Definition: multi_grid_octree_data.hpp:990
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:477
pcl::poisson::Octree::GetIsoValue
Real GetIsoValue(void)
Definition: multi_grid_octree_data.hpp:2817
pcl::Poisson::~Poisson
~Poisson()
Destructor.
Definition: poisson.hpp:90
pcl::Poisson::Poisson
Poisson()
Constructor that sets all the parameters to working default values.
Definition: poisson.hpp:64
pcl::poisson::CoredVectorMeshData::nextPolygon
int nextPolygon(std::vector< CoredVertexIndex > &vertices)
pcl::poisson::Point3D::coords
Real coords[3]
Definition: geometry.h:52
pcl::poisson::CoredVectorMeshData::polygonCount
int polygonCount(void)
pcl::Vertices
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
Definition: Vertices.h:14
pcl::PolygonMesh::polygons
std::vector< ::pcl::Vertices > polygons
Definition: PolygonMesh.h:23
pcl::poisson::CoredVectorMeshData::nextOutOfCorePoint
int nextOutOfCorePoint(Point3D< float > &p)
pcl::toPCLPointCloud2
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:240
pcl::poisson::Real
float Real
Definition: multi_grid_octree_data.h:85
pcl::poisson::Octree::maxMemoryUsage
static double maxMemoryUsage
Definition: multi_grid_octree_data.h:336
pcl::poisson::Octree::setBSplineData
void setBSplineData(int maxDepth, Real normalSmooth=-1, bool reflectBoundary=false)
Definition: multi_grid_octree_data.hpp:981
pcl::poisson::Octree::LaplacianMatrixIteration
int LaplacianMatrixIteration(int subdivideDepth, bool showResidual, int minIters, double accuracy)
Definition: multi_grid_octree_data.hpp:1901
pcl::Poisson::setThreads
void setThreads(int threads)
Set the number of threads to use.
Definition: poisson.hpp:96