Point Cloud Library (PCL) 1.12.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
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 Eigen::Array4f dy1dy2 = (p1.getArray4fMap () - p0.getArray4fMap ()) / (p2.getArray4fMap () - p0.getArray4fMap ());
89 while (!( (dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1]) ) )
90 {
91 p0 = (*input_)[(*indices_)[rand () % indices_->size ()]];
92 p1 = (*input_)[(*indices_)[rand () % indices_->size ()]];
93 p2 = (*input_)[(*indices_)[rand () % indices_->size ()]];
94 dy1dy2 = (p1.getArray4fMap () - p0.getArray4fMap ()) / (p2.getArray4fMap () - p0.getArray4fMap ());
95 }
96
98 normal_calc_cloud.resize (3);
100 normal_calc_cloud[1] = p1;
101 normal_calc_cloud[2] = p2;
102
103 Eigen::Vector4d normal_calc_centroid;
104 Eigen::Matrix3d normal_calc_covariance;
107
108 // Need to set -1 here. See eigen33 for explanations.
109 Eigen::Vector3d::Scalar eigen_value;
110 Eigen::Vector3d plane_params;
112 float theta_x = std::abs (static_cast<float> (plane_params.dot (x_axis_)));
113 float theta_y = std::abs (static_cast<float> (plane_params.dot (y_axis_)));
114 float theta_z = std::abs (static_cast<float> (plane_params.dot (z_axis_)));
115
116 // Check for degenerate cases of each projection
117 // We must avoid projections in which the plane projects as a line
118 if (theta_z > projection_angle_thresh_)
119 {
120 xz_proj_safe = false;
121 yz_proj_safe = false;
122 }
123 if (theta_x > projection_angle_thresh_)
124 {
125 xz_proj_safe = false;
126 xy_proj_safe = false;
127 }
128 if (theta_y > projection_angle_thresh_)
129 {
130 xy_proj_safe = false;
131 yz_proj_safe = false;
132 }
133
134 // True if qhull should free points in qh_freeqhull() or reallocation
136 // output from qh_produce_output(), use NULL to skip qh_produce_output()
137 FILE *outfile = nullptr;
138
139 if (compute_area_)
140 outfile = stderr;
141
142 // option flags for qhull, see qh_opt.htm
143 const char* flags = qhull_flags.c_str ();
144 // error messages from qhull code
146
147 // Array of coordinates for each point
148 coordT *points = reinterpret_cast<coordT*> (calloc (indices_->size () * dimension, sizeof (coordT)));
149
150 // Build input data, using appropriate projection
151 int j = 0;
152 if (xy_proj_safe)
153 {
154 for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension)
155 {
156 points[j + 0] = static_cast<coordT> ((*input_)[(*indices_)[i]].x);
157 points[j + 1] = static_cast<coordT> ((*input_)[(*indices_)[i]].y);
158 }
159 }
160 else if (yz_proj_safe)
161 {
162 for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension)
163 {
164 points[j + 0] = static_cast<coordT> ((*input_)[(*indices_)[i]].y);
165 points[j + 1] = static_cast<coordT> ((*input_)[(*indices_)[i]].z);
166 }
167 }
168 else if (xz_proj_safe)
169 {
170 for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension)
171 {
172 points[j + 0] = static_cast<coordT> ((*input_)[(*indices_)[i]].x);
173 points[j + 1] = static_cast<coordT> ((*input_)[(*indices_)[i]].z);
174 }
175 }
176 else
177 {
178 // This should only happen if we had invalid input
179 PCL_ERROR ("[pcl::%s::performReconstruction2D] Invalid input!\n", getClassName ().c_str ());
180 }
181
182 qhT qh_qh;
183 qhT* qh = &qh_qh;
186
187 // Compute convex hull
188 int exitcode = qh_new_qhull (qh, dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
189 if (compute_area_)
190 {
192 }
193
194 // 0 if no error from qhull or it doesn't find any vertices
195 if (exitcode != 0 || qh->num_vertices == 0)
196 {
197 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 ());
198
199 hull.resize (0);
200 hull.width = hull.height = 0;
201 polygons.resize (0);
202
204 int curlong, totlong;
206
207 return;
208 }
209
210 // Qhull returns the area in volume for 2D
211 if (compute_area_)
212 {
213 total_area_ = qh->totvol;
214 total_volume_ = 0.0;
215 }
216
217 int num_vertices = qh->num_vertices;
218
219 hull.clear();
220 hull.resize(num_vertices, PointInT{});
221
222 vertexT * vertex;
223 int i = 0;
224
226
228 {
229 hull[i] = (*input_)[(*indices_)[qh_pointid (qh, vertex->point)]];
230 idx_points[i].first = qh_pointid (qh, vertex->point);
231 ++i;
232 }
233
234 // Sort
235 Eigen::Vector4f centroid;
236 pcl::compute3DCentroid (hull, centroid);
237 if (xy_proj_safe)
238 {
239 for (std::size_t j = 0; j < hull.size (); j++)
240 {
241 idx_points[j].second[0] = hull[j].x - centroid[0];
242 idx_points[j].second[1] = hull[j].y - centroid[1];
243 }
244 }
245 else if (yz_proj_safe)
246 {
247 for (std::size_t j = 0; j < hull.size (); j++)
248 {
249 idx_points[j].second[0] = hull[j].y - centroid[1];
250 idx_points[j].second[1] = hull[j].z - centroid[2];
251 }
252 }
253 else if (xz_proj_safe)
254 {
255 for (std::size_t j = 0; j < hull.size (); j++)
256 {
257 idx_points[j].second[0] = hull[j].x - centroid[0];
258 idx_points[j].second[1] = hull[j].z - centroid[2];
259 }
260 }
261 std::sort (idx_points.begin (), idx_points.end (), comparePoints2D);
262
263 polygons.resize (1);
264 polygons[0].vertices.resize (hull.size ());
265
266 hull_indices_.header = input_->header;
267 hull_indices_.indices.clear ();
268 hull_indices_.indices.reserve (hull.size ());
269
270 for (int j = 0; j < static_cast<int> (hull.size ()); j++)
271 {
272 hull_indices_.indices.push_back ((*indices_)[idx_points[j].first]);
273 hull[j] = (*input_)[(*indices_)[idx_points[j].first]];
274 polygons[0].vertices[j] = static_cast<unsigned int> (j);
275 }
276
278 int curlong, totlong;
280
281 hull.width = hull.size ();
282 hull.height = 1;
283 hull.is_dense = false;
284 return;
285}
286
287#ifdef __GNUC__
288#pragma GCC diagnostic ignored "-Wold-style-cast"
289#endif
290//////////////////////////////////////////////////////////////////////////
291template <typename PointInT> void
293 PointCloud &hull, std::vector<pcl::Vertices> &polygons, bool fill_polygon_data)
294{
295 int dimension = 3;
296
297 // True if qhull should free points in qh_freeqhull() or reallocation
299 // output from qh_produce_output(), use NULL to skip qh_produce_output()
300 FILE *outfile = nullptr;
301
302 if (compute_area_)
303 outfile = stderr;
304
305 // option flags for qhull, see qh_opt.htm
306 const char *flags = qhull_flags.c_str ();
307 // error messages from qhull code
309
310 // Array of coordinates for each point
311 coordT *points = reinterpret_cast<coordT*> (calloc (indices_->size () * dimension, sizeof (coordT)));
312
313 int j = 0;
314 for (std::size_t i = 0; i < indices_->size (); ++i, j+=dimension)
315 {
316 points[j + 0] = static_cast<coordT> ((*input_)[(*indices_)[i]].x);
317 points[j + 1] = static_cast<coordT> ((*input_)[(*indices_)[i]].y);
318 points[j + 2] = static_cast<coordT> ((*input_)[(*indices_)[i]].z);
319 }
320
321 qhT qh_qh;
322 qhT* qh = &qh_qh;
325
326 // Compute convex hull
327 int exitcode = qh_new_qhull (qh, dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
328 if (compute_area_)
329 {
331 }
332
333 // 0 if no error from qhull
334 if (exitcode != 0)
335 {
336 PCL_ERROR("[pcl::%s::performReconstrution3D] ERROR: qhull was unable to compute a "
337 "convex hull for the given point cloud (%zu)!\n",
338 getClassName().c_str(),
339 static_cast<std::size_t>(input_->size()));
340
341 hull.resize (0);
342 hull.width = hull.height = 0;
343 polygons.resize (0);
344
346 int curlong, totlong;
348
349 return;
350 }
351
353
354 int num_facets = qh->num_facets;
355
356 int num_vertices = qh->num_vertices;
357 hull.resize (num_vertices);
358
359 vertexT * vertex;
360 int i = 0;
361 // Max vertex id
362 unsigned int max_vertex_id = 0;
364 {
365 if (vertex->id + 1 > max_vertex_id)
366 max_vertex_id = vertex->id + 1;
367 }
368
370 std::vector<int> qhid_to_pcidx (max_vertex_id);
371
372 hull_indices_.header = input_->header;
373 hull_indices_.indices.clear ();
374 hull_indices_.indices.reserve (num_vertices);
375
377 {
378 // Add vertices to hull point_cloud and store index
379 hull_indices_.indices.push_back ((*indices_)[qh_pointid (qh, vertex->point)]);
380 hull[i] = (*input_)[hull_indices_.indices.back ()];
381
382 qhid_to_pcidx[vertex->id] = i; // map the vertex id of qhull to the point cloud index
383 ++i;
384 }
385
386 if (compute_area_)
387 {
388 total_area_ = qh->totarea;
389 total_volume_ = qh->totvol;
390 }
391
393 {
394 polygons.resize (num_facets);
395 int dd = 0;
396
397 facetT * facet;
399 {
400 polygons[dd].vertices.resize (3);
401
402 // Needed by FOREACHvertex_i_
403 int vertex_n, vertex_i;
404 FOREACHvertex_i_ (qh, (*facet).vertices)
405 //facet_vertices.vertices.push_back (qhid_to_pcidx[vertex->id]);
406 polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
407 ++dd;
408 }
409 }
410 // Deallocates memory (also the points)
412 int curlong, totlong;
414
415 hull.width = hull.size ();
416 hull.height = 1;
417 hull.is_dense = false;
418}
419#ifdef __GNUC__
420#pragma GCC diagnostic warning "-Wold-style-cast"
421#endif
422
423//////////////////////////////////////////////////////////////////////////
424template <typename PointInT> void
427{
428 if (dimension_ == 0)
429 calculateInputDimension ();
430 if (dimension_ == 2)
431 performReconstruction2D (hull, polygons, fill_polygon_data);
432 else if (dimension_ == 3)
433 performReconstruction3D (hull, polygons, fill_polygon_data);
434 else
435 PCL_ERROR ("[pcl::%s::performReconstruction] Error: invalid input dimension requested: %d\n",getClassName ().c_str (),dimension_);
436}
437
438//////////////////////////////////////////////////////////////////////////
439template <typename PointInT> void
441{
442 points.header = input_->header;
443 if (!initCompute () || input_->points.empty () || indices_->empty ())
444 {
445 points.clear ();
446 return;
447 }
448
449 // Perform the actual surface reconstruction
450 std::vector<pcl::Vertices> polygons;
451 performReconstruction (points, polygons, false);
452
453 points.width = points.size ();
454 points.height = 1;
455 points.is_dense = true;
456
457 deinitCompute ();
458}
459
460
461//////////////////////////////////////////////////////////////////////////
462template <typename PointInT> void
464{
465 // Perform reconstruction
467 performReconstruction (hull_points, output.polygons, true);
468
469 // Convert the PointCloud into a PCLPointCloud2
471}
472
473//////////////////////////////////////////////////////////////////////////
474template <typename PointInT> void
475pcl::ConvexHull<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
476{
478 performReconstruction (hull_points, polygons, true);
479}
480
481//////////////////////////////////////////////////////////////////////////
482template <typename PointInT> void
483pcl::ConvexHull<PointInT>::reconstruct (PointCloud &points, std::vector<pcl::Vertices> &polygons)
484{
485 points.header = input_->header;
486 if (!initCompute () || input_->points.empty () || indices_->empty ())
487 {
488 points.clear ();
489 return;
490 }
491
492 // Perform the actual surface reconstruction
493 performReconstruction (points, polygons, true);
494
495 points.width = points.size ();
496 points.height = 1;
497 points.is_dense = true;
498
499 deinitCompute ();
500}
501//////////////////////////////////////////////////////////////////////////
502template <typename PointInT> void
507
508#define PCL_INSTANTIATE_ConvexHull(T) template class PCL_EXPORTS pcl::ConvexHull<T>;
509
510#endif // PCL_SURFACE_IMPL_CONVEX_HULL_H_
511#endif
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
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).
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:251
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:296
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:56
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
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.