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.
size () * dim_,
sizeof(coordT)));
188 for (std::size_t i = 0; i < cloud_transformed.
size (); ++i)
190 points[i * dim_ + 0] =
static_cast<coordT
> (cloud_transformed[i].x);
191 points[i * dim_ + 1] =
static_cast<coordT
> (cloud_transformed[i].y);
194 points[i * dim_ + 2] =
static_cast<coordT
> (cloud_transformed[i].z);
200 qh_zero(qh, errfile);
203 exitcode = qh_new_qhull (qh, dim_,
static_cast<int> (cloud_transformed.
size ()), points, ismalloc, flags, outfile, errfile);
207 PCL_ERROR(
"[pcl::%s::performReconstrution] ERROR: qhull was unable to compute a "
208 "concave hull for the given point cloud (%zu)!\n",
209 getClassName().c_str(),
210 static_cast<std::size_t
>(cloud_transformed.
size()));
215 bool NaNvalues =
false;
216 for (std::size_t i = 0; i < cloud_transformed.
size (); ++i)
218 if (!std::isfinite (cloud_transformed[i].x) ||
219 !std::isfinite (cloud_transformed[i].y) ||
220 !std::isfinite (cloud_transformed[i].z))
228 PCL_ERROR (
"[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
235 qh_freeqhull (qh, !qh_ALL);
236 int curlong, totlong;
237 qh_memfreeshort (qh, &curlong, &totlong);
242 qh_setvoronoi_all (qh);
244 int num_vertices = qh->num_vertices;
245 alpha_shape.
resize (num_vertices);
249 int max_vertex_id = 0;
252 if (vertex->id + 1 > unsigned (max_vertex_id))
253 max_vertex_id = vertex->id + 1;
259 std::vector<int> qhid_to_pcidx (max_vertex_id);
261 int num_facets = qh->num_facets;
265 setT *triangles_set = qh_settemp (qh, 4 * num_facets);
266 if (voronoi_centers_)
267 voronoi_centers_->points.resize (num_facets);
273 if (!facet->upperdelaunay)
275 auto *anyVertex =
static_cast<vertexT*
> (facet->vertices->e[0].p);
276 double *center = facet->center;
277 double r = qh_pointdist (anyVertex->point,center,dim_);
279 if (voronoi_centers_)
281 (*voronoi_centers_)[non_upper].x =
static_cast<float> (facet->center[0]);
282 (*voronoi_centers_)[non_upper].y =
static_cast<float> (facet->center[1]);
283 (*voronoi_centers_)[non_upper].z =
static_cast<float> (facet->center[2]);
291 qh_makeridges (qh, facet);
293 facet->visitid = qh->visit_id;
294 ridgeT *ridge, **ridgep;
295 FOREACHridge_ (facet->ridges)
297 facetT *neighb = otherfacet_ (ridge, facet);
298 if ((neighb->visitid != qh->visit_id))
299 qh_setappend (qh, &triangles_set, ridge);
306 facet->visitid = qh->visit_id;
307 qh_makeridges (qh, facet);
308 ridgeT *ridge, **ridgep;
309 FOREACHridge_ (facet->ridges)
312 neighb = otherfacet_ (ridge, facet);
313 if ((neighb->visitid != qh->visit_id))
318 a.x =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[0].p))->point[0]);
319 a.y =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[0].p))->point[1]);
320 a.z =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[0].p))->point[2]);
321 b.x =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[1].p))->point[0]);
322 b.y =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[1].p))->point[1]);
323 b.z =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[1].p))->point[2]);
324 c.x =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[2].p))->point[0]);
325 c.y =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[2].p))->point[1]);
326 c.z =
static_cast<float> ((
static_cast<vertexT*
>(ridge->vertices->e[2].p))->point[2]);
330 qh_setappend (qh, &triangles_set, ridge);
337 if (voronoi_centers_)
338 voronoi_centers_->points.resize (non_upper);
342 int num_good_triangles = 0;
343 ridgeT *ridge, **ridgep;
344 FOREACHridge_ (triangles_set)
346 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
347 num_good_triangles++;
350 polygons.resize (num_good_triangles);
353 std::vector<bool> added_vertices (max_vertex_id,
false);
356 FOREACHridge_ (triangles_set)
358 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
360 polygons[triangles].vertices.resize (3);
361 int vertex_n, vertex_i;
362 FOREACHvertex_i_ (qh, (*ridge).vertices)
364 if (!added_vertices[vertex->id])
366 alpha_shape[vertices].x =
static_cast<float> (vertex->point[0]);
367 alpha_shape[vertices].y =
static_cast<float> (vertex->point[1]);
368 alpha_shape[vertices].z =
static_cast<float> (vertex->point[2]);
370 qhid_to_pcidx[vertex->id] = vertices;
371 added_vertices[vertex->id] =
true;
375 polygons[triangles].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
383 alpha_shape.
resize (vertices);
391 setT *edges_set = qh_settemp (qh, 3 * num_facets);
392 if (voronoi_centers_)
393 voronoi_centers_->points.resize (num_facets);
399 if (!facet->upperdelaunay)
403 auto *anyVertex =
static_cast<vertexT*
>(facet->vertices->e[0].p);
404 double r = (sqrt ((anyVertex->point[0] - facet->center[0]) *
405 (anyVertex->point[0] - facet->center[0]) +
406 (anyVertex->point[1] - facet->center[1]) *
407 (anyVertex->point[1] - facet->center[1])));
411 qh_makeridges (qh, facet);
414 ridgeT *ridge, **ridgep;
415 FOREACHridge_ (facet->ridges)
416 qh_setappend (qh, &edges_set, ridge);
418 if (voronoi_centers_)
420 (*voronoi_centers_)[dd].x =
static_cast<float> (facet->center[0]);
421 (*voronoi_centers_)[dd].y =
static_cast<float> (facet->center[1]);
422 (*voronoi_centers_)[dd].z = 0.0f;
433 std::vector<bool> added_vertices (max_vertex_id,
false);
434 std::map<int, std::vector<int> > edges;
436 ridgeT *ridge, **ridgep;
437 FOREACHridge_ (edges_set)
439 if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
441 int vertex_n, vertex_i;
442 int vertices_in_ridge=0;
443 std::vector<int> pcd_indices;
444 pcd_indices.resize (2);
446 FOREACHvertex_i_ (qh, (*ridge).vertices)
448 if (!added_vertices[vertex->id])
450 alpha_shape[vertices].x =
static_cast<float> (vertex->point[0]);
451 alpha_shape[vertices].y =
static_cast<float> (vertex->point[1]);
454 alpha_shape[vertices].z =
static_cast<float> (vertex->point[2]);
456 alpha_shape[vertices].z = 0;
458 qhid_to_pcidx[vertex->id] = vertices;
459 added_vertices[vertex->id] =
true;
460 pcd_indices[vertices_in_ridge] = vertices;
465 pcd_indices[vertices_in_ridge] = qhid_to_pcidx[vertex->id];
472 edges[pcd_indices[0]].
push_back (pcd_indices[1]);
473 edges[pcd_indices[1]].push_back (pcd_indices[0]);
477 alpha_shape.
resize (vertices);
480 alpha_shape_sorted.
resize (vertices);
483 auto curr = edges.begin ();
485 std::vector<bool> used (vertices,
false);
486 std::vector<int> pcd_idx_start_polygons;
487 pcd_idx_start_polygons.push_back (0);
491 while (!edges.empty ())
493 alpha_shape_sorted[sorted_idx] = alpha_shape[(*curr).first];
495 for (
const auto &i : (*curr).second)
505 used[(*curr).first] =
true;
514 curr = edges.find (next);
515 if (curr == edges.end ())
518 curr = edges.begin ();
519 pcd_idx_start_polygons.push_back (sorted_idx);
523 pcd_idx_start_polygons.push_back (sorted_idx);
527 polygons.reserve (pcd_idx_start_polygons.size () - 1);
529 for (std::size_t poly_id = 0; poly_id < pcd_idx_start_polygons.size () - 1; poly_id++)
532 if (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id] >= 3)
535 vertices.
vertices.resize (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id]);
537 for (
int j = pcd_idx_start_polygons[poly_id]; j < pcd_idx_start_polygons[poly_id + 1]; ++j)
538 vertices.
vertices[j - pcd_idx_start_polygons[poly_id]] =
static_cast<std::uint32_t
> (j);
540 polygons.push_back (vertices);
544 if (voronoi_centers_)
545 voronoi_centers_->points.resize (dd);
548 qh_freeqhull (qh, !qh_ALL);
549 int curlong, totlong;
550 qh_memfreeshort (qh, &curlong, &totlong);
552 Eigen::Affine3d transInverse = transform1.inverse ();
554 xyz_centroid[0] = - xyz_centroid[0];
555 xyz_centroid[1] = - xyz_centroid[1];
556 xyz_centroid[2] = - xyz_centroid[2];
560 if (voronoi_centers_)
566 if (keep_information_)
573 std::vector<float> distances;
575 distances.resize (1);
578 hull_indices_.header = input_->header;
579 hull_indices_.indices.clear ();
580 hull_indices_.indices.reserve (alpha_shape.
size ());
582 for (
const auto& point: alpha_shape)
585 hull_indices_.indices.push_back (neighbor[0]);