Point Cloud Library (PCL)  1.8.1-dev
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
concave_hull.hpp
1 /**
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, 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 #include <pcl/pcl_config.h>
41 #ifdef HAVE_QHULL
42 
43 #ifndef PCL_SURFACE_IMPL_CONCAVE_HULL_H_
44 #define PCL_SURFACE_IMPL_CONCAVE_HULL_H_
45 
46 #include <map>
47 #include <pcl/surface/concave_hull.h>
48 #include <pcl/common/common.h>
49 #include <pcl/common/eigen.h>
50 #include <pcl/common/centroid.h>
51 #include <pcl/common/transforms.h>
52 #include <pcl/kdtree/kdtree_flann.h>
53 #include <pcl/common/io.h>
54 #include <stdio.h>
55 #include <stdlib.h>
56 #include <pcl/surface/qhull.h>
57 
58 //////////////////////////////////////////////////////////////////////////
59 template <typename PointInT> void
61 {
62  output.header = input_->header;
63  if (alpha_ <= 0)
64  {
65  PCL_ERROR ("[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
66  output.points.clear ();
67  return;
68  }
69 
70  if (!initCompute ())
71  {
72  output.points.clear ();
73  return;
74  }
75 
76  // Perform the actual surface reconstruction
77  std::vector<pcl::Vertices> polygons;
78  performReconstruction (output, polygons);
79 
80  output.width = static_cast<uint32_t> (output.points.size ());
81  output.height = 1;
82  output.is_dense = true;
83 
84  deinitCompute ();
85 }
86 
87 //////////////////////////////////////////////////////////////////////////
88 template <typename PointInT> void
89 pcl::ConcaveHull<PointInT>::reconstruct (PointCloud &output, std::vector<pcl::Vertices> &polygons)
90 {
91  output.header = input_->header;
92  if (alpha_ <= 0)
93  {
94  PCL_ERROR ("[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
95  output.points.clear ();
96  return;
97  }
98 
99  if (!initCompute ())
100  {
101  output.points.clear ();
102  return;
103  }
104 
105  // Perform the actual surface reconstruction
106  performReconstruction (output, polygons);
107 
108  output.width = static_cast<uint32_t> (output.points.size ());
109  output.height = 1;
110  output.is_dense = true;
111 
112  deinitCompute ();
113 }
114 
115 #ifdef __GNUC__
116 #pragma GCC diagnostic ignored "-Wold-style-cast"
117 #endif
118 //////////////////////////////////////////////////////////////////////////
119 template <typename PointInT> void
120 pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std::vector<pcl::Vertices> &polygons)
121 {
122  Eigen::Vector4d xyz_centroid;
123  compute3DCentroid (*input_, *indices_, xyz_centroid);
124  EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix = Eigen::Matrix3d::Zero ();
125  computeCovarianceMatrixNormalized (*input_, *indices_, xyz_centroid, covariance_matrix);
126 
127  // Check if the covariance matrix is finite or not.
128  for (int i = 0; i < 3; ++i)
129  for (int j = 0; j < 3; ++j)
130  if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
131  return;
132 
133  EIGEN_ALIGN16 Eigen::Vector3d eigen_values;
134  EIGEN_ALIGN16 Eigen::Matrix3d eigen_vectors;
135  pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
136 
137  Eigen::Affine3d transform1;
138  transform1.setIdentity ();
139 
140  // If no input dimension is specified, determine automatically
141  if (dim_ == 0)
142  {
143  PCL_DEBUG ("[pcl::%s] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ());
144  if (std::abs (eigen_values[0]) < std::numeric_limits<double>::epsilon () || std::abs (eigen_values[0] / eigen_values[2]) < 1.0e-3)
145  dim_ = 2;
146  else
147  dim_ = 3;
148  }
149 
150  if (dim_ == 2)
151  {
152  // we have points laying on a plane, using 2d convex hull
153  // compute transformation bring eigen_vectors.col(i) to z-axis
154 
155  transform1 (2, 0) = eigen_vectors (0, 0);
156  transform1 (2, 1) = eigen_vectors (1, 0);
157  transform1 (2, 2) = eigen_vectors (2, 0);
158 
159  transform1 (1, 0) = eigen_vectors (0, 1);
160  transform1 (1, 1) = eigen_vectors (1, 1);
161  transform1 (1, 2) = eigen_vectors (2, 1);
162  transform1 (0, 0) = eigen_vectors (0, 2);
163  transform1 (0, 1) = eigen_vectors (1, 2);
164  transform1 (0, 2) = eigen_vectors (2, 2);
165  }
166  else
167  {
168  transform1.setIdentity ();
169  }
170 
171  PointCloud cloud_transformed;
172  pcl::demeanPointCloud (*input_, *indices_, xyz_centroid, cloud_transformed);
173  pcl::transformPointCloud (cloud_transformed, cloud_transformed, transform1);
174 
175  // True if qhull should free points in qh_freeqhull() or reallocation
176  boolT ismalloc = True;
177  // option flags for qhull, see qh_opt.htm
178  char flags[] = "qhull d QJ";
179  // output from qh_produce_output(), use NULL to skip qh_produce_output()
180  FILE *outfile = NULL;
181  // error messages from qhull code
182  FILE *errfile = stderr;
183  // 0 if no error from qhull
184  int exitcode;
185 
186  // Array of coordinates for each point
187  coordT *points = reinterpret_cast<coordT*> (calloc (cloud_transformed.points.size () * dim_, sizeof(coordT)));
188 
189  for (size_t i = 0; i < cloud_transformed.points.size (); ++i)
190  {
191  points[i * dim_ + 0] = static_cast<coordT> (cloud_transformed.points[i].x);
192  points[i * dim_ + 1] = static_cast<coordT> (cloud_transformed.points[i].y);
193 
194  if (dim_ > 2)
195  points[i * dim_ + 2] = static_cast<coordT> (cloud_transformed.points[i].z);
196  }
197 
198  // Compute concave hull
199  exitcode = qh_new_qhull (dim_, static_cast<int> (cloud_transformed.points.size ()), points, ismalloc, flags, outfile, errfile);
200 
201  if (exitcode != 0)
202  {
203  PCL_ERROR ("[pcl::%s::performReconstrution] ERROR: qhull was unable to compute a concave hull for the given point cloud (%lu)!\n", getClassName ().c_str (), cloud_transformed.points.size ());
204 
205  //check if it fails because of NaN values...
206  if (!cloud_transformed.is_dense)
207  {
208  bool NaNvalues = false;
209  for (size_t i = 0; i < cloud_transformed.size (); ++i)
210  {
211  if (!pcl_isfinite (cloud_transformed.points[i].x) ||
212  !pcl_isfinite (cloud_transformed.points[i].y) ||
213  !pcl_isfinite (cloud_transformed.points[i].z))
214  {
215  NaNvalues = true;
216  break;
217  }
218  }
219 
220  if (NaNvalues)
221  PCL_ERROR ("[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
222  }
223 
224  alpha_shape.points.resize (0);
225  alpha_shape.width = alpha_shape.height = 0;
226  polygons.resize (0);
227 
228  qh_freeqhull (!qh_ALL);
229  int curlong, totlong;
230  qh_memfreeshort (&curlong, &totlong);
231 
232  return;
233  }
234 
235  qh_setvoronoi_all ();
236 
237  int num_vertices = qh num_vertices;
238  alpha_shape.points.resize (num_vertices);
239 
240  vertexT *vertex;
241  // Max vertex id
242  int max_vertex_id = 0;
243  FORALLvertices
244  {
245  if (vertex->id + 1 > max_vertex_id)
246  max_vertex_id = vertex->id + 1;
247  }
248 
249  facetT *facet; // set by FORALLfacets
250 
251  ++max_vertex_id;
252  std::vector<int> qhid_to_pcidx (max_vertex_id);
253 
254  int num_facets = qh num_facets;
255  int dd = 0;
256 
257  if (dim_ == 3)
258  {
259  setT *triangles_set = qh_settemp (4 * num_facets);
260  if (voronoi_centers_)
261  voronoi_centers_->points.resize (num_facets);
262 
263  int non_upper = 0;
264  FORALLfacets
265  {
266  // Facets are tetrahedrons (3d)
267  if (!facet->upperdelaunay)
268  {
269  vertexT *anyVertex = static_cast<vertexT*> (facet->vertices->e[0].p);
270  double *center = facet->center;
271  double r = qh_pointdist (anyVertex->point,center,dim_);
272  facetT *neighb;
273 
274  if (voronoi_centers_)
275  {
276  voronoi_centers_->points[non_upper].x = static_cast<float> (facet->center[0]);
277  voronoi_centers_->points[non_upper].y = static_cast<float> (facet->center[1]);
278  voronoi_centers_->points[non_upper].z = static_cast<float> (facet->center[2]);
279  }
280 
281  non_upper++;
282 
283  if (r <= alpha_)
284  {
285  // all triangles in tetrahedron are good, add them all to the alpha shape (triangles_set)
286  qh_makeridges (facet);
287  facet->good = true;
288  facet->visitid = qh visit_id;
289  ridgeT *ridge, **ridgep;
290  FOREACHridge_ (facet->ridges)
291  {
292  neighb = otherfacet_ (ridge, facet);
293  if ((neighb->visitid != qh visit_id))
294  qh_setappend (&triangles_set, ridge);
295  }
296  }
297  else
298  {
299  // consider individual triangles from the tetrahedron...
300  facet->good = false;
301  facet->visitid = qh visit_id;
302  qh_makeridges (facet);
303  ridgeT *ridge, **ridgep;
304  FOREACHridge_ (facet->ridges)
305  {
306  facetT *neighb;
307  neighb = otherfacet_ (ridge, facet);
308  if ((neighb->visitid != qh visit_id))
309  {
310  // check if individual triangle is good and add it to triangles_set
311 
312  PointInT a, b, c;
313  a.x = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[0].p))->point[0]);
314  a.y = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[0].p))->point[1]);
315  a.z = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[0].p))->point[2]);
316  b.x = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[1].p))->point[0]);
317  b.y = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[1].p))->point[1]);
318  b.z = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[1].p))->point[2]);
319  c.x = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[2].p))->point[0]);
320  c.y = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[2].p))->point[1]);
321  c.z = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[2].p))->point[2]);
322 
323  double r = pcl::getCircumcircleRadius (a, b, c);
324  if (r <= alpha_)
325  qh_setappend (&triangles_set, ridge);
326  }
327  }
328  }
329  }
330  }
331 
332  if (voronoi_centers_)
333  voronoi_centers_->points.resize (non_upper);
334 
335  // filter, add points to alpha_shape and create polygon structure
336 
337  int num_good_triangles = 0;
338  ridgeT *ridge, **ridgep;
339  FOREACHridge_ (triangles_set)
340  {
341  if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
342  num_good_triangles++;
343  }
344 
345  polygons.resize (num_good_triangles);
346 
347  int vertices = 0;
348  std::vector<bool> added_vertices (max_vertex_id, false);
349 
350  int triangles = 0;
351  FOREACHridge_ (triangles_set)
352  {
353  if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
354  {
355  polygons[triangles].vertices.resize (3);
356  int vertex_n, vertex_i;
357  FOREACHvertex_i_ ((*ridge).vertices) //3 vertices per ridge!
358  {
359  if (!added_vertices[vertex->id])
360  {
361  alpha_shape.points[vertices].x = static_cast<float> (vertex->point[0]);
362  alpha_shape.points[vertices].y = static_cast<float> (vertex->point[1]);
363  alpha_shape.points[vertices].z = static_cast<float> (vertex->point[2]);
364 
365  qhid_to_pcidx[vertex->id] = vertices; //map the vertex id of qhull to the point cloud index
366  added_vertices[vertex->id] = true;
367  vertices++;
368  }
369 
370  polygons[triangles].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
371 
372  }
373 
374  triangles++;
375  }
376  }
377 
378  alpha_shape.points.resize (vertices);
379  alpha_shape.width = static_cast<uint32_t> (alpha_shape.points.size ());
380  alpha_shape.height = 1;
381  }
382  else
383  {
384  // Compute the alpha complex for the set of points
385  // Filters the delaunay triangles
386  setT *edges_set = qh_settemp (3 * num_facets);
387  if (voronoi_centers_)
388  voronoi_centers_->points.resize (num_facets);
389 
390  FORALLfacets
391  {
392  // Facets are the delaunay triangles (2d)
393  if (!facet->upperdelaunay)
394  {
395  // Check if the distance from any vertex to the facet->center
396  // (center of the voronoi cell) is smaller than alpha
397  vertexT *anyVertex = static_cast<vertexT*>(facet->vertices->e[0].p);
398  double r = (sqrt ((anyVertex->point[0] - facet->center[0]) *
399  (anyVertex->point[0] - facet->center[0]) +
400  (anyVertex->point[1] - facet->center[1]) *
401  (anyVertex->point[1] - facet->center[1])));
402  if (r <= alpha_)
403  {
404  pcl::Vertices facet_vertices; //TODO: is not used!!
405  qh_makeridges (facet);
406  facet->good = true;
407 
408  ridgeT *ridge, **ridgep;
409  FOREACHridge_ (facet->ridges)
410  qh_setappend (&edges_set, ridge);
411 
412  if (voronoi_centers_)
413  {
414  voronoi_centers_->points[dd].x = static_cast<float> (facet->center[0]);
415  voronoi_centers_->points[dd].y = static_cast<float> (facet->center[1]);
416  voronoi_centers_->points[dd].z = 0.0f;
417  }
418 
419  ++dd;
420  }
421  else
422  facet->good = false;
423  }
424  }
425 
426  int vertices = 0;
427  std::vector<bool> added_vertices (max_vertex_id, false);
428  std::map<int, std::vector<int> > edges;
429 
430  ridgeT *ridge, **ridgep;
431  FOREACHridge_ (edges_set)
432  {
433  if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
434  {
435  int vertex_n, vertex_i;
436  int vertices_in_ridge=0;
437  std::vector<int> pcd_indices;
438  pcd_indices.resize (2);
439 
440  FOREACHvertex_i_ ((*ridge).vertices) //in 2-dim, 2 vertices per ridge!
441  {
442  if (!added_vertices[vertex->id])
443  {
444  alpha_shape.points[vertices].x = static_cast<float> (vertex->point[0]);
445  alpha_shape.points[vertices].y = static_cast<float> (vertex->point[1]);
446 
447  if (dim_ > 2)
448  alpha_shape.points[vertices].z = static_cast<float> (vertex->point[2]);
449  else
450  alpha_shape.points[vertices].z = 0;
451 
452  qhid_to_pcidx[vertex->id] = vertices; //map the vertex id of qhull to the point cloud index
453  added_vertices[vertex->id] = true;
454  pcd_indices[vertices_in_ridge] = vertices;
455  vertices++;
456  }
457  else
458  {
459  pcd_indices[vertices_in_ridge] = qhid_to_pcidx[vertex->id];
460  }
461 
462  vertices_in_ridge++;
463  }
464 
465  // make edges bidirectional and pointing to alpha_shape pointcloud...
466  edges[pcd_indices[0]].push_back (pcd_indices[1]);
467  edges[pcd_indices[1]].push_back (pcd_indices[0]);
468  }
469  }
470 
471  alpha_shape.points.resize (vertices);
472 
473  std::vector<std::vector<int> > connected;
474  PointCloud alpha_shape_sorted;
475  alpha_shape_sorted.points.resize (vertices);
476 
477  // iterate over edges until they are empty!
478  std::map<int, std::vector<int> >::iterator curr = edges.begin ();
479  int next = - 1;
480  std::vector<bool> used (vertices, false); // used to decide which direction should we take!
481  std::vector<int> pcd_idx_start_polygons;
482  pcd_idx_start_polygons.push_back (0);
483 
484  // start following edges and removing elements
485  int sorted_idx = 0;
486  while (!edges.empty ())
487  {
488  alpha_shape_sorted.points[sorted_idx] = alpha_shape.points[(*curr).first];
489  // check where we can go from (*curr).first
490  for (size_t i = 0; i < (*curr).second.size (); i++)
491  {
492  if (!used[((*curr).second)[i]])
493  {
494  // we can go there
495  next = ((*curr).second)[i];
496  break;
497  }
498  }
499 
500  used[(*curr).first] = true;
501  edges.erase (curr); // remove edges starting from curr
502 
503  sorted_idx++;
504 
505  if (edges.empty ())
506  break;
507 
508  // reassign current
509  curr = edges.find (next); // if next is not found, then we have unconnected polygons.
510  if (curr == edges.end ())
511  {
512  // set current to any of the remaining in edge!
513  curr = edges.begin ();
514  pcd_idx_start_polygons.push_back (sorted_idx);
515  }
516  }
517 
518  pcd_idx_start_polygons.push_back (sorted_idx);
519 
520  alpha_shape.points = alpha_shape_sorted.points;
521 
522  polygons.reserve (pcd_idx_start_polygons.size () - 1);
523 
524  for (size_t poly_id = 0; poly_id < pcd_idx_start_polygons.size () - 1; poly_id++)
525  {
526  // Check if we actually have a polygon, and not some degenerated output from QHull
527  if (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id] >= 3)
528  {
529  pcl::Vertices vertices;
530  vertices.vertices.resize (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id]);
531  // populate points in the corresponding polygon
532  for (int j = pcd_idx_start_polygons[poly_id]; j < pcd_idx_start_polygons[poly_id + 1]; ++j)
533  vertices.vertices[j - pcd_idx_start_polygons[poly_id]] = static_cast<uint32_t> (j);
534 
535  polygons.push_back (vertices);
536  }
537  }
538 
539  if (voronoi_centers_)
540  voronoi_centers_->points.resize (dd);
541  }
542 
543  qh_freeqhull (!qh_ALL);
544  int curlong, totlong;
545  qh_memfreeshort (&curlong, &totlong);
546 
547  Eigen::Affine3d transInverse = transform1.inverse ();
548  pcl::transformPointCloud (alpha_shape, alpha_shape, transInverse);
549  xyz_centroid[0] = - xyz_centroid[0];
550  xyz_centroid[1] = - xyz_centroid[1];
551  xyz_centroid[2] = - xyz_centroid[2];
552  pcl::demeanPointCloud (alpha_shape, xyz_centroid, alpha_shape);
553 
554  // also transform voronoi_centers_...
555  if (voronoi_centers_)
556  {
557  pcl::transformPointCloud (*voronoi_centers_, *voronoi_centers_, transInverse);
558  pcl::demeanPointCloud (*voronoi_centers_, xyz_centroid, *voronoi_centers_);
559  }
560 
561  if (keep_information_)
562  {
563  // build a tree with the original points
564  pcl::KdTreeFLANN<PointInT> tree (true);
565  tree.setInputCloud (input_, indices_);
566 
567  std::vector<int> neighbor;
568  std::vector<float> distances;
569  neighbor.resize (1);
570  distances.resize (1);
571 
572  // for each point in the concave hull, search for the nearest neighbor in the original point cloud
573  hull_indices_.header = input_->header;
574  hull_indices_.indices.clear ();
575  hull_indices_.indices.reserve (alpha_shape.points.size ());
576 
577  for (size_t i = 0; i < alpha_shape.points.size (); i++)
578  {
579  tree.nearestKSearch (alpha_shape.points[i], 1, neighbor, distances);
580  hull_indices_.indices.push_back (neighbor[0]);
581  }
582 
583  // replace point with the closest neighbor in the original point cloud
584  pcl::copyPointCloud (*input_, hull_indices_.indices, alpha_shape);
585  }
586 }
587 #ifdef __GNUC__
588 #pragma GCC diagnostic warning "-Wold-style-cast"
589 #endif
590 
591 //////////////////////////////////////////////////////////////////////////////////////////
592 template <typename PointInT> void
594 {
595  // Perform reconstruction
596  pcl::PointCloud<PointInT> hull_points;
597  performReconstruction (hull_points, output.polygons);
598 
599  // Convert the PointCloud into a PCLPointCloud2
600  pcl::toPCLPointCloud2 (hull_points, output.cloud);
601 }
602 
603 //////////////////////////////////////////////////////////////////////////////////////////
604 template <typename PointInT> void
605 pcl::ConcaveHull<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
606 {
607  pcl::PointCloud<PointInT> hull_points;
608  performReconstruction (hull_points, polygons);
609 }
610 
611 //////////////////////////////////////////////////////////////////////////////////////////
612 template <typename PointInT> void
614 {
615  hull_point_indices = hull_indices_;
616 }
617 
618 #define PCL_INSTANTIATE_ConcaveHull(T) template class PCL_EXPORTS pcl::ConcaveHull<T>;
619 
620 #endif // PCL_SURFACE_IMPL_CONCAVE_HULL_H_
621 #endif
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:69
struct pcl::PointXYZIEdge EIGEN_ALIGN16
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
std::vector< uint32_t > vertices
Definition: Vertices.h:19
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for k-nearest neighbors for the given query point.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
void getHullPointIndices(pcl::PointIndices &hull_point_indices) const
Retrieve the indices of the input point cloud that for the convex hull.
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
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
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:215
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > &centroid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
Definition: centroid.hpp:631
void reconstruct(PointCloud &points, std::vector< pcl::Vertices > &polygons)
Compute a concave hull for all points given.
size_t size() const
Definition: point_cloud.h:448
std::vector< ::pcl::Vertices > polygons
Definition: PolygonMesh.h:24
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
Definition: eigen.hpp:251
::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
double getCircumcircleRadius(const PointT &pa, const PointT &pb, const PointT &pc)
Compute the radius of a circumscribed circle for a triangle formed of three points pa...
Definition: common.hpp:376
void performReconstruction(PointCloud &points, std::vector< pcl::Vertices > &polygons)
The actual reconstruction method.
unsigned int computeCovarianceMatrixNormalized(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > &centroid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute normalized the 3x3 covariance matrix of a given set of points.
Definition: centroid.hpp:251
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition: centroid.hpp:50
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:418
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415