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