40 #include <pcl/pcl_config.h>
43 #ifndef PCL_SURFACE_IMPL_CONVEX_HULL_H_
44 #define PCL_SURFACE_IMPL_CONVEX_HULL_H_
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>
53 #include <pcl/surface/qhull.h>
56 template <
typename Po
intInT>
void
59 PCL_DEBUG (
"[pcl::%s::calculateInputDimension] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ());
61 Eigen::Vector4d xyz_centroid;
67 if (eigen_values[0] / eigen_values[2] < 1.0e-3)
74 template <
typename Po
intInT>
void
79 bool xy_proj_safe =
true;
80 bool yz_proj_safe =
true;
81 bool xz_proj_safe =
true;
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]) ) )
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 ());
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;
102 Eigen::Vector4d normal_calc_centroid;
103 Eigen::Matrix3d normal_calc_covariance;
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_)));
115 if (theta_z > projection_angle_thresh_)
117 xz_proj_safe =
false;
118 yz_proj_safe =
false;
120 if (theta_x > projection_angle_thresh_)
122 xz_proj_safe =
false;
123 xy_proj_safe =
false;
125 if (theta_y > projection_angle_thresh_)
127 xy_proj_safe =
false;
128 yz_proj_safe =
false;
132 boolT ismalloc = True;
134 FILE *outfile = NULL;
136 #ifndef HAVE_QHULL_2011
142 const char* flags = qhull_flags.c_str ();
144 FILE *errfile = stderr;
147 coordT *points =
reinterpret_cast<coordT*
> (calloc (indices_->size () * dimension,
sizeof (coordT)));
153 for (
size_t i = 0; i < indices_->size (); ++i, j+=dimension)
155 points[j + 0] =
static_cast<coordT
> (input_->points[(*indices_)[i]].x);
156 points[j + 1] =
static_cast<coordT
> (input_->points[(*indices_)[i]].y);
159 else if (yz_proj_safe)
161 for (
size_t i = 0; i < indices_->size (); ++i, j+=dimension)
163 points[j + 0] =
static_cast<coordT
> (input_->points[(*indices_)[i]].y);
164 points[j + 1] =
static_cast<coordT
> (input_->points[(*indices_)[i]].z);
167 else if (xz_proj_safe)
169 for (
size_t i = 0; i < indices_->size (); ++i, j+=dimension)
171 points[j + 0] =
static_cast<coordT
> (input_->points[(*indices_)[i]].x);
172 points[j + 1] =
static_cast<coordT
> (input_->points[(*indices_)[i]].z);
178 PCL_ERROR (
"[pcl::%s::performReconstruction2D] Invalid input!\n", getClassName ().c_str ());
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
191 if (exitcode != 0 || qh num_vertices == 0)
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 ());
199 qh_freeqhull (!qh_ALL);
200 int curlong, totlong;
201 qh_memfreeshort (&curlong, &totlong);
209 total_area_ = qh totvol;
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));
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>));
226 hull.
points[i] = input_->points[(*indices_)[qh_pointid (vertex->point)]];
227 idx_points[i].first = qh_pointid (vertex->point);
232 Eigen::Vector4f centroid;
236 for (
size_t j = 0; j < hull.
points.size (); j++)
238 idx_points[j].second[0] = hull.
points[j].x - centroid[0];
239 idx_points[j].second[1] = hull.
points[j].y - centroid[1];
242 else if (yz_proj_safe)
244 for (
size_t j = 0; j < hull.
points.size (); j++)
246 idx_points[j].second[0] = hull.
points[j].y - centroid[1];
247 idx_points[j].second[1] = hull.
points[j].z - centroid[2];
250 else if (xz_proj_safe)
252 for (
size_t j = 0; j < hull.
points.size (); j++)
254 idx_points[j].second[0] = hull.
points[j].x - centroid[0];
255 idx_points[j].second[1] = hull.
points[j].z - centroid[2];
261 polygons[0].vertices.resize (hull.
points.size ());
263 for (
int j = 0; j < static_cast<int> (hull.
points.size ()); j++)
265 hull.
points[j] = input_->points[(*indices_)[idx_points[j].first]];
266 polygons[0].vertices[j] =
static_cast<unsigned int> (j);
269 qh_freeqhull (!qh_ALL);
270 int curlong, totlong;
271 qh_memfreeshort (&curlong, &totlong);
273 hull.
width =
static_cast<uint32_t
> (hull.
points.size ());
280 #pragma GCC diagnostic ignored "-Wold-style-cast"
283 template <
typename Po
intInT>
void
285 PointCloud &hull, std::vector<pcl::Vertices> &polygons,
bool fill_polygon_data)
290 boolT ismalloc = True;
292 FILE *outfile = NULL;
294 #ifndef HAVE_QHULL_2011
300 const char *flags = qhull_flags.c_str ();
302 FILE *errfile = stderr;
305 coordT *points =
reinterpret_cast<coordT*
> (calloc (indices_->size () * dimension,
sizeof (coordT)));
308 for (
size_t i = 0; i < indices_->size (); ++i, j+=dimension)
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);
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
327 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 qh_freeqhull (!qh_ALL);
334 int curlong, totlong;
335 qh_memfreeshort (&curlong, &totlong);
342 int num_facets = qh num_facets;
344 int num_vertices = qh num_vertices;
345 hull.
points.resize (num_vertices);
350 unsigned int max_vertex_id = 0;
353 if (vertex->id + 1 > max_vertex_id)
354 max_vertex_id = vertex->id + 1;
358 std::vector<int> qhid_to_pcidx (max_vertex_id);
363 hull.
points[i] = input_->points[(*indices_)[qh_pointid (vertex->point)]];
365 qhid_to_pcidx[vertex->id] = i;
371 total_area_ = qh totarea;
372 total_volume_ = qh totvol;
375 if (fill_polygon_data)
377 polygons.resize (num_facets);
383 polygons[dd].vertices.resize (3);
386 int vertex_n, vertex_i;
387 FOREACHvertex_i_ ((*facet).vertices)
389 polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
394 qh_freeqhull (!qh_ALL);
395 int curlong, totlong;
396 qh_memfreeshort (&curlong, &totlong);
398 hull.
width =
static_cast<uint32_t
> (hull.
points.size ());
403 #pragma GCC diagnostic warning "-Wold-style-cast"
407 template <
typename Po
intInT>
void
409 bool fill_polygon_data)
412 calculateInputDimension ();
414 performReconstruction2D (hull, polygons, fill_polygon_data);
415 else if (dimension_ == 3)
416 performReconstruction3D (hull, polygons, fill_polygon_data);
418 PCL_ERROR (
"[pcl::%s::performReconstruction] Error: invalid input dimension requested: %d\n",getClassName ().c_str (),dimension_);
422 template <
typename Po
intInT>
void
425 points.
header = input_->header;
426 if (!initCompute () || input_->points.empty () || indices_->empty ())
433 std::vector<pcl::Vertices> polygons;
434 performReconstruction (points, polygons,
false);
436 points.
width =
static_cast<uint32_t
> (points.
points.size ());
445 template <
typename Po
intInT>
void
450 performReconstruction (hull_points, output.
polygons,
true);
457 template <
typename Po
intInT>
void
461 performReconstruction (hull_points, polygons,
true);
465 template <
typename Po
intInT>
void
468 points.
header = input_->header;
469 if (!initCompute () || input_->points.empty () || indices_->empty ())
476 performReconstruction (points, polygons,
true);
478 points.
width =
static_cast<uint32_t
> (points.
points.size ());
485 #define PCL_INSTANTIATE_ConvexHull(T) template class PCL_EXPORTS pcl::ConvexHull<T>;
487 #endif // PCL_SURFACE_IMPL_CONVEX_HULL_H_
void performReconstruction(PointCloud &points, std::vector< pcl::Vertices > &polygons, bool fill_polygon_data=false)
The actual reconstruction method.
struct pcl::_PointXYZHSV EIGEN_ALIGN16
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
void calculateInputDimension()
Automatically determines the dimension of input data - 2D or 3D.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.
std::vector< ::pcl::Vertices > polygons
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...
void reconstruct(PointCloud &points, std::vector< pcl::Vertices > &polygons)
Compute a convex hull for all points given.
::pcl::PCLPointCloud2 cloud
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
bool comparePoints2D(const std::pair< int, Eigen::Vector4f > &p1, const std::pair< int, Eigen::Vector4f > &p2)
Sort 2D points in a vector structure.
uint32_t height
The point cloud height (if organized as an image-structure).