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/common/io.h>
55 #include <pcl/surface/qhull.h>
58 template <
typename Po
intInT>
void
61 output.
header = input_->header;
64 PCL_ERROR (
"[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
76 std::vector<pcl::Vertices> polygons;
77 performReconstruction (output, polygons);
79 output.
width = static_cast<std::uint32_t> (output.
points.size ());
87 template <
typename Po
intInT>
void
90 output.
header = input_->header;
93 PCL_ERROR (
"[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
105 performReconstruction (output, polygons);
107 output.
width = static_cast<std::uint32_t> (output.
points.size ());
115 #pragma GCC diagnostic ignored "-Wold-style-cast"
118 template <
typename Po
intInT>
void
121 Eigen::Vector4d xyz_centroid;
123 EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix = Eigen::Matrix3d::Zero ();
127 for (
int i = 0; i < 3; ++i)
128 for (
int j = 0; j < 3; ++j)
129 if (!std::isfinite (covariance_matrix.coeffRef (i, j)))
132 EIGEN_ALIGN16 Eigen::Vector3d eigen_values;
133 EIGEN_ALIGN16 Eigen::Matrix3d eigen_vectors;
134 pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
136 Eigen::Affine3d transform1;
137 transform1.setIdentity ();
142 PCL_DEBUG (
"[pcl::%s] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ());
143 if (std::abs (eigen_values[0]) < std::numeric_limits<double>::epsilon () || std::abs (eigen_values[0] / eigen_values[2]) < 1.0e-3)
154 transform1 (2, 0) = eigen_vectors (0, 0);
155 transform1 (2, 1) = eigen_vectors (1, 0);
156 transform1 (2, 2) = eigen_vectors (2, 0);
158 transform1 (1, 0) = eigen_vectors (0, 1);
159 transform1 (1, 1) = eigen_vectors (1, 1);
160 transform1 (1, 2) = eigen_vectors (2, 1);
161 transform1 (0, 0) = eigen_vectors (0, 2);
162 transform1 (0, 1) = eigen_vectors (1, 2);
163 transform1 (0, 2) = eigen_vectors (2, 2);
167 transform1.setIdentity ();
175 boolT ismalloc = True;
177 char flags[] =
"qhull d QJ";
179 FILE *outfile =
nullptr;
181 FILE *errfile = stderr;
186 coordT *points = reinterpret_cast<coordT*> (calloc (cloud_transformed.
points.size () * dim_,
sizeof(coordT)));
188 for (std::size_t i = 0; i < cloud_transformed.
points.size (); ++i)
190 points[i * dim_ + 0] = static_cast<coordT> (cloud_transformed.
points[i].x);
191 points[i * dim_ + 1] = static_cast<coordT> (cloud_transformed.
points[i].y);
194 points[i * dim_ + 2] = static_cast<coordT> (cloud_transformed.
points[i].z);
198 exitcode = qh_new_qhull (dim_, static_cast<int> (cloud_transformed.
points.size ()), points, ismalloc, flags, outfile, errfile);
202 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 ());
207 bool NaNvalues =
false;
208 for (std::size_t i = 0; i < cloud_transformed.
size (); ++i)
210 if (!std::isfinite (cloud_transformed.
points[i].x) ||
211 !std::isfinite (cloud_transformed.
points[i].y) ||
212 !std::isfinite (cloud_transformed.
points[i].z))
220 PCL_ERROR (
"[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
223 alpha_shape.
points.resize (0);
227 qh_freeqhull (!qh_ALL);
228 int curlong, totlong;
229 qh_memfreeshort (&curlong, &totlong);
234 qh_setvoronoi_all ();
236 int num_vertices = qh num_vertices;
237 alpha_shape.
points.resize (num_vertices);
241 int max_vertex_id = 0;
244 if (vertex->id + 1 > unsigned (max_vertex_id))
245 max_vertex_id = vertex->id + 1;
251 std::vector<int> qhid_to_pcidx (max_vertex_id);
253 int num_facets = qh num_facets;
258 setT *triangles_set = qh_settemp (4 * num_facets);
259 if (voronoi_centers_)
260 voronoi_centers_->points.resize (num_facets);
266 if (!facet->upperdelaunay)
268 vertexT *anyVertex = static_cast<vertexT*> (facet->vertices->e[0].p);
269 double *center = facet->center;
270 double r = qh_pointdist (anyVertex->point,center,dim_);
273 if (voronoi_centers_)
275 voronoi_centers_->points[non_upper].x = static_cast<float> (facet->center[0]);
276 voronoi_centers_->points[non_upper].y = static_cast<float> (facet->center[1]);
277 voronoi_centers_->points[non_upper].z = static_cast<float> (facet->center[2]);
285 qh_makeridges (facet);
287 facet->visitid = qh visit_id;
288 ridgeT *ridge, **ridgep;
289 FOREACHridge_ (facet->ridges)
291 neighb = otherfacet_ (ridge, facet);
292 if ((neighb->visitid != qh visit_id))
293 qh_setappend (&triangles_set, ridge);
300 facet->visitid = qh visit_id;
301 qh_makeridges (facet);
302 ridgeT *ridge, **ridgep;
303 FOREACHridge_ (facet->ridges)
306 neighb = otherfacet_ (ridge, facet);
307 if ((neighb->visitid != qh visit_id))
312 a.x = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[0].p))->point[0]);
313 a.y = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[0].p))->point[1]);
314 a.z = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[0].p))->point[2]);
315 b.x = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[1].p))->point[0]);
316 b.y = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[1].p))->point[1]);
317 b.z = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[1].p))->point[2]);
318 c.x = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[2].p))->point[0]);
319 c.y = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[2].p))->point[1]);
320 c.z = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[2].p))->point[2]);
324 qh_setappend (&triangles_set, ridge);
331 if (voronoi_centers_)
332 voronoi_centers_->points.resize (non_upper);
336 int num_good_triangles = 0;
337 ridgeT *ridge, **ridgep;
338 FOREACHridge_ (triangles_set)
340 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
341 num_good_triangles++;
344 polygons.resize (num_good_triangles);
347 std::vector<bool> added_vertices (max_vertex_id,
false);
350 FOREACHridge_ (triangles_set)
352 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
354 polygons[triangles].vertices.resize (3);
355 int vertex_n, vertex_i;
356 FOREACHvertex_i_ ((*ridge).vertices)
358 if (!added_vertices[vertex->id])
360 alpha_shape.
points[vertices].x = static_cast<float> (vertex->point[0]);
361 alpha_shape.
points[vertices].y = static_cast<float> (vertex->point[1]);
362 alpha_shape.
points[vertices].z = static_cast<float> (vertex->point[2]);
364 qhid_to_pcidx[vertex->id] = vertices;
365 added_vertices[vertex->id] =
true;
369 polygons[triangles].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
377 alpha_shape.
points.resize (vertices);
378 alpha_shape.
width = static_cast<std::uint32_t> (alpha_shape.
points.size ());
385 setT *edges_set = qh_settemp (3 * num_facets);
386 if (voronoi_centers_)
387 voronoi_centers_->points.resize (num_facets);
392 if (!facet->upperdelaunay)
396 vertexT *anyVertex = static_cast<vertexT*>(facet->vertices->e[0].p);
397 double r = (sqrt ((anyVertex->point[0] - facet->center[0]) *
398 (anyVertex->point[0] - facet->center[0]) +
399 (anyVertex->point[1] - facet->center[1]) *
400 (anyVertex->point[1] - facet->center[1])));
404 qh_makeridges (facet);
407 ridgeT *ridge, **ridgep;
408 FOREACHridge_ (facet->ridges)
409 qh_setappend (&edges_set, ridge);
411 if (voronoi_centers_)
413 voronoi_centers_->points[dd].x = static_cast<float> (facet->center[0]);
414 voronoi_centers_->points[dd].y = static_cast<float> (facet->center[1]);
415 voronoi_centers_->points[dd].z = 0.0f;
426 std::vector<bool> added_vertices (max_vertex_id,
false);
427 std::map<int, std::vector<int> > edges;
429 ridgeT *ridge, **ridgep;
430 FOREACHridge_ (edges_set)
432 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
434 int vertex_n, vertex_i;
435 int vertices_in_ridge=0;
436 std::vector<int> pcd_indices;
437 pcd_indices.resize (2);
439 FOREACHvertex_i_ ((*ridge).vertices)
441 if (!added_vertices[vertex->id])
443 alpha_shape.
points[vertices].x = static_cast<float> (vertex->point[0]);
444 alpha_shape.
points[vertices].y = static_cast<float> (vertex->point[1]);
447 alpha_shape.
points[vertices].z = static_cast<float> (vertex->point[2]);
449 alpha_shape.
points[vertices].z = 0;
451 qhid_to_pcidx[vertex->id] = vertices;
452 added_vertices[vertex->id] =
true;
453 pcd_indices[vertices_in_ridge] = vertices;
458 pcd_indices[vertices_in_ridge] = qhid_to_pcidx[vertex->id];
465 edges[pcd_indices[0]].push_back (pcd_indices[1]);
466 edges[pcd_indices[1]].push_back (pcd_indices[0]);
470 alpha_shape.
points.resize (vertices);
472 std::vector<std::vector<int> > connected;
474 alpha_shape_sorted.
points.resize (vertices);
477 std::map<int, std::vector<int> >::iterator curr = edges.begin ();
479 std::vector<bool> used (vertices,
false);
480 std::vector<int> pcd_idx_start_polygons;
481 pcd_idx_start_polygons.push_back (0);
485 while (!edges.empty ())
487 alpha_shape_sorted.
points[sorted_idx] = alpha_shape.
points[(*curr).first];
489 for (
const int &i : (*curr).second)
499 used[(*curr).first] =
true;
508 curr = edges.find (next);
509 if (curr == edges.end ())
512 curr = edges.begin ();
513 pcd_idx_start_polygons.push_back (sorted_idx);
517 pcd_idx_start_polygons.push_back (sorted_idx);
521 polygons.reserve (pcd_idx_start_polygons.size () - 1);
523 for (std::size_t poly_id = 0; poly_id < pcd_idx_start_polygons.size () - 1; poly_id++)
526 if (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id] >= 3)
529 vertices.
vertices.resize (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id]);
531 for (
int j = pcd_idx_start_polygons[poly_id]; j < pcd_idx_start_polygons[poly_id + 1]; ++j)
532 vertices.
vertices[j - pcd_idx_start_polygons[poly_id]] = static_cast<std::uint32_t> (j);
534 polygons.push_back (vertices);
538 if (voronoi_centers_)
539 voronoi_centers_->points.resize (dd);
542 qh_freeqhull (!qh_ALL);
543 int curlong, totlong;
544 qh_memfreeshort (&curlong, &totlong);
546 Eigen::Affine3d transInverse = transform1.inverse ();
548 xyz_centroid[0] = - xyz_centroid[0];
549 xyz_centroid[1] = - xyz_centroid[1];
550 xyz_centroid[2] = - xyz_centroid[2];
554 if (voronoi_centers_)
560 if (keep_information_)
566 std::vector<int> neighbor;
567 std::vector<float> distances;
569 distances.resize (1);
572 hull_indices_.header = input_->header;
573 hull_indices_.indices.clear ();
574 hull_indices_.indices.reserve (alpha_shape.
points.size ());
576 for (std::size_t i = 0; i < alpha_shape.
points.size (); i++)
579 hull_indices_.indices.push_back (neighbor[0]);
587 #pragma GCC diagnostic warning "-Wold-style-cast"
591 template <
typename Po
intInT>
void
596 performReconstruction (hull_points, output.
polygons);
603 template <
typename Po
intInT>
void
607 performReconstruction (hull_points, polygons);
611 template <
typename Po
intInT>
void
614 hull_point_indices = hull_indices_;
617 #define PCL_INSTANTIATE_ConcaveHull(T) template class PCL_EXPORTS pcl::ConcaveHull<T>;
619 #endif // PCL_SURFACE_IMPL_CONCAVE_HULL_H_