Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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 <cstdio>
52#include <cstdlib>
53#include <pcl/surface/qhull.h>
54
55//////////////////////////////////////////////////////////////////////////
56template <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::Vector4d xyz_centroid;
61 compute3DCentroid (*input_, *indices_, xyz_centroid);
62 EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix = Eigen::Matrix3d::Zero ();
63 computeCovarianceMatrixNormalized (*input_, *indices_, xyz_centroid, covariance_matrix);
64
65 EIGEN_ALIGN16 Eigen::Vector3d eigen_values;
66 pcl::eigen33 (covariance_matrix, eigen_values);
67
68 if (std::abs (eigen_values[0]) < std::numeric_limits<double>::epsilon () || std::abs (eigen_values[0] / eigen_values[2]) < 1.0e-3)
69 dimension_ = 2;
70 else
71 dimension_ = 3;
72}
73
74//////////////////////////////////////////////////////////////////////////
75template <typename PointInT> void
76pcl::ConvexHull<PointInT>::performReconstruction2D (PointCloud &hull, std::vector<pcl::Vertices> &polygons,
77 bool)
78{
79 int dimension = 2;
80 bool xy_proj_safe = true;
81 bool yz_proj_safe = true;
82 bool xz_proj_safe = true;
83
84 // Check the input's normal to see which projection to use
85 PointInT p0 = (*input_)[(*indices_)[0]];
86 PointInT p1 = (*input_)[(*indices_)[indices_->size () - 1]];
87 PointInT p2 = (*input_)[(*indices_)[indices_->size () / 2]];
88 while (!pcl::isXYZFinite(p0) || !pcl::isXYZFinite(p1) || !pcl::isXYZFinite(p2) ||
89 (p1.getVector3fMap() - p0.getVector3fMap()).cross(p2.getVector3fMap() - p0.getVector3fMap()).stableNorm() < Eigen::NumTraits<float>::dummy_precision ())
90 {
91 p0 = (*input_)[(*indices_)[rand () % indices_->size ()]];
92 p1 = (*input_)[(*indices_)[rand () % indices_->size ()]];
93 p2 = (*input_)[(*indices_)[rand () % indices_->size ()]];
94 }
95
96 pcl::PointCloud<PointInT> normal_calc_cloud;
97 normal_calc_cloud.resize (3);
98 normal_calc_cloud[0] = p0;
99 normal_calc_cloud[1] = p1;
100 normal_calc_cloud[2] = p2;
101
102 Eigen::Vector4d normal_calc_centroid;
103 Eigen::Matrix3d normal_calc_covariance;
104 pcl::compute3DCentroid (normal_calc_cloud, normal_calc_centroid);
105 pcl::computeCovarianceMatrixNormalized (normal_calc_cloud, normal_calc_centroid, normal_calc_covariance);
106
107 // Need to set -1 here. See eigen33 for explanations.
108 Eigen::Vector3d::Scalar eigen_value;
109 Eigen::Vector3d plane_params;
110 pcl::eigen33 (normal_calc_covariance, eigen_value, plane_params);
111 float theta_x = std::abs (static_cast<float> (plane_params.dot (x_axis_)));
112 float theta_y = std::abs (static_cast<float> (plane_params.dot (y_axis_)));
113 float theta_z = std::abs (static_cast<float> (plane_params.dot (z_axis_)));
114
115 // Check for degenerate cases of each projection
116 // We must avoid projections in which the plane projects as a line
117 if (theta_z > projection_angle_thresh_)
118 {
119 xz_proj_safe = false;
120 yz_proj_safe = false;
121 }
122 if (theta_x > projection_angle_thresh_)
123 {
124 xz_proj_safe = false;
125 xy_proj_safe = false;
126 }
127 if (theta_y > projection_angle_thresh_)
128 {
129 xy_proj_safe = false;
130 yz_proj_safe = false;
131 }
132
133 // True if qhull should free points in qh_freeqhull() or reallocation
134 boolT ismalloc = True;
135 // output from qh_produce_output(), use NULL to skip qh_produce_output()
136 FILE *outfile = nullptr;
137
139 outfile = stderr;
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 (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension)
154 {
155 points[j + 0] = static_cast<coordT> ((*input_)[(*indices_)[i]].x);
156 points[j + 1] = static_cast<coordT> ((*input_)[(*indices_)[i]].y);
157 }
158 }
159 else if (yz_proj_safe)
160 {
161 for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension)
162 {
163 points[j + 0] = static_cast<coordT> ((*input_)[(*indices_)[i]].y);
164 points[j + 1] = static_cast<coordT> ((*input_)[(*indices_)[i]].z);
165 }
166 }
167 else if (xz_proj_safe)
168 {
169 for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension)
170 {
171 points[j + 0] = static_cast<coordT> ((*input_)[(*indices_)[i]].x);
172 points[j + 1] = static_cast<coordT> ((*input_)[(*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 qhT qh_qh;
182 qhT* qh = &qh_qh;
183 QHULL_LIB_CHECK
184 qh_zero(qh, errfile);
185
186 // Compute convex hull
187 int exitcode = qh_new_qhull (qh, dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
188 if (compute_area_)
189 {
190 qh_prepare_output(qh);
191 }
192
193 // 0 if no error from qhull or it doesn't find any vertices
194 if (exitcode != 0 || qh->num_vertices == 0)
195 {
196 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 ());
197
198 hull.resize (0);
199 hull.width = hull.height = 0;
200 polygons.resize (0);
201
202 qh_freeqhull (qh, !qh_ALL);
203 int curlong, totlong;
204 qh_memfreeshort (qh, &curlong, &totlong);
205
206 return;
207 }
208
209 // Qhull returns the area in volume for 2D
210 if (compute_area_)
211 {
212 total_area_ = qh->totvol;
213 total_volume_ = 0.0;
214 }
215
216 int num_vertices = qh->num_vertices;
217
218 hull.clear();
219 hull.resize(num_vertices, PointInT{});
220
221 vertexT * vertex;
222 int i = 0;
223
224 AlignedVector<std::pair<int, Eigen::Vector4f>> idx_points (num_vertices);
225
226 FORALLvertices
227 {
228 hull[i] = (*input_)[(*indices_)[qh_pointid (qh, vertex->point)]];
229 idx_points[i].first = qh_pointid (qh, vertex->point);
230 ++i;
231 }
232
233 // Sort
234 Eigen::Vector4f centroid;
235 pcl::compute3DCentroid (hull, centroid);
236 if (xy_proj_safe)
237 {
238 for (std::size_t j = 0; j < hull.size (); j++)
239 {
240 idx_points[j].second[0] = hull[j].x - centroid[0];
241 idx_points[j].second[1] = hull[j].y - centroid[1];
242 }
243 }
244 else if (yz_proj_safe)
245 {
246 for (std::size_t j = 0; j < hull.size (); j++)
247 {
248 idx_points[j].second[0] = hull[j].y - centroid[1];
249 idx_points[j].second[1] = hull[j].z - centroid[2];
250 }
251 }
252 else if (xz_proj_safe)
253 {
254 for (std::size_t j = 0; j < hull.size (); j++)
255 {
256 idx_points[j].second[0] = hull[j].x - centroid[0];
257 idx_points[j].second[1] = hull[j].z - centroid[2];
258 }
259 }
260 std::sort (idx_points.begin (), idx_points.end (), comparePoints2D);
261
262 polygons.resize (1);
263 polygons[0].vertices.resize (hull.size ());
264
265 hull_indices_.header = input_->header;
266 hull_indices_.indices.clear ();
267 hull_indices_.indices.reserve (hull.size ());
268
269 for (int j = 0; j < static_cast<int> (hull.size ()); j++)
270 {
271 hull_indices_.indices.push_back ((*indices_)[idx_points[j].first]);
272 hull[j] = (*input_)[(*indices_)[idx_points[j].first]];
273 polygons[0].vertices[j] = static_cast<unsigned int> (j);
274 }
275
276 qh_freeqhull (qh, !qh_ALL);
277 int curlong, totlong;
278 qh_memfreeshort (qh, &curlong, &totlong);
279
280 hull.width = hull.size ();
281 hull.height = 1;
282 hull.is_dense = false;
283 return;
284}
285
286#ifdef __GNUC__
287#pragma GCC diagnostic ignored "-Wold-style-cast"
288#endif
289//////////////////////////////////////////////////////////////////////////
290template <typename PointInT> void
292 PointCloud &hull, std::vector<pcl::Vertices> &polygons, bool fill_polygon_data)
293{
294 int dimension = 3;
295
296 // True if qhull should free points in qh_freeqhull() or reallocation
297 boolT ismalloc = True;
298 // output from qh_produce_output(), use NULL to skip qh_produce_output()
299 FILE *outfile = nullptr;
300
302 outfile = stderr;
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 (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension)
314 {
315 points[j + 0] = static_cast<coordT> ((*input_)[(*indices_)[i]].x);
316 points[j + 1] = static_cast<coordT> ((*input_)[(*indices_)[i]].y);
317 points[j + 2] = static_cast<coordT> ((*input_)[(*indices_)[i]].z);
318 }
319
320 qhT qh_qh;
321 qhT* qh = &qh_qh;
322 QHULL_LIB_CHECK
323 qh_zero(qh, errfile);
324
325 // Compute convex hull
326 int exitcode = qh_new_qhull (qh, dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
327 if (compute_area_)
328 {
329 qh_prepare_output(qh);
330 }
331
332 // 0 if no error from qhull
333 if (exitcode != 0)
334 {
335 PCL_ERROR("[pcl::%s::performReconstrution3D] ERROR: qhull was unable to compute a "
336 "convex hull for the given point cloud (%zu)!\n",
337 getClassName().c_str(),
338 static_cast<std::size_t>(input_->size()));
339
340 hull.resize (0);
341 hull.width = hull.height = 0;
342 polygons.resize (0);
343
344 qh_freeqhull (qh, !qh_ALL);
345 int curlong, totlong;
346 qh_memfreeshort (qh, &curlong, &totlong);
347
348 return;
349 }
350
351 qh_triangulate (qh);
352
353 int num_facets = qh->num_facets;
354
355 int num_vertices = qh->num_vertices;
356 hull.resize (num_vertices);
357
358 vertexT * vertex;
359 int i = 0;
360 // Max vertex id
361 unsigned int max_vertex_id = 0;
362 FORALLvertices
363 {
364 if (vertex->id + 1 > max_vertex_id)
365 max_vertex_id = vertex->id + 1;
366 }
367
368 ++max_vertex_id;
369 std::vector<int> qhid_to_pcidx (max_vertex_id);
370
371 hull_indices_.header = input_->header;
372 hull_indices_.indices.clear ();
373 hull_indices_.indices.reserve (num_vertices);
374
375 FORALLvertices
376 {
377 // Add vertices to hull point_cloud and store index
378 hull_indices_.indices.push_back ((*indices_)[qh_pointid (qh, vertex->point)]);
379 hull[i] = (*input_)[hull_indices_.indices.back ()];
380
381 qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index
382 ++i;
383 }
384
385 if (compute_area_)
386 {
387 total_area_ = qh->totarea;
388 total_volume_ = qh->totvol;
389 }
390
391 if (fill_polygon_data)
392 {
393 polygons.resize (num_facets);
394 int dd = 0;
395
396 facetT * facet;
397 FORALLfacets
398 {
399 polygons[dd].vertices.resize (3);
400
401 // Needed by FOREACHvertex_i_
402 int vertex_n, vertex_i;
403 FOREACHvertex_i_ (qh, (*facet).vertices)
404 //facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]);
405 polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
406 ++dd;
407 }
408 }
409 // Deallocates memory (also the points)
410 qh_freeqhull (qh, !qh_ALL);
411 int curlong, totlong;
412 qh_memfreeshort (qh, &curlong, &totlong);
413
414 hull.width = hull.size ();
415 hull.height = 1;
416 hull.is_dense = false;
417}
418#ifdef __GNUC__
419#pragma GCC diagnostic warning "-Wold-style-cast"
420#endif
421
422//////////////////////////////////////////////////////////////////////////
423template <typename PointInT> void
424pcl::ConvexHull<PointInT>::performReconstruction (PointCloud &hull, std::vector<pcl::Vertices> &polygons,
425 bool fill_polygon_data)
426{
427 if (dimension_ == 0)
428 calculateInputDimension ();
429 if (dimension_ == 2)
430 performReconstruction2D (hull, polygons, fill_polygon_data);
431 else if (dimension_ == 3)
432 performReconstruction3D (hull, polygons, fill_polygon_data);
433 else
434 PCL_ERROR ("[pcl::%s::performReconstruction] Error: invalid input dimension requested: %d\n",getClassName ().c_str (),dimension_);
435}
436
437//////////////////////////////////////////////////////////////////////////
438template <typename PointInT> void
440{
441 points.header = input_->header;
442 if (!initCompute () || input_->points.empty () || indices_->empty ())
443 {
444 points.clear ();
445 return;
446 }
447
448 // Perform the actual surface reconstruction
449 std::vector<pcl::Vertices> polygons;
450 performReconstruction (points, polygons, false);
451
452 points.width = points.size ();
453 points.height = 1;
454 points.is_dense = true;
455
456 deinitCompute ();
457}
458
459
460//////////////////////////////////////////////////////////////////////////
461template <typename PointInT> void
463{
464 // Perform reconstruction
465 pcl::PointCloud<PointInT> hull_points;
466 performReconstruction (hull_points, output.polygons, true);
467
468 // Convert the PointCloud into a PCLPointCloud2
469 pcl::toPCLPointCloud2 (hull_points, output.cloud);
470}
471
472//////////////////////////////////////////////////////////////////////////
473template <typename PointInT> void
474pcl::ConvexHull<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
475{
476 pcl::PointCloud<PointInT> hull_points;
477 performReconstruction (hull_points, polygons, true);
478}
479
480//////////////////////////////////////////////////////////////////////////
481template <typename PointInT> void
482pcl::ConvexHull<PointInT>::reconstruct (PointCloud &points, std::vector<pcl::Vertices> &polygons)
483{
484 points.header = input_->header;
485 if (!initCompute () || input_->points.empty () || indices_->empty ())
486 {
487 points.clear ();
488 return;
489 }
490
491 // Perform the actual surface reconstruction
492 performReconstruction (points, polygons, true);
493
494 points.width = points.size ();
495 points.height = 1;
496 points.is_dense = true;
497
498 deinitCompute ();
499}
500//////////////////////////////////////////////////////////////////////////
501template <typename PointInT> void
503{
504 hull_point_indices = hull_indices_;
505}
506
507#define PCL_INSTANTIATE_ConvexHull(T) template class PCL_EXPORTS pcl::ConvexHull<T>;
508
509#endif // PCL_SURFACE_IMPL_CONVEX_HULL_H_
510#endif
void calculateInputDimension()
Automatically determines the dimension of input data - 2D or 3D.
void performReconstruction2D(PointCloud &points, std::vector< pcl::Vertices > &polygons, bool fill_polygon_data=false)
The reconstruction method for 2D data.
void getHullPointIndices(pcl::PointIndices &hull_point_indices) const
Retrieve the indices of the input point cloud that for the convex hull.
void performReconstruction(PointCloud &points, std::vector< pcl::Vertices > &polygons, bool fill_polygon_data=false)
The actual reconstruction method.
void reconstruct(PointCloud &points, std::vector< pcl::Vertices > &polygons)
Compute a convex hull for all points given.
void performReconstruction3D(PointCloud &points, std::vector< pcl::Vertices > &polygons, bool fill_polygon_data=false)
The reconstruction method for 3D data.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
void clear()
Removes all points in a cloud and sets the width and height to 0.
std::size_t size() const
Define standard C methods and C++ classes that are common to all methods.
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:269
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:295
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:57
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
PCL_EXPORTS bool isVerbosityLevelEnabled(VERBOSITY_LEVEL severity)
is verbosity level enabled?
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg, bool padding)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
std::vector< T, Eigen::aligned_allocator< T > > AlignedVector
Type used for aligned vector of Eigen objects in PCL.
Definition types.h:139
constexpr bool isXYZFinite(const PointT &) noexcept
std::vector< ::pcl::Vertices > polygons
Definition PolygonMesh.h:22
::pcl::PCLPointCloud2 cloud
Definition PolygonMesh.h:20