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> 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 ());
60 Eigen::Vector4d xyz_centroid;
62 EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix = Eigen::Matrix3d::Zero ();
68 if (std::abs (eigen_values[0]) < std::numeric_limits<double>::epsilon () || std::abs (eigen_values[0] / eigen_values[2]) < 1.0e-3)
75 template <
typename Po
intInT>
void 80 bool xy_proj_safe =
true;
81 bool yz_proj_safe =
true;
82 bool xz_proj_safe =
true;
85 PointInT p0 = input_->points[(*indices_)[0]];
86 PointInT p1 = input_->points[(*indices_)[indices_->size () - 1]];
87 PointInT p2 = input_->points[(*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]) ) )
91 p0 = input_->points[(*indices_)[rand () % indices_->size ()]];
92 p1 = input_->points[(*indices_)[rand () % indices_->size ()]];
93 p2 = input_->points[(*indices_)[rand () % indices_->size ()]];
94 dy1dy2 = (p1.getArray4fMap () - p0.getArray4fMap ()) / (p2.getArray4fMap () - p0.getArray4fMap ());
98 normal_calc_cloud.
points.resize (3);
99 normal_calc_cloud.
points[0] = p0;
100 normal_calc_cloud.
points[1] = p1;
101 normal_calc_cloud.
points[2] = p2;
103 Eigen::Vector4d normal_calc_centroid;
104 Eigen::Matrix3d normal_calc_covariance;
109 Eigen::Vector3d::Scalar eigen_value;
110 Eigen::Vector3d plane_params;
111 pcl::eigen33 (normal_calc_covariance, eigen_value, plane_params);
112 float theta_x = fabsf (static_cast<float> (plane_params.dot (x_axis_)));
113 float theta_y = fabsf (static_cast<float> (plane_params.dot (y_axis_)));
114 float theta_z = fabsf (static_cast<float> (plane_params.dot (z_axis_)));
118 if (theta_z > projection_angle_thresh_)
120 xz_proj_safe =
false;
121 yz_proj_safe =
false;
123 if (theta_x > projection_angle_thresh_)
125 xz_proj_safe =
false;
126 xy_proj_safe =
false;
128 if (theta_y > projection_angle_thresh_)
130 xy_proj_safe =
false;
131 yz_proj_safe =
false;
135 boolT ismalloc = True;
137 FILE *outfile = NULL;
139 #ifndef HAVE_QHULL_2011 145 const char* flags = qhull_flags.c_str ();
147 FILE *errfile = stderr;
150 coordT *points = reinterpret_cast<coordT*> (calloc (indices_->size () * dimension,
sizeof (coordT)));
156 for (
size_t i = 0; i < indices_->size (); ++i, j+=dimension)
158 points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x);
159 points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].y);
162 else if (yz_proj_safe)
164 for (
size_t i = 0; i < indices_->size (); ++i, j+=dimension)
166 points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].y);
167 points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].z);
170 else if (xz_proj_safe)
172 for (
size_t i = 0; i < indices_->size (); ++i, j+=dimension)
174 points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x);
175 points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].z);
181 PCL_ERROR (
"[pcl::%s::performReconstruction2D] Invalid input!\n", getClassName ().c_str ());
185 int exitcode = qh_new_qhull (dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
186 #ifdef HAVE_QHULL_2011 194 if (exitcode != 0 || qh num_vertices == 0)
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 ());
202 qh_freeqhull (!qh_ALL);
203 int curlong, totlong;
204 qh_memfreeshort (&curlong, &totlong);
212 total_area_ = qh totvol;
216 int num_vertices = qh num_vertices;
217 hull.
points.resize (num_vertices);
218 memset (&hull.
points[0], static_cast<int> (hull.
points.size ()),
sizeof (PointInT));
223 std::vector<std::pair<int, Eigen::Vector4f>, Eigen::aligned_allocator<std::pair<int, Eigen::Vector4f> > > idx_points (num_vertices);
224 idx_points.resize (hull.
points.size ());
225 memset (&idx_points[0], static_cast<int> (hull.
points.size ()),
sizeof (std::pair<int, Eigen::Vector4f>));
229 hull.
points[i] = input_->points[(*indices_)[qh_pointid (vertex->point)]];
230 idx_points[i].first = qh_pointid (vertex->point);
235 Eigen::Vector4f centroid;
239 for (
size_t j = 0; j < hull.
points.size (); j++)
241 idx_points[j].second[0] = hull.
points[j].x - centroid[0];
242 idx_points[j].second[1] = hull.
points[j].y - centroid[1];
245 else if (yz_proj_safe)
247 for (
size_t j = 0; j < hull.
points.size (); j++)
249 idx_points[j].second[0] = hull.
points[j].y - centroid[1];
250 idx_points[j].second[1] = hull.
points[j].z - centroid[2];
253 else if (xz_proj_safe)
255 for (
size_t j = 0; j < hull.
points.size (); j++)
257 idx_points[j].second[0] = hull.
points[j].x - centroid[0];
258 idx_points[j].second[1] = hull.
points[j].z - centroid[2];
264 polygons[0].vertices.resize (hull.
points.size ());
266 hull_indices_.header = input_->header;
267 hull_indices_.indices.clear ();
268 hull_indices_.indices.reserve (hull.
points.size ());
270 for (
int j = 0; j < static_cast<int> (hull.
points.size ()); j++)
272 hull_indices_.indices.push_back ((*indices_)[idx_points[j].first]);
273 hull.
points[j] = input_->points[(*indices_)[idx_points[j].first]];
274 polygons[0].vertices[j] = static_cast<unsigned int> (j);
277 qh_freeqhull (!qh_ALL);
278 int curlong, totlong;
279 qh_memfreeshort (&curlong, &totlong);
281 hull.
width = static_cast<uint32_t> (hull.
points.size ());
288 #pragma GCC diagnostic ignored "-Wold-style-cast" 291 template <
typename Po
intInT>
void 293 PointCloud &hull, std::vector<pcl::Vertices> &polygons,
bool fill_polygon_data)
298 boolT ismalloc = True;
300 FILE *outfile = NULL;
302 #ifndef HAVE_QHULL_2011 308 const char *flags = qhull_flags.c_str ();
310 FILE *errfile = stderr;
313 coordT *points = reinterpret_cast<coordT*> (calloc (indices_->size () * dimension,
sizeof (coordT)));
316 for (
size_t i = 0; i < indices_->size (); ++i, j+=dimension)
318 points[j + 0] = static_cast<coordT> (input_->points[(*indices_)[i]].x);
319 points[j + 1] = static_cast<coordT> (input_->points[(*indices_)[i]].y);
320 points[j + 2] = static_cast<coordT> (input_->points[(*indices_)[i]].z);
324 int exitcode = qh_new_qhull (dimension, static_cast<int> (indices_->size ()), points, ismalloc, const_cast<char*> (flags), outfile, errfile);
325 #ifdef HAVE_QHULL_2011 335 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 ());
341 qh_freeqhull (!qh_ALL);
342 int curlong, totlong;
343 qh_memfreeshort (&curlong, &totlong);
350 int num_facets = qh num_facets;
352 int num_vertices = qh num_vertices;
353 hull.
points.resize (num_vertices);
358 unsigned int max_vertex_id = 0;
361 if (vertex->id + 1 > max_vertex_id)
362 max_vertex_id = vertex->id + 1;
366 std::vector<int> qhid_to_pcidx (max_vertex_id);
368 hull_indices_.header = input_->header;
369 hull_indices_.indices.clear ();
370 hull_indices_.indices.reserve (num_vertices);
375 hull_indices_.indices.push_back ((*indices_)[qh_pointid (vertex->point)]);
376 hull.
points[i] = input_->points[(*indices_)[hull_indices_.indices.back ()]];
378 qhid_to_pcidx[vertex->id] = i;
384 total_area_ = qh totarea;
385 total_volume_ = qh totvol;
388 if (fill_polygon_data)
390 polygons.resize (num_facets);
396 polygons[dd].vertices.resize (3);
399 int vertex_n, vertex_i;
400 FOREACHvertex_i_ ((*facet).vertices)
402 polygons[dd].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
407 qh_freeqhull (!qh_ALL);
408 int curlong, totlong;
409 qh_memfreeshort (&curlong, &totlong);
411 hull.
width = static_cast<uint32_t> (hull.
points.size ());
416 #pragma GCC diagnostic warning "-Wold-style-cast" 420 template <
typename Po
intInT>
void 422 bool fill_polygon_data)
425 calculateInputDimension ();
427 performReconstruction2D (hull, polygons, fill_polygon_data);
428 else if (dimension_ == 3)
429 performReconstruction3D (hull, polygons, fill_polygon_data);
431 PCL_ERROR (
"[pcl::%s::performReconstruction] Error: invalid input dimension requested: %d\n",getClassName ().c_str (),dimension_);
435 template <
typename Po
intInT>
void 438 points.
header = input_->header;
439 if (!initCompute () || input_->points.empty () || indices_->empty ())
446 std::vector<pcl::Vertices> polygons;
447 performReconstruction (points, polygons,
false);
449 points.
width = static_cast<uint32_t> (points.
points.size ());
458 template <
typename Po
intInT>
void 463 performReconstruction (hull_points, output.
polygons,
true);
470 template <
typename Po
intInT>
void 474 performReconstruction (hull_points, polygons,
true);
478 template <
typename Po
intInT>
void 481 points.
header = input_->header;
482 if (!initCompute () || input_->points.empty () || indices_->empty ())
489 performReconstruction (points, polygons,
true);
491 points.
width = static_cast<uint32_t> (points.
points.size ());
498 template <
typename Po
intInT>
void 501 hull_point_indices = hull_indices_;
504 #define PCL_INSTANTIATE_ConvexHull(T) template class PCL_EXPORTS pcl::ConvexHull<T>; 506 #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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
struct pcl::PointXYZIEdge EIGEN_ALIGN16
uint32_t height
The point cloud height (if organized as an image-structure).
void calculateInputDimension()
Automatically determines the dimension of input data - 2D or 3D.
Define standard C methods and C++ classes that are common to all methods.
uint32_t width
The point cloud width (if organized as an image-structure).
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
pcl::PCLHeader header
The point cloud header.
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.
void getHullPointIndices(pcl::PointIndices &hull_point_indices) const
Retrieve the indices of the input point cloud that for the convex hull.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
unsigned int computeCovarianceMatrixNormalized(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute normalized the 3x3 covariance matrix of a given set of points.
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 comparePoints2D(const std::pair< int, Eigen::Vector4f > &p1, const std::pair< int, Eigen::Vector4f > &p2)
Sort 2D points in a vector structure.