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>
48 #include <pcl/common/common.h>
49 #include <pcl/common/eigen.h>
50 #include <pcl/common/centroid.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
133 Eigen::Vector4d xyz_centroid;
137 for (
int i = 0; i < 3; ++i)
138 for (
int j = 0; j < 3; ++j)
139 if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
144 pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
146 Eigen::Affine3d transform1;
147 transform1.setIdentity ();
152 PCL_DEBUG (
"[pcl::%s] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ());
153 if (eigen_values[0] / eigen_values[2] < 1.0e-3)
164 transform1 (2, 0) = eigen_vectors (0, 0);
165 transform1 (2, 1) = eigen_vectors (1, 0);
166 transform1 (2, 2) = eigen_vectors (2, 0);
168 transform1 (1, 0) = eigen_vectors (0, 1);
169 transform1 (1, 1) = eigen_vectors (1, 1);
170 transform1 (1, 2) = eigen_vectors (2, 1);
171 transform1 (0, 0) = eigen_vectors (0, 2);
172 transform1 (0, 1) = eigen_vectors (1, 2);
173 transform1 (0, 2) = eigen_vectors (2, 2);
177 transform1.setIdentity ();
185 boolT ismalloc = True;
187 char flags[] =
"qhull d QJ";
189 FILE *outfile = NULL;
191 FILE *errfile = stderr;
196 coordT *points =
reinterpret_cast<coordT*
> (calloc (cloud_transformed.
points.size () * dim_,
sizeof(coordT)));
198 for (
size_t i = 0; i < cloud_transformed.
points.size (); ++i)
200 points[i * dim_ + 0] =
static_cast<coordT
> (cloud_transformed.
points[i].x);
201 points[i * dim_ + 1] =
static_cast<coordT
> (cloud_transformed.
points[i].y);
204 points[i * dim_ + 2] =
static_cast<coordT
> (cloud_transformed.
points[i].z);
208 exitcode = qh_new_qhull (dim_, static_cast<int> (cloud_transformed.
points.size ()), points, ismalloc, flags, outfile, errfile);
212 PCL_ERROR (
"[pcl::%s::performReconstrution] ERROR: qhull was unable to compute a concave hull for the given point cloud (%zu)!\n", getClassName ().c_str (), cloud_transformed.
points.size ());
217 bool NaNvalues =
false;
218 for (
size_t i = 0; i < cloud_transformed.
size (); ++i)
220 if (!pcl_isfinite (cloud_transformed.
points[i].x) ||
221 !pcl_isfinite (cloud_transformed.
points[i].y) ||
222 !pcl_isfinite (cloud_transformed.
points[i].z))
230 PCL_ERROR (
"[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
233 alpha_shape.
points.resize (0);
237 qh_freeqhull (!qh_ALL);
238 int curlong, totlong;
239 qh_memfreeshort (&curlong, &totlong);
244 qh_setvoronoi_all ();
246 int num_vertices = qh num_vertices;
247 alpha_shape.
points.resize (num_vertices);
251 int max_vertex_id = 0;
254 if (vertex->id + 1 > max_vertex_id)
255 max_vertex_id = vertex->id + 1;
261 std::vector<int> qhid_to_pcidx (max_vertex_id);
263 int num_facets = qh num_facets;
268 setT *triangles_set = qh_settemp (4 * num_facets);
269 if (voronoi_centers_)
270 voronoi_centers_->points.resize (num_facets);
276 if (!facet->upperdelaunay)
278 vertexT *anyVertex =
static_cast<vertexT*
> (facet->vertices->e[0].p);
279 double *center = facet->center;
280 double r = qh_pointdist (anyVertex->point,center,dim_);
283 if (voronoi_centers_)
285 voronoi_centers_->points[non_upper].x =
static_cast<float> (facet->center[0]);
286 voronoi_centers_->points[non_upper].y =
static_cast<float> (facet->center[1]);
287 voronoi_centers_->points[non_upper].z =
static_cast<float> (facet->center[2]);
295 qh_makeridges (facet);
297 facet->visitid = qh visit_id;
298 ridgeT *ridge, **ridgep;
299 FOREACHridge_ (facet->ridges)
301 neighb = otherfacet_ (ridge, facet);
302 if ((neighb->visitid != qh visit_id))
303 qh_setappend (&triangles_set, ridge);
310 facet->visitid = qh visit_id;
311 qh_makeridges (facet);
312 ridgeT *ridge, **ridgep;
313 FOREACHridge_ (facet->ridges)
316 neighb = otherfacet_ (ridge, facet);
317 if ((neighb->visitid != qh visit_id))
322 a.x =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[0].p))->point[0]);
323 a.y =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[0].p))->point[1]);
324 a.z =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[0].p))->point[2]);
325 b.x =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[1].p))->point[0]);
326 b.y =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[1].p))->point[1]);
327 b.z =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[1].p))->point[2]);
328 c.x =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[2].p))->point[0]);
329 c.y =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[2].p))->point[1]);
330 c.z =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[2].p))->point[2]);
334 qh_setappend (&triangles_set, ridge);
341 if (voronoi_centers_)
342 voronoi_centers_->points.resize (non_upper);
346 int num_good_triangles = 0;
347 ridgeT *ridge, **ridgep;
348 FOREACHridge_ (triangles_set)
350 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
351 num_good_triangles++;
354 polygons.resize (num_good_triangles);
357 std::vector<bool> added_vertices (max_vertex_id,
false);
360 FOREACHridge_ (triangles_set)
362 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
364 polygons[triangles].vertices.resize (3);
365 int vertex_n, vertex_i;
366 FOREACHvertex_i_ ((*ridge).vertices)
368 if (!added_vertices[vertex->id])
370 alpha_shape.
points[vertices].x =
static_cast<float> (vertex->point[0]);
371 alpha_shape.
points[vertices].y =
static_cast<float> (vertex->point[1]);
372 alpha_shape.
points[vertices].z =
static_cast<float> (vertex->point[2]);
374 qhid_to_pcidx[vertex->id] = vertices;
375 added_vertices[vertex->id] =
true;
379 polygons[triangles].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
387 alpha_shape.
points.resize (vertices);
388 alpha_shape.
width =
static_cast<uint32_t
> (alpha_shape.
points.size ());
395 setT *edges_set = qh_settemp (3 * num_facets);
396 if (voronoi_centers_)
397 voronoi_centers_->points.resize (num_facets);
402 if (!facet->upperdelaunay)
406 vertexT *anyVertex =
static_cast<vertexT*
>(facet->vertices->e[0].p);
407 double r = (sqrt ((anyVertex->point[0] - facet->center[0]) *
408 (anyVertex->point[0] - facet->center[0]) +
409 (anyVertex->point[1] - facet->center[1]) *
410 (anyVertex->point[1] - facet->center[1])));
414 qh_makeridges (facet);
417 ridgeT *ridge, **ridgep;
418 FOREACHridge_ (facet->ridges)
419 qh_setappend (&edges_set, ridge);
421 if (voronoi_centers_)
423 voronoi_centers_->points[dd].x =
static_cast<float> (facet->center[0]);
424 voronoi_centers_->points[dd].y =
static_cast<float> (facet->center[1]);
425 voronoi_centers_->points[dd].z = 0.0f;
436 std::vector<bool> added_vertices (max_vertex_id,
false);
437 std::map<int, std::vector<int> > edges;
439 ridgeT *ridge, **ridgep;
440 FOREACHridge_ (edges_set)
442 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
444 int vertex_n, vertex_i;
445 int vertices_in_ridge=0;
446 std::vector<int> pcd_indices;
447 pcd_indices.resize (2);
449 FOREACHvertex_i_ ((*ridge).vertices)
451 if (!added_vertices[vertex->id])
453 alpha_shape.
points[vertices].x =
static_cast<float> (vertex->point[0]);
454 alpha_shape.
points[vertices].y =
static_cast<float> (vertex->point[1]);
457 alpha_shape.
points[vertices].z =
static_cast<float> (vertex->point[2]);
459 alpha_shape.
points[vertices].z = 0;
461 qhid_to_pcidx[vertex->id] = vertices;
462 added_vertices[vertex->id] =
true;
463 pcd_indices[vertices_in_ridge] = vertices;
468 pcd_indices[vertices_in_ridge] = qhid_to_pcidx[vertex->id];
475 edges[pcd_indices[0]].push_back (pcd_indices[1]);
476 edges[pcd_indices[1]].push_back (pcd_indices[0]);
480 alpha_shape.
points.resize (vertices);
482 std::vector<std::vector<int> > connected;
484 alpha_shape_sorted.
points.resize (vertices);
487 std::map<int, std::vector<int> >::iterator curr = edges.begin ();
489 std::vector<bool> used (vertices,
false);
490 std::vector<int> pcd_idx_start_polygons;
491 pcd_idx_start_polygons.push_back (0);
495 while (!edges.empty ())
497 alpha_shape_sorted.
points[sorted_idx] = alpha_shape.
points[(*curr).first];
499 for (
size_t i = 0; i < (*curr).second.size (); i++)
501 if (!used[((*curr).second)[i]])
504 next = ((*curr).second)[i];
509 used[(*curr).first] =
true;
518 curr = edges.find (next);
519 if (curr == edges.end ())
522 curr = edges.begin ();
523 pcd_idx_start_polygons.push_back (sorted_idx);
527 pcd_idx_start_polygons.push_back (sorted_idx);
531 polygons.reserve (pcd_idx_start_polygons.size () - 1);
533 for (
size_t poly_id = 0; poly_id < pcd_idx_start_polygons.size () - 1; poly_id++)
536 if (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id] >= 3)
539 vertices.
vertices.resize (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id]);
541 for (
int j = pcd_idx_start_polygons[poly_id]; j < pcd_idx_start_polygons[poly_id + 1]; ++j)
542 vertices.
vertices[j - pcd_idx_start_polygons[poly_id]] = static_cast<uint32_t> (j);
544 polygons.push_back (vertices);
548 if (voronoi_centers_)
549 voronoi_centers_->points.resize (dd);
552 qh_freeqhull (!qh_ALL);
553 int curlong, totlong;
554 qh_memfreeshort (&curlong, &totlong);
556 Eigen::Affine3d transInverse = transform1.inverse ();
558 xyz_centroid[0] = - xyz_centroid[0];
559 xyz_centroid[1] = - xyz_centroid[1];
560 xyz_centroid[2] = - xyz_centroid[2];
564 if (voronoi_centers_)
570 if (keep_information_)
576 std::vector<int> neighbor;
577 std::vector<float> distances;
579 distances.resize (1);
583 std::vector<int> indices;
584 indices.resize (alpha_shape.
points.size ());
586 for (
size_t i = 0; i < alpha_shape.
points.size (); i++)
589 indices[i] = neighbor[0];
597 #pragma GCC diagnostic warning "-Wold-style-cast"
601 template <
typename Po
intInT>
void
606 performReconstruction (hull_points, output.
polygons);
613 template <
typename Po
intInT>
void
617 performReconstruction (hull_points, polygons);
621 #define PCL_INSTANTIATE_ConcaveHull(T) template class PCL_EXPORTS pcl::ConcaveHull<T>;
623 #endif // PCL_SURFACE_IMPL_CONCAVE_HULL_H_