Point Cloud Library (PCL)  1.8.0
convex_hull.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 #include <pcl/pcl_config.h>
41 #ifdef HAVE_QHULL
42 
43 #ifndef PCL_SURFACE_IMPL_CONVEX_HULL_H_
44 #define PCL_SURFACE_IMPL_CONVEX_HULL_H_
45 
46 #include <pcl/surface/convex_hull.h>
47 #include <pcl/common/common.h>
48 #include <pcl/common/eigen.h>
49 #include <pcl/common/transforms.h>
50 #include <pcl/common/io.h>
51 #include <stdio.h>
52 #include <stdlib.h>
53 #include <pcl/surface/qhull.h>
54 
55 //////////////////////////////////////////////////////////////////////////
56 template <typename PointInT> void
58 {
59  PCL_DEBUG ("[pcl::%s::calculateInputDimension] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ());
60  EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix;
61  Eigen::Vector4d xyz_centroid;
62  computeMeanAndCovarianceMatrix (*input_, *indices_, covariance_matrix, xyz_centroid);
63 
64  EIGEN_ALIGN16 Eigen::Vector3d eigen_values;
65  pcl::eigen33 (covariance_matrix, eigen_values);
66 
67  if (eigen_values[0] / eigen_values[2] < 1.0e-3)
68  dimension_ = 2;
69  else
70  dimension_ = 3;
71 }
72 
73 //////////////////////////////////////////////////////////////////////////
74 template <typename PointInT> void
75 pcl::ConvexHull<PointInT>::performReconstruction2D (PointCloud &hull, std::vector<pcl::Vertices> &polygons,
76  bool)
77 {
78  int dimension = 2;
79  bool xy_proj_safe = true;
80  bool yz_proj_safe = true;
81  bool xz_proj_safe = true;
82 
83  // Check the input's normal to see which projection to use
84  PointInT p0 = input_->points[(*indices_)[0]];
85  PointInT p1 = input_->points[(*indices_)[indices_->size () - 1]];
86  PointInT p2 = input_->points[(*indices_)[indices_->size () / 2]];
87  Eigen::Array4f dy1dy2 = (p1.getArray4fMap () - p0.getArray4fMap ()) / (p2.getArray4fMap () - p0.getArray4fMap ());
88  while (!( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) ) )
89  {
90  p0 = input_->points[(*indices_)[rand () % indices_->size ()]];
91  p1 = input_->points[(*indices_)[rand () % indices_->size ()]];
92  p2 = input_->points[(*indices_)[rand () % indices_->size ()]];
93  dy1dy2 = (p1.getArray4fMap () - p0.getArray4fMap ()) / (p2.getArray4fMap () - p0.getArray4fMap ());
94  }
95 
96  pcl::PointCloud<PointInT> normal_calc_cloud;
97  normal_calc_cloud.points.resize (3);
98  normal_calc_cloud.points[0] = p0;
99  normal_calc_cloud.points[1] = p1;
100  normal_calc_cloud.points[2] = p2;
101 
102  Eigen::Vector4d normal_calc_centroid;
103  Eigen::Matrix3d normal_calc_covariance;
104  pcl::computeMeanAndCovarianceMatrix (normal_calc_cloud, normal_calc_covariance, normal_calc_centroid);
105  // Need to set -1 here. See eigen33 for explanations.
106  Eigen::Vector3d::Scalar eigen_value;
107  Eigen::Vector3d plane_params;
108  pcl::eigen33 (normal_calc_covariance, eigen_value, plane_params);
109  float theta_x = fabsf (static_cast<float> (plane_params.dot (x_axis_)));
110  float theta_y = fabsf (static_cast<float> (plane_params.dot (y_axis_)));
111  float theta_z = fabsf (static_cast<float> (plane_params.dot (z_axis_)));
112 
113  // Check for degenerate cases of each projection
114  // We must avoid projections in which the plane projects as a line
115  if (theta_z > projection_angle_thresh_)
116  {
117  xz_proj_safe = false;
118  yz_proj_safe = false;
119  }
120  if (theta_x > projection_angle_thresh_)
121  {
122  xz_proj_safe = false;
123  xy_proj_safe = false;
124  }
125  if (theta_y > projection_angle_thresh_)
126  {
127  xy_proj_safe = false;
128  yz_proj_safe = false;
129  }
130 
131  // True if qhull should free points in qh_freeqhull() or reallocation
132  boolT ismalloc = True;
133  // output from qh_produce_output(), use NULL to skip qh_produce_output()
134  FILE *outfile = NULL;
135 
136 #ifndef HAVE_QHULL_2011
137  if (compute_area_)
138  outfile = stderr;
139 #endif
140 
141  // option flags for qhull, see qh_opt.htm
142  const char* flags = qhull_flags.c_str ();
143  // error messages from qhull code
144  FILE *errfile = stderr;
145 
146  // Array of coordinates for each point
147  coordT *points = reinterpret_cast<coordT*> (calloc (indices_->size () * dimension, sizeof (coordT)));
148 
149  // Build input data, using appropriate projection
150  int j = 0;
151  if (xy_proj_safe)
152  {
153  for (size_t i = 0; i < indices_->size (); ++i, j+=dimension)
154  {
155  points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x);
156  points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].y);
157  }
158  }
159  else if (yz_proj_safe)
160  {
161  for (size_t i = 0; i < indices_->size (); ++i, j+=dimension)
162  {
163  points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].y);
164  points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].z);
165  }
166  }
167  else if (xz_proj_safe)
168  {
169  for (size_t i = 0; i < indices_->size (); ++i, j+=dimension)
170  {
171  points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x);
172  points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].z);
173  }
174  }
175  else
176  {
177  // This should only happen if we had invalid input
178  PCL_ERROR ("[pcl::%s::performReconstruction2D] Invalid input!\n", getClassName ().c_str ());
179  }
180 
181  // Compute convex hull
182  int exitcode = qh_new_qhull (dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
183 #ifdef HAVE_QHULL_2011
184  if (compute_area_)
185  {
186  qh_prepare_output();
187  }
188 #endif
189 
190  // 0 if no error from qhull or it doesn't find any vertices
191  if (exitcode != 0 || qh num_vertices == 0)
192  {
193  PCL_ERROR ("[pcl::%s::performReconstrution2D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%lu)!\n", getClassName ().c_str (), indices_->size ());
194 
195  hull.points.resize (0);
196  hull.width = hull.height = 0;
197  polygons.resize (0);
198 
199  qh_freeqhull (!qh_ALL);
200  int curlong, totlong;
201  qh_memfreeshort (&curlong, &totlong);
202 
203  return;
204  }
205 
206  // Qhull returns the area in volume for 2D
207  if (compute_area_)
208  {
209  total_area_ = qh totvol;
210  total_volume_ = 0.0;
211  }
212 
213  int num_vertices = qh num_vertices;
214  hull.points.resize (num_vertices);
215  memset (&hull.points[0], static_cast<int> (hull.points.size ()), sizeof (PointInT));
216 
217  vertexT * vertex;
218  int i = 0;
219 
220  std::vector<std::pair<int, Eigen::Vector4f>, Eigen::aligned_allocator<std::pair<int, Eigen::Vector4f> > > idx_points (num_vertices);
221  idx_points.resize (hull.points.size ());
222  memset (&idx_points[0], static_cast<int> (hull.points.size ()), sizeof (std::pair<int, Eigen::Vector4f>));
223 
224  FORALLvertices
225  {
226  hull.points[i] = input_->points[(*indices_)[qh_pointid (vertex->point)]];
227  idx_points[i].first = qh_pointid (vertex->point);
228  ++i;
229  }
230 
231  // Sort
232  Eigen::Vector4f centroid;
233  pcl::compute3DCentroid (hull, centroid);
234  if (xy_proj_safe)
235  {
236  for (size_t j = 0; j < hull.points.size (); j++)
237  {
238  idx_points[j].second[0] = hull.points[j].x - centroid[0];
239  idx_points[j].second[1] = hull.points[j].y - centroid[1];
240  }
241  }
242  else if (yz_proj_safe)
243  {
244  for (size_t j = 0; j < hull.points.size (); j++)
245  {
246  idx_points[j].second[0] = hull.points[j].y - centroid[1];
247  idx_points[j].second[1] = hull.points[j].z - centroid[2];
248  }
249  }
250  else if (xz_proj_safe)
251  {
252  for (size_t j = 0; j < hull.points.size (); j++)
253  {
254  idx_points[j].second[0] = hull.points[j].x - centroid[0];
255  idx_points[j].second[1] = hull.points[j].z - centroid[2];
256  }
257  }
258  std::sort (idx_points.begin (), idx_points.end (), comparePoints2D);
259 
260  polygons.resize (1);
261  polygons[0].vertices.resize (hull.points.size ());
262 
263  hull_indices_.header = input_->header;
264  hull_indices_.indices.clear ();
265  hull_indices_.indices.reserve (hull.points.size ());
266 
267  for (int j = 0; j < static_cast<int> (hull.points.size ()); j++)
268  {
269  hull_indices_.indices.push_back ((*indices_)[idx_points[j].first]);
270  hull.points[j] = input_->points[(*indices_)[idx_points[j].first]];
271  polygons[0].vertices[j] = static_cast<unsigned int> (j);
272  }
273 
274  qh_freeqhull (!qh_ALL);
275  int curlong, totlong;
276  qh_memfreeshort (&curlong, &totlong);
277 
278  hull.width = static_cast<uint32_t> (hull.points.size ());
279  hull.height = 1;
280  hull.is_dense = false;
281  return;
282 }
283 
284 #ifdef __GNUC__
285 #pragma GCC diagnostic ignored "-Wold-style-cast"
286 #endif
287 //////////////////////////////////////////////////////////////////////////
288 template <typename PointInT> void
290  PointCloud &hull, std::vector<pcl::Vertices> &polygons, bool fill_polygon_data)
291 {
292  int dimension = 3;
293 
294  // True if qhull should free points in qh_freeqhull() or reallocation
295  boolT ismalloc = True;
296  // output from qh_produce_output(), use NULL to skip qh_produce_output()
297  FILE *outfile = NULL;
298 
299 #ifndef HAVE_QHULL_2011
300  if (compute_area_)
301  outfile = stderr;
302 #endif
303 
304  // option flags for qhull, see qh_opt.htm
305  const char *flags = qhull_flags.c_str ();
306  // error messages from qhull code
307  FILE *errfile = stderr;
308 
309  // Array of coordinates for each point
310  coordT *points = reinterpret_cast<coordT*> (calloc (indices_->size () * dimension, sizeof (coordT)));
311 
312  int j = 0;
313  for (size_t i = 0; i < indices_->size (); ++i, j+=dimension)
314  {
315  points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x);
316  points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].y);
317  points[j + 2] = static_cast<coordT> (input_->points[(*indices_)[i]].z);
318  }
319 
320  // Compute convex hull
321  int exitcode = qh_new_qhull (dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
322 #ifdef HAVE_QHULL_2011
323  if (compute_area_)
324  {
325  qh_prepare_output();
326  }
327 #endif
328 
329  // 0 if no error from qhull
330  if (exitcode != 0)
331  {
332  PCL_ERROR ("[pcl::%s::performReconstrution3D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%lu)!\n", getClassName ().c_str (), input_->points.size ());
333 
334  hull.points.resize (0);
335  hull.width = hull.height = 0;
336  polygons.resize (0);
337 
338  qh_freeqhull (!qh_ALL);
339  int curlong, totlong;
340  qh_memfreeshort (&curlong, &totlong);
341 
342  return;
343  }
344 
345  qh_triangulate ();
346 
347  int num_facets = qh num_facets;
348 
349  int num_vertices = qh num_vertices;
350  hull.points.resize (num_vertices);
351 
352  vertexT * vertex;
353  int i = 0;
354  // Max vertex id
355  unsigned int max_vertex_id = 0;
356  FORALLvertices
357  {
358  if (vertex->id + 1 > max_vertex_id)
359  max_vertex_id = vertex->id + 1;
360  }
361 
362  ++max_vertex_id;
363  std::vector<int> qhid_to_pcidx (max_vertex_id);
364 
365  hull_indices_.header = input_->header;
366  hull_indices_.indices.clear ();
367  hull_indices_.indices.reserve (num_vertices);
368 
369  FORALLvertices
370  {
371  // Add vertices to hull point_cloud and store index
372  hull_indices_.indices.push_back ((*indices_)[qh_pointid (vertex->point)]);
373  hull.points[i] = input_->points[(*indices_)[hull_indices_.indices.back ()]];
374 
375  qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index
376  ++i;
377  }
378 
379  if (compute_area_)
380  {
381  total_area_ = qh totarea;
382  total_volume_ = qh totvol;
383  }
384 
385  if (fill_polygon_data)
386  {
387  polygons.resize (num_facets);
388  int dd = 0;
389 
390  facetT * facet;
391  FORALLfacets
392  {
393  polygons[dd].vertices.resize (3);
394 
395  // Needed by FOREACHvertex_i_
396  int vertex_n, vertex_i;
397  FOREACHvertex_i_ ((*facet).vertices)
398  //facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]);
399  polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
400  ++dd;
401  }
402  }
403  // Deallocates memory (also the points)
404  qh_freeqhull (!qh_ALL);
405  int curlong, totlong;
406  qh_memfreeshort (&curlong, &totlong);
407 
408  hull.width = static_cast<uint32_t> (hull.points.size ());
409  hull.height = 1;
410  hull.is_dense = false;
411 }
412 #ifdef __GNUC__
413 #pragma GCC diagnostic warning "-Wold-style-cast"
414 #endif
415 
416 //////////////////////////////////////////////////////////////////////////
417 template <typename PointInT> void
418 pcl::ConvexHull<PointInT>::performReconstruction (PointCloud &hull, std::vector<pcl::Vertices> &polygons,
419  bool fill_polygon_data)
420 {
421  if (dimension_ == 0)
422  calculateInputDimension ();
423  if (dimension_ == 2)
424  performReconstruction2D (hull, polygons, fill_polygon_data);
425  else if (dimension_ == 3)
426  performReconstruction3D (hull, polygons, fill_polygon_data);
427  else
428  PCL_ERROR ("[pcl::%s::performReconstruction] Error: invalid input dimension requested: %d\n",getClassName ().c_str (),dimension_);
429 }
430 
431 //////////////////////////////////////////////////////////////////////////
432 template <typename PointInT> void
434 {
435  points.header = input_->header;
436  if (!initCompute () || input_->points.empty () || indices_->empty ())
437  {
438  points.points.clear ();
439  return;
440  }
441 
442  // Perform the actual surface reconstruction
443  std::vector<pcl::Vertices> polygons;
444  performReconstruction (points, polygons, false);
445 
446  points.width = static_cast<uint32_t> (points.points.size ());
447  points.height = 1;
448  points.is_dense = true;
449 
450  deinitCompute ();
451 }
452 
453 
454 //////////////////////////////////////////////////////////////////////////
455 template <typename PointInT> void
457 {
458  // Perform reconstruction
459  pcl::PointCloud<PointInT> hull_points;
460  performReconstruction (hull_points, output.polygons, true);
461 
462  // Convert the PointCloud into a PCLPointCloud2
463  pcl::toPCLPointCloud2 (hull_points, output.cloud);
464 }
465 
466 //////////////////////////////////////////////////////////////////////////
467 template <typename PointInT> void
468 pcl::ConvexHull<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
469 {
470  pcl::PointCloud<PointInT> hull_points;
471  performReconstruction (hull_points, polygons, true);
472 }
473 
474 //////////////////////////////////////////////////////////////////////////
475 template <typename PointInT> void
476 pcl::ConvexHull<PointInT>::reconstruct (PointCloud &points, std::vector<pcl::Vertices> &polygons)
477 {
478  points.header = input_->header;
479  if (!initCompute () || input_->points.empty () || indices_->empty ())
480  {
481  points.points.clear ();
482  return;
483  }
484 
485  // Perform the actual surface reconstruction
486  performReconstruction (points, polygons, true);
487 
488  points.width = static_cast<uint32_t> (points.points.size ());
489  points.height = 1;
490  points.is_dense = true;
491 
492  deinitCompute ();
493 }
494 //////////////////////////////////////////////////////////////////////////
495 template <typename PointInT> void
497 {
498  hull_point_indices = hull_indices_;
499 }
500 
501 #define PCL_INSTANTIATE_ConvexHull(T) template class PCL_EXPORTS pcl::ConvexHull<T>;
502 
503 #endif // PCL_SURFACE_IMPL_CONVEX_HULL_H_
504 #endif
void performReconstruction(PointCloud &points, std::vector< pcl::Vertices > &polygons, bool fill_polygon_data=false)
The actual reconstruction method.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
struct pcl::PointXYZIEdge EIGEN_ALIGN16
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
Definition: centroid.hpp:490
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
void calculateInputDimension()
Automatically determines the dimension of input data - 2D or 3D.
Definition: convex_hull.hpp:57
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
void performReconstruction3D(PointCloud &points, std::vector< pcl::Vertices > &polygons, bool fill_polygon_data=false)
The reconstruction method for 3D data.
void performReconstruction2D(PointCloud &points, std::vector< pcl::Vertices > &polygons, bool fill_polygon_data=false)
The reconstruction method for 2D data.
Definition: convex_hull.hpp:75
void getHullPointIndices(pcl::PointIndices &hull_point_indices) const
Retrieve the indices of the input point cloud that for the convex hull.
std::vector< ::pcl::Vertices > polygons
Definition: PolygonMesh.h:24
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
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
void reconstruct(PointCloud &points, std::vector< pcl::Vertices > &polygons)
Compute a convex hull for all points given.
::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:239
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
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 comparePoints2D(const std::pair< int, Eigen::Vector4f > &p1, const std::pair< int, Eigen::Vector4f > &p2)
Sort 2D points in a vector structure.
Definition: convex_hull.h:59