40 #include <pcl/pcl_config.h> 43 #ifndef PCL_SURFACE_IMPL_CONCAVE_HULL_H_ 44 #define PCL_SURFACE_IMPL_CONCAVE_HULL_H_ 47 #include <pcl/surface/concave_hull.h> 49 #include <pcl/common/eigen.h> 51 #include <pcl/common/transforms.h> 52 #include <pcl/kdtree/kdtree_flann.h> 53 #include <pcl/common/io.h> 56 #include <pcl/surface/qhull.h> 62 template <
typename Po
intInT>
int 65 return (getDimension ());
69 template <
typename Po
intInT>
void 72 output.
header = input_->header;
75 PCL_ERROR (
"[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
87 std::vector<pcl::Vertices> polygons;
88 performReconstruction (output, polygons);
90 output.
width = static_cast<uint32_t> (output.
points.size ());
98 template <
typename Po
intInT>
void 101 output.
header = input_->header;
104 PCL_ERROR (
"[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
116 performReconstruction (output, polygons);
118 output.
width = static_cast<uint32_t> (output.
points.size ());
126 #pragma GCC diagnostic ignored "-Wold-style-cast" 129 template <
typename Po
intInT>
void 132 Eigen::Vector4d xyz_centroid;
138 for (
int i = 0; i < 3; ++i)
139 for (
int j = 0; j < 3; ++j)
140 if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
145 pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
147 Eigen::Affine3d transform1;
148 transform1.setIdentity ();
153 PCL_DEBUG (
"[pcl::%s] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ());
154 if (std::abs (eigen_values[0]) < std::numeric_limits<double>::epsilon () || std::abs (eigen_values[0] / eigen_values[2]) < 1.0e-3)
165 transform1 (2, 0) = eigen_vectors (0, 0);
166 transform1 (2, 1) = eigen_vectors (1, 0);
167 transform1 (2, 2) = eigen_vectors (2, 0);
169 transform1 (1, 0) = eigen_vectors (0, 1);
170 transform1 (1, 1) = eigen_vectors (1, 1);
171 transform1 (1, 2) = eigen_vectors (2, 1);
172 transform1 (0, 0) = eigen_vectors (0, 2);
173 transform1 (0, 1) = eigen_vectors (1, 2);
174 transform1 (0, 2) = eigen_vectors (2, 2);
178 transform1.setIdentity ();
186 boolT ismalloc = True;
188 char flags[] =
"qhull d QJ";
190 FILE *outfile = NULL;
192 FILE *errfile = stderr;
197 coordT *points = reinterpret_cast<coordT*> (calloc (cloud_transformed.
points.size () * dim_,
sizeof(coordT)));
199 for (
size_t i = 0; i < cloud_transformed.
points.size (); ++i)
201 points[i * dim_ + 0] = static_cast<coordT> (cloud_transformed.
points[i].x);
202 points[i * dim_ + 1] = static_cast<coordT> (cloud_transformed.
points[i].y);
205 points[i * dim_ + 2] = static_cast<coordT> (cloud_transformed.
points[i].z);
209 exitcode = qh_new_qhull (dim_, static_cast<int> (cloud_transformed.
points.size ()), points, ismalloc, flags, outfile, errfile);
213 PCL_ERROR (
"[pcl::%s::performReconstrution] ERROR: qhull was unable to compute a concave hull for the given point cloud (%lu)!\n", getClassName ().c_str (), cloud_transformed.
points.size ());
218 bool NaNvalues =
false;
219 for (
size_t i = 0; i < cloud_transformed.
size (); ++i)
221 if (!pcl_isfinite (cloud_transformed.
points[i].x) ||
222 !pcl_isfinite (cloud_transformed.
points[i].y) ||
223 !pcl_isfinite (cloud_transformed.
points[i].z))
231 PCL_ERROR (
"[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
234 alpha_shape.
points.resize (0);
238 qh_freeqhull (!qh_ALL);
239 int curlong, totlong;
240 qh_memfreeshort (&curlong, &totlong);
245 qh_setvoronoi_all ();
247 int num_vertices = qh num_vertices;
248 alpha_shape.
points.resize (num_vertices);
252 int max_vertex_id = 0;
255 if (vertex->id + 1 > max_vertex_id)
256 max_vertex_id = vertex->id + 1;
262 std::vector<int> qhid_to_pcidx (max_vertex_id);
264 int num_facets = qh num_facets;
269 setT *triangles_set = qh_settemp (4 * num_facets);
270 if (voronoi_centers_)
271 voronoi_centers_->points.resize (num_facets);
277 if (!facet->upperdelaunay)
279 vertexT *anyVertex = static_cast<vertexT*> (facet->vertices->e[0].p);
280 double *center = facet->center;
281 double r = qh_pointdist (anyVertex->point,center,dim_);
284 if (voronoi_centers_)
286 voronoi_centers_->points[non_upper].x = static_cast<float> (facet->center[0]);
287 voronoi_centers_->points[non_upper].y = static_cast<float> (facet->center[1]);
288 voronoi_centers_->points[non_upper].z = static_cast<float> (facet->center[2]);
296 qh_makeridges (facet);
298 facet->visitid = qh visit_id;
299 ridgeT *ridge, **ridgep;
300 FOREACHridge_ (facet->ridges)
302 neighb = otherfacet_ (ridge, facet);
303 if ((neighb->visitid != qh visit_id))
304 qh_setappend (&triangles_set, ridge);
311 facet->visitid = qh visit_id;
312 qh_makeridges (facet);
313 ridgeT *ridge, **ridgep;
314 FOREACHridge_ (facet->ridges)
317 neighb = otherfacet_ (ridge, facet);
318 if ((neighb->visitid != qh visit_id))
323 a.x = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[0].p))->point[0]);
324 a.y = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[0].p))->point[1]);
325 a.z = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[0].p))->point[2]);
326 b.x = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[1].p))->point[0]);
327 b.y = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[1].p))->point[1]);
328 b.z = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[1].p))->point[2]);
329 c.x = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[2].p))->point[0]);
330 c.y = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[2].p))->point[1]);
331 c.z = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[2].p))->point[2]);
335 qh_setappend (&triangles_set, ridge);
342 if (voronoi_centers_)
343 voronoi_centers_->points.resize (non_upper);
347 int num_good_triangles = 0;
348 ridgeT *ridge, **ridgep;
349 FOREACHridge_ (triangles_set)
351 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
352 num_good_triangles++;
355 polygons.resize (num_good_triangles);
358 std::vector<bool> added_vertices (max_vertex_id,
false);
361 FOREACHridge_ (triangles_set)
363 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
365 polygons[triangles].vertices.resize (3);
366 int vertex_n, vertex_i;
367 FOREACHvertex_i_ ((*ridge).vertices)
369 if (!added_vertices[vertex->id])
371 alpha_shape.
points[vertices].x = static_cast<float> (vertex->point[0]);
372 alpha_shape.
points[vertices].y = static_cast<float> (vertex->point[1]);
373 alpha_shape.
points[vertices].z = static_cast<float> (vertex->point[2]);
375 qhid_to_pcidx[vertex->id] = vertices;
376 added_vertices[vertex->id] =
true;
380 polygons[triangles].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
388 alpha_shape.
points.resize (vertices);
389 alpha_shape.
width = static_cast<uint32_t> (alpha_shape.
points.size ());
396 setT *edges_set = qh_settemp (3 * num_facets);
397 if (voronoi_centers_)
398 voronoi_centers_->points.resize (num_facets);
403 if (!facet->upperdelaunay)
407 vertexT *anyVertex = static_cast<vertexT*>(facet->vertices->e[0].p);
408 double r = (sqrt ((anyVertex->point[0] - facet->center[0]) *
409 (anyVertex->point[0] - facet->center[0]) +
410 (anyVertex->point[1] - facet->center[1]) *
411 (anyVertex->point[1] - facet->center[1])));
415 qh_makeridges (facet);
418 ridgeT *ridge, **ridgep;
419 FOREACHridge_ (facet->ridges)
420 qh_setappend (&edges_set, ridge);
422 if (voronoi_centers_)
424 voronoi_centers_->points[dd].x = static_cast<float> (facet->center[0]);
425 voronoi_centers_->points[dd].y = static_cast<float> (facet->center[1]);
426 voronoi_centers_->points[dd].z = 0.0f;
437 std::vector<bool> added_vertices (max_vertex_id,
false);
438 std::map<int, std::vector<int> > edges;
440 ridgeT *ridge, **ridgep;
441 FOREACHridge_ (edges_set)
443 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
445 int vertex_n, vertex_i;
446 int vertices_in_ridge=0;
447 std::vector<int> pcd_indices;
448 pcd_indices.resize (2);
450 FOREACHvertex_i_ ((*ridge).vertices)
452 if (!added_vertices[vertex->id])
454 alpha_shape.
points[vertices].x = static_cast<float> (vertex->point[0]);
455 alpha_shape.
points[vertices].y = static_cast<float> (vertex->point[1]);
458 alpha_shape.
points[vertices].z = static_cast<float> (vertex->point[2]);
460 alpha_shape.
points[vertices].z = 0;
462 qhid_to_pcidx[vertex->id] = vertices;
463 added_vertices[vertex->id] =
true;
464 pcd_indices[vertices_in_ridge] = vertices;
469 pcd_indices[vertices_in_ridge] = qhid_to_pcidx[vertex->id];
476 edges[pcd_indices[0]].push_back (pcd_indices[1]);
477 edges[pcd_indices[1]].push_back (pcd_indices[0]);
481 alpha_shape.
points.resize (vertices);
483 std::vector<std::vector<int> > connected;
485 alpha_shape_sorted.
points.resize (vertices);
488 std::map<int, std::vector<int> >::iterator curr = edges.begin ();
490 std::vector<bool> used (vertices,
false);
491 std::vector<int> pcd_idx_start_polygons;
492 pcd_idx_start_polygons.push_back (0);
496 while (!edges.empty ())
498 alpha_shape_sorted.
points[sorted_idx] = alpha_shape.
points[(*curr).first];
500 for (
size_t i = 0; i < (*curr).second.size (); i++)
502 if (!used[((*curr).second)[i]])
505 next = ((*curr).second)[i];
510 used[(*curr).first] =
true;
519 curr = edges.find (next);
520 if (curr == edges.end ())
523 curr = edges.begin ();
524 pcd_idx_start_polygons.push_back (sorted_idx);
528 pcd_idx_start_polygons.push_back (sorted_idx);
532 polygons.reserve (pcd_idx_start_polygons.size () - 1);
534 for (
size_t poly_id = 0; poly_id < pcd_idx_start_polygons.size () - 1; poly_id++)
537 if (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id] >= 3)
540 vertices.
vertices.resize (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id]);
542 for (
int j = pcd_idx_start_polygons[poly_id]; j < pcd_idx_start_polygons[poly_id + 1]; ++j)
543 vertices.
vertices[j - pcd_idx_start_polygons[poly_id]] = static_cast<uint32_t> (j);
545 polygons.push_back (vertices);
549 if (voronoi_centers_)
550 voronoi_centers_->points.resize (dd);
553 qh_freeqhull (!qh_ALL);
554 int curlong, totlong;
555 qh_memfreeshort (&curlong, &totlong);
557 Eigen::Affine3d transInverse = transform1.inverse ();
559 xyz_centroid[0] = - xyz_centroid[0];
560 xyz_centroid[1] = - xyz_centroid[1];
561 xyz_centroid[2] = - xyz_centroid[2];
565 if (voronoi_centers_)
571 if (keep_information_)
577 std::vector<int> neighbor;
578 std::vector<float> distances;
580 distances.resize (1);
583 hull_indices_.header = input_->header;
584 hull_indices_.indices.clear ();
585 hull_indices_.indices.reserve (alpha_shape.
points.size ());
587 for (
size_t i = 0; i < alpha_shape.
points.size (); i++)
590 hull_indices_.indices.push_back (neighbor[0]);
598 #pragma GCC diagnostic warning "-Wold-style-cast" 602 template <
typename Po
intInT>
void 607 performReconstruction (hull_points, output.
polygons);
614 template <
typename Po
intInT>
void 618 performReconstruction (hull_points, polygons);
622 template <
typename Po
intInT>
void 625 hull_point_indices = hull_indices_;
628 #define PCL_INSTANTIATE_ConcaveHull(T) template class PCL_EXPORTS pcl::ConcaveHull<T>; 630 #endif // PCL_SURFACE_IMPL_CONCAVE_HULL_H_ KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
struct pcl::PointXYZIEdge EIGEN_ALIGN16
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
std::vector< uint32_t > vertices
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
uint32_t height
The point cloud height (if organized as an image-structure).
Describes a set of vertices in a polygon mesh, by basically storing an array of indices.
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 transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
void demeanPointCloud(ConstCloudIterator< PointT > &cloud_iterator, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, pcl::PointCloud< PointT > &cloud_out, int npts=0)
Subtract a centroid from a point cloud and return the de-meaned representation.
void reconstruct(PointCloud &points, std::vector< pcl::Vertices > &polygons)
Compute a concave hull for all points given.
void getHullPointIndices(pcl::PointIndices &hull_point_indices) const
Retrieve the indices of the input point cloud that for the convex hull.
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...
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for k-nearest neighbors for the given query point.
::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.
double getCircumcircleRadius(const PointT &pa, const PointT &pb, const PointT &pc)
Compute the radius of a circumscribed circle for a triangle formed of three points pa,...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void performReconstruction(PointCloud &points, std::vector< pcl::Vertices > &polygons)
The actual reconstruction method.
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.
int getDim() const
Returns the dimensionality (2 or 3) of the calculated hull.
Define methods for centroid estimation and covariance matrix calculus.