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>
59 template <
typename Po
intInT>
void
62 output.
header = input_->header;
65 PCL_ERROR (
"[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
77 std::vector<pcl::Vertices> polygons;
78 performReconstruction (output, polygons);
80 output.
width =
static_cast<uint32_t
> (output.
points.size ());
88 template <
typename Po
intInT>
void
91 output.
header = input_->header;
94 PCL_ERROR (
"[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
106 performReconstruction (output, polygons);
108 output.
width =
static_cast<uint32_t
> (output.
points.size ());
116 #pragma GCC diagnostic ignored "-Wold-style-cast"
119 template <
typename Po
intInT>
void
122 Eigen::Vector4d xyz_centroid;
124 EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix = Eigen::Matrix3d::Zero ();
128 for (
int i = 0; i < 3; ++i)
129 for (
int j = 0; j < 3; ++j)
130 if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
135 pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
137 Eigen::Affine3d transform1;
138 transform1.setIdentity ();
143 PCL_DEBUG (
"[pcl::%s] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ());
144 if (std::abs (eigen_values[0]) < std::numeric_limits<double>::epsilon () || std::abs (eigen_values[0] / eigen_values[2]) < 1.0e-3)
155 transform1 (2, 0) = eigen_vectors (0, 0);
156 transform1 (2, 1) = eigen_vectors (1, 0);
157 transform1 (2, 2) = eigen_vectors (2, 0);
159 transform1 (1, 0) = eigen_vectors (0, 1);
160 transform1 (1, 1) = eigen_vectors (1, 1);
161 transform1 (1, 2) = eigen_vectors (2, 1);
162 transform1 (0, 0) = eigen_vectors (0, 2);
163 transform1 (0, 1) = eigen_vectors (1, 2);
164 transform1 (0, 2) = eigen_vectors (2, 2);
168 transform1.setIdentity ();
176 boolT ismalloc = True;
178 char flags[] =
"qhull d QJ";
180 FILE *outfile = NULL;
182 FILE *errfile = stderr;
187 coordT *points =
reinterpret_cast<coordT*
> (calloc (cloud_transformed.
points.size () * dim_,
sizeof(coordT)));
189 for (
size_t i = 0; i < cloud_transformed.
points.size (); ++i)
191 points[i * dim_ + 0] =
static_cast<coordT
> (cloud_transformed.
points[i].x);
192 points[i * dim_ + 1] =
static_cast<coordT
> (cloud_transformed.
points[i].y);
195 points[i * dim_ + 2] =
static_cast<coordT
> (cloud_transformed.
points[i].z);
199 exitcode = qh_new_qhull (dim_,
static_cast<int> (cloud_transformed.
points.size ()), points, ismalloc, flags, outfile, errfile);
203 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 ());
208 bool NaNvalues =
false;
209 for (
size_t i = 0; i < cloud_transformed.
size (); ++i)
211 if (!pcl_isfinite (cloud_transformed.
points[i].x) ||
212 !pcl_isfinite (cloud_transformed.
points[i].y) ||
213 !pcl_isfinite (cloud_transformed.
points[i].z))
221 PCL_ERROR (
"[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
224 alpha_shape.
points.resize (0);
228 qh_freeqhull (!qh_ALL);
229 int curlong, totlong;
230 qh_memfreeshort (&curlong, &totlong);
235 qh_setvoronoi_all ();
237 int num_vertices = qh num_vertices;
238 alpha_shape.
points.resize (num_vertices);
242 int max_vertex_id = 0;
245 if (vertex->id + 1 > unsigned (max_vertex_id))
246 max_vertex_id = vertex->id + 1;
252 std::vector<int> qhid_to_pcidx (max_vertex_id);
254 int num_facets = qh num_facets;
259 setT *triangles_set = qh_settemp (4 * num_facets);
260 if (voronoi_centers_)
261 voronoi_centers_->points.resize (num_facets);
267 if (!facet->upperdelaunay)
269 vertexT *anyVertex =
static_cast<vertexT*
> (facet->vertices->e[0].p);
270 double *center = facet->center;
271 double r = qh_pointdist (anyVertex->point,center,dim_);
274 if (voronoi_centers_)
276 voronoi_centers_->points[non_upper].x =
static_cast<float> (facet->center[0]);
277 voronoi_centers_->points[non_upper].y =
static_cast<float> (facet->center[1]);
278 voronoi_centers_->points[non_upper].z =
static_cast<float> (facet->center[2]);
286 qh_makeridges (facet);
288 facet->visitid = qh visit_id;
289 ridgeT *ridge, **ridgep;
290 FOREACHridge_ (facet->ridges)
292 neighb = otherfacet_ (ridge, facet);
293 if ((neighb->visitid != qh visit_id))
294 qh_setappend (&triangles_set, ridge);
301 facet->visitid = qh visit_id;
302 qh_makeridges (facet);
303 ridgeT *ridge, **ridgep;
304 FOREACHridge_ (facet->ridges)
307 neighb = otherfacet_ (ridge, facet);
308 if ((neighb->visitid != qh visit_id))
313 a.x =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[0].p))->point[0]);
314 a.y =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[0].p))->point[1]);
315 a.z =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[0].p))->point[2]);
316 b.x =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[1].p))->point[0]);
317 b.y =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[1].p))->point[1]);
318 b.z =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[1].p))->point[2]);
319 c.x =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[2].p))->point[0]);
320 c.y =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[2].p))->point[1]);
321 c.z =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[2].p))->point[2]);
325 qh_setappend (&triangles_set, ridge);
332 if (voronoi_centers_)
333 voronoi_centers_->points.resize (non_upper);
337 int num_good_triangles = 0;
338 ridgeT *ridge, **ridgep;
339 FOREACHridge_ (triangles_set)
341 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
342 num_good_triangles++;
345 polygons.resize (num_good_triangles);
348 std::vector<bool> added_vertices (max_vertex_id,
false);
351 FOREACHridge_ (triangles_set)
353 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
355 polygons[triangles].vertices.resize (3);
356 int vertex_n, vertex_i;
357 FOREACHvertex_i_ ((*ridge).vertices)
359 if (!added_vertices[vertex->id])
361 alpha_shape.
points[vertices].x =
static_cast<float> (vertex->point[0]);
362 alpha_shape.
points[vertices].y =
static_cast<float> (vertex->point[1]);
363 alpha_shape.
points[vertices].z =
static_cast<float> (vertex->point[2]);
365 qhid_to_pcidx[vertex->id] = vertices;
366 added_vertices[vertex->id] =
true;
370 polygons[triangles].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
378 alpha_shape.
points.resize (vertices);
379 alpha_shape.
width =
static_cast<uint32_t
> (alpha_shape.
points.size ());
386 setT *edges_set = qh_settemp (3 * num_facets);
387 if (voronoi_centers_)
388 voronoi_centers_->points.resize (num_facets);
393 if (!facet->upperdelaunay)
397 vertexT *anyVertex =
static_cast<vertexT*
>(facet->vertices->e[0].p);
398 double r = (sqrt ((anyVertex->point[0] - facet->center[0]) *
399 (anyVertex->point[0] - facet->center[0]) +
400 (anyVertex->point[1] - facet->center[1]) *
401 (anyVertex->point[1] - facet->center[1])));
405 qh_makeridges (facet);
408 ridgeT *ridge, **ridgep;
409 FOREACHridge_ (facet->ridges)
410 qh_setappend (&edges_set, ridge);
412 if (voronoi_centers_)
414 voronoi_centers_->points[dd].x =
static_cast<float> (facet->center[0]);
415 voronoi_centers_->points[dd].y =
static_cast<float> (facet->center[1]);
416 voronoi_centers_->points[dd].z = 0.0f;
427 std::vector<bool> added_vertices (max_vertex_id,
false);
428 std::map<int, std::vector<int> > edges;
430 ridgeT *ridge, **ridgep;
431 FOREACHridge_ (edges_set)
433 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
435 int vertex_n, vertex_i;
436 int vertices_in_ridge=0;
437 std::vector<int> pcd_indices;
438 pcd_indices.resize (2);
440 FOREACHvertex_i_ ((*ridge).vertices)
442 if (!added_vertices[vertex->id])
444 alpha_shape.
points[vertices].x =
static_cast<float> (vertex->point[0]);
445 alpha_shape.
points[vertices].y =
static_cast<float> (vertex->point[1]);
448 alpha_shape.
points[vertices].z =
static_cast<float> (vertex->point[2]);
450 alpha_shape.
points[vertices].z = 0;
452 qhid_to_pcidx[vertex->id] = vertices;
453 added_vertices[vertex->id] =
true;
454 pcd_indices[vertices_in_ridge] = vertices;
459 pcd_indices[vertices_in_ridge] = qhid_to_pcidx[vertex->id];
466 edges[pcd_indices[0]].push_back (pcd_indices[1]);
467 edges[pcd_indices[1]].push_back (pcd_indices[0]);
471 alpha_shape.
points.resize (vertices);
473 std::vector<std::vector<int> > connected;
475 alpha_shape_sorted.
points.resize (vertices);
478 std::map<int, std::vector<int> >::iterator curr = edges.begin ();
480 std::vector<bool> used (vertices,
false);
481 std::vector<int> pcd_idx_start_polygons;
482 pcd_idx_start_polygons.push_back (0);
486 while (!edges.empty ())
488 alpha_shape_sorted.
points[sorted_idx] = alpha_shape.
points[(*curr).first];
490 for (
size_t i = 0; i < (*curr).second.size (); i++)
492 if (!used[((*curr).second)[i]])
495 next = ((*curr).second)[i];
500 used[(*curr).first] =
true;
509 curr = edges.find (next);
510 if (curr == edges.end ())
513 curr = edges.begin ();
514 pcd_idx_start_polygons.push_back (sorted_idx);
518 pcd_idx_start_polygons.push_back (sorted_idx);
522 polygons.reserve (pcd_idx_start_polygons.size () - 1);
524 for (
size_t poly_id = 0; poly_id < pcd_idx_start_polygons.size () - 1; poly_id++)
527 if (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id] >= 3)
530 vertices.
vertices.resize (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id]);
532 for (
int j = pcd_idx_start_polygons[poly_id]; j < pcd_idx_start_polygons[poly_id + 1]; ++j)
533 vertices.
vertices[j - pcd_idx_start_polygons[poly_id]] =
static_cast<uint32_t
> (j);
535 polygons.push_back (vertices);
539 if (voronoi_centers_)
540 voronoi_centers_->points.resize (dd);
543 qh_freeqhull (!qh_ALL);
544 int curlong, totlong;
545 qh_memfreeshort (&curlong, &totlong);
547 Eigen::Affine3d transInverse = transform1.inverse ();
549 xyz_centroid[0] = - xyz_centroid[0];
550 xyz_centroid[1] = - xyz_centroid[1];
551 xyz_centroid[2] = - xyz_centroid[2];
555 if (voronoi_centers_)
561 if (keep_information_)
567 std::vector<int> neighbor;
568 std::vector<float> distances;
570 distances.resize (1);
573 hull_indices_.header = input_->header;
574 hull_indices_.indices.clear ();
575 hull_indices_.indices.reserve (alpha_shape.
points.size ());
577 for (
size_t i = 0; i < alpha_shape.
points.size (); i++)
580 hull_indices_.indices.push_back (neighbor[0]);
588 #pragma GCC diagnostic warning "-Wold-style-cast"
592 template <
typename Po
intInT>
void
597 performReconstruction (hull_points, output.
polygons);
604 template <
typename Po
intInT>
void
608 performReconstruction (hull_points, polygons);
612 template <
typename Po
intInT>
void
615 hull_point_indices = hull_indices_;
618 #define PCL_INSTANTIATE_ConcaveHull(T) template class PCL_EXPORTS pcl::ConcaveHull<T>;
620 #endif // PCL_SURFACE_IMPL_CONCAVE_HULL_H_