Point Cloud Library (PCL)  1.7.1
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 (%zu)!\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  for (int j = 0; j < static_cast<int> (hull.points.size ()); j++)
264  {
265  hull.points[j] = input_->points[(*indices_)[idx_points[j].first]];
266  polygons[0].vertices[j] = static_cast<unsigned int> (j);
267  }
268 
269  qh_freeqhull (!qh_ALL);
270  int curlong, totlong;
271  qh_memfreeshort (&curlong, &totlong);
272 
273  hull.width = static_cast<uint32_t> (hull.points.size ());
274  hull.height = 1;
275  hull.is_dense = false;
276  return;
277 }
278 
279 #ifdef __GNUC__
280 #pragma GCC diagnostic ignored "-Wold-style-cast"
281 #endif
282 //////////////////////////////////////////////////////////////////////////
283 template <typename PointInT> void
285  PointCloud &hull, std::vector<pcl::Vertices> &polygons, bool fill_polygon_data)
286 {
287  int dimension = 3;
288 
289  // True if qhull should free points in qh_freeqhull() or reallocation
290  boolT ismalloc = True;
291  // output from qh_produce_output(), use NULL to skip qh_produce_output()
292  FILE *outfile = NULL;
293 
294 #ifndef HAVE_QHULL_2011
295  if (compute_area_)
296  outfile = stderr;
297 #endif
298 
299  // option flags for qhull, see qh_opt.htm
300  const char *flags = qhull_flags.c_str ();
301  // error messages from qhull code
302  FILE *errfile = stderr;
303 
304  // Array of coordinates for each point
305  coordT *points = reinterpret_cast<coordT*> (calloc (indices_->size () * dimension, sizeof (coordT)));
306 
307  int j = 0;
308  for (size_t i = 0; i < indices_->size (); ++i, j+=dimension)
309  {
310  points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x);
311  points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].y);
312  points[j + 2] = static_cast<coordT> (input_->points[(*indices_)[i]].z);
313  }
314 
315  // Compute convex hull
316  int exitcode = qh_new_qhull (dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
317 #ifdef HAVE_QHULL_2011
318  if (compute_area_)
319  {
320  qh_prepare_output();
321  }
322 #endif
323 
324  // 0 if no error from qhull
325  if (exitcode != 0)
326  {
327  PCL_ERROR ("[pcl::%s::performReconstrution3D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%zu)!\n", getClassName ().c_str (), input_->points.size ());
328 
329  hull.points.resize (0);
330  hull.width = hull.height = 0;
331  polygons.resize (0);
332 
333  qh_freeqhull (!qh_ALL);
334  int curlong, totlong;
335  qh_memfreeshort (&curlong, &totlong);
336 
337  return;
338  }
339 
340  qh_triangulate ();
341 
342  int num_facets = qh num_facets;
343 
344  int num_vertices = qh num_vertices;
345  hull.points.resize (num_vertices);
346 
347  vertexT * vertex;
348  int i = 0;
349  // Max vertex id
350  unsigned int max_vertex_id = 0;
351  FORALLvertices
352  {
353  if (vertex->id + 1 > max_vertex_id)
354  max_vertex_id = vertex->id + 1;
355  }
356 
357  ++max_vertex_id;
358  std::vector<int> qhid_to_pcidx (max_vertex_id);
359 
360  FORALLvertices
361  {
362  // Add vertices to hull point_cloud
363  hull.points[i] = input_->points[(*indices_)[qh_pointid (vertex->point)]];
364 
365  qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index
366  ++i;
367  }
368 
369  if (compute_area_)
370  {
371  total_area_ = qh totarea;
372  total_volume_ = qh totvol;
373  }
374 
375  if (fill_polygon_data)
376  {
377  polygons.resize (num_facets);
378  int dd = 0;
379 
380  facetT * facet;
381  FORALLfacets
382  {
383  polygons[dd].vertices.resize (3);
384 
385  // Needed by FOREACHvertex_i_
386  int vertex_n, vertex_i;
387  FOREACHvertex_i_ ((*facet).vertices)
388  //facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]);
389  polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
390  ++dd;
391  }
392  }
393  // Deallocates memory (also the points)
394  qh_freeqhull (!qh_ALL);
395  int curlong, totlong;
396  qh_memfreeshort (&curlong, &totlong);
397 
398  hull.width = static_cast<uint32_t> (hull.points.size ());
399  hull.height = 1;
400  hull.is_dense = false;
401 }
402 #ifdef __GNUC__
403 #pragma GCC diagnostic warning "-Wold-style-cast"
404 #endif
405 
406 //////////////////////////////////////////////////////////////////////////
407 template <typename PointInT> void
408 pcl::ConvexHull<PointInT>::performReconstruction (PointCloud &hull, std::vector<pcl::Vertices> &polygons,
409  bool fill_polygon_data)
410 {
411  if (dimension_ == 0)
412  calculateInputDimension ();
413  if (dimension_ == 2)
414  performReconstruction2D (hull, polygons, fill_polygon_data);
415  else if (dimension_ == 3)
416  performReconstruction3D (hull, polygons, fill_polygon_data);
417  else
418  PCL_ERROR ("[pcl::%s::performReconstruction] Error: invalid input dimension requested: %d\n",getClassName ().c_str (),dimension_);
419 }
420 
421 //////////////////////////////////////////////////////////////////////////
422 template <typename PointInT> void
424 {
425  points.header = input_->header;
426  if (!initCompute () || input_->points.empty () || indices_->empty ())
427  {
428  points.points.clear ();
429  return;
430  }
431 
432  // Perform the actual surface reconstruction
433  std::vector<pcl::Vertices> polygons;
434  performReconstruction (points, polygons, false);
435 
436  points.width = static_cast<uint32_t> (points.points.size ());
437  points.height = 1;
438  points.is_dense = true;
439 
440  deinitCompute ();
441 }
442 
443 
444 //////////////////////////////////////////////////////////////////////////
445 template <typename PointInT> void
447 {
448  // Perform reconstruction
449  pcl::PointCloud<PointInT> hull_points;
450  performReconstruction (hull_points, output.polygons, true);
451 
452  // Convert the PointCloud into a PCLPointCloud2
453  pcl::toPCLPointCloud2 (hull_points, output.cloud);
454 }
455 
456 //////////////////////////////////////////////////////////////////////////
457 template <typename PointInT> void
458 pcl::ConvexHull<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
459 {
460  pcl::PointCloud<PointInT> hull_points;
461  performReconstruction (hull_points, polygons, true);
462 }
463 
464 //////////////////////////////////////////////////////////////////////////
465 template <typename PointInT> void
466 pcl::ConvexHull<PointInT>::reconstruct (PointCloud &points, std::vector<pcl::Vertices> &polygons)
467 {
468  points.header = input_->header;
469  if (!initCompute () || input_->points.empty () || indices_->empty ())
470  {
471  points.points.clear ();
472  return;
473  }
474 
475  // Perform the actual surface reconstruction
476  performReconstruction (points, polygons, true);
477 
478  points.width = static_cast<uint32_t> (points.points.size ());
479  points.height = 1;
480  points.is_dense = true;
481 
482  deinitCompute ();
483 }
484 
485 #define PCL_INSTANTIATE_ConvexHull(T) template class PCL_EXPORTS pcl::ConvexHull<T>;
486 
487 #endif // PCL_SURFACE_IMPL_CONVEX_HULL_H_
488 #endif