627 lines
20 KiB
C++
627 lines
20 KiB
C++
/**
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2010-2011, Willow Garage, Inc.
|
|
*
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of Willow Garage, Inc. nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
* $Id$
|
|
*
|
|
*/
|
|
|
|
#include <pcl/pcl_config.h>
|
|
#ifdef HAVE_QHULL
|
|
|
|
#ifndef PCL_SURFACE_IMPL_CONCAVE_HULL_H_
|
|
#define PCL_SURFACE_IMPL_CONCAVE_HULL_H_
|
|
|
|
#include <map>
|
|
#include <pcl/surface/concave_hull.h>
|
|
#include <pcl/common/common.h>
|
|
#include <pcl/common/eigen.h>
|
|
#include <pcl/common/centroid.h>
|
|
#include <pcl/common/transforms.h>
|
|
#include <pcl/common/io.h>
|
|
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <pcl/surface/qhull.h>
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
template <typename PointInT> void
|
|
pcl::ConcaveHull<PointInT>::reconstruct (PointCloud &output)
|
|
{
|
|
output.header = input_->header;
|
|
if (alpha_ <= 0)
|
|
{
|
|
PCL_ERROR ("[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
|
|
output.clear ();
|
|
return;
|
|
}
|
|
|
|
if (!initCompute ())
|
|
{
|
|
output.clear ();
|
|
return;
|
|
}
|
|
|
|
// Perform the actual surface reconstruction
|
|
std::vector<pcl::Vertices> polygons;
|
|
performReconstruction (output, polygons);
|
|
|
|
output.width = output.size ();
|
|
output.height = 1;
|
|
output.is_dense = true;
|
|
|
|
deinitCompute ();
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
template <typename PointInT> void
|
|
pcl::ConcaveHull<PointInT>::reconstruct (PointCloud &output, std::vector<pcl::Vertices> &polygons)
|
|
{
|
|
output.header = input_->header;
|
|
if (alpha_ <= 0)
|
|
{
|
|
PCL_ERROR ("[pcl::%s::reconstruct] Alpha parameter must be set to a positive number!\n", getClassName ().c_str ());
|
|
output.clear ();
|
|
return;
|
|
}
|
|
|
|
if (!initCompute ())
|
|
{
|
|
output.clear ();
|
|
return;
|
|
}
|
|
|
|
// Perform the actual surface reconstruction
|
|
performReconstruction (output, polygons);
|
|
|
|
output.width = output.size ();
|
|
output.height = 1;
|
|
output.is_dense = true;
|
|
|
|
deinitCompute ();
|
|
}
|
|
|
|
#ifdef __GNUC__
|
|
#pragma GCC diagnostic ignored "-Wold-style-cast"
|
|
#endif
|
|
//////////////////////////////////////////////////////////////////////////
|
|
template <typename PointInT> void
|
|
pcl::ConcaveHull<PointInT>::performReconstruction (PointCloud &alpha_shape, std::vector<pcl::Vertices> &polygons)
|
|
{
|
|
Eigen::Vector4d xyz_centroid;
|
|
compute3DCentroid (*input_, *indices_, xyz_centroid);
|
|
EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix = Eigen::Matrix3d::Zero ();
|
|
computeCovarianceMatrixNormalized (*input_, *indices_, xyz_centroid, covariance_matrix);
|
|
|
|
// Check if the covariance matrix is finite or not.
|
|
for (int i = 0; i < 3; ++i)
|
|
for (int j = 0; j < 3; ++j)
|
|
if (!std::isfinite (covariance_matrix.coeffRef (i, j)))
|
|
return;
|
|
|
|
EIGEN_ALIGN16 Eigen::Vector3d eigen_values;
|
|
EIGEN_ALIGN16 Eigen::Matrix3d eigen_vectors;
|
|
pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
|
|
|
|
Eigen::Affine3d transform1;
|
|
transform1.setIdentity ();
|
|
|
|
// If no input dimension is specified, determine automatically
|
|
if (dim_ == 0)
|
|
{
|
|
PCL_DEBUG ("[pcl::%s] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ());
|
|
if (std::abs (eigen_values[0]) < std::numeric_limits<double>::epsilon () || std::abs (eigen_values[0] / eigen_values[2]) < 1.0e-3)
|
|
dim_ = 2;
|
|
else
|
|
dim_ = 3;
|
|
}
|
|
|
|
if (dim_ == 2)
|
|
{
|
|
// we have points laying on a plane, using 2d convex hull
|
|
// compute transformation bring eigen_vectors.col(i) to z-axis
|
|
|
|
transform1 (2, 0) = eigen_vectors (0, 0);
|
|
transform1 (2, 1) = eigen_vectors (1, 0);
|
|
transform1 (2, 2) = eigen_vectors (2, 0);
|
|
|
|
transform1 (1, 0) = eigen_vectors (0, 1);
|
|
transform1 (1, 1) = eigen_vectors (1, 1);
|
|
transform1 (1, 2) = eigen_vectors (2, 1);
|
|
transform1 (0, 0) = eigen_vectors (0, 2);
|
|
transform1 (0, 1) = eigen_vectors (1, 2);
|
|
transform1 (0, 2) = eigen_vectors (2, 2);
|
|
}
|
|
else
|
|
{
|
|
transform1.setIdentity ();
|
|
}
|
|
|
|
PointCloud cloud_transformed;
|
|
pcl::demeanPointCloud (*input_, *indices_, xyz_centroid, cloud_transformed);
|
|
pcl::transformPointCloud (cloud_transformed, cloud_transformed, transform1);
|
|
|
|
// True if qhull should free points in qh_freeqhull() or reallocation
|
|
boolT ismalloc = True;
|
|
// option flags for qhull, see qh_opt.htm
|
|
char flags[] = "qhull d QJ";
|
|
// output from qh_produce_output(), use NULL to skip qh_produce_output()
|
|
FILE *outfile = nullptr;
|
|
// error messages from qhull code
|
|
FILE *errfile = stderr;
|
|
// 0 if no error from qhull
|
|
int exitcode;
|
|
|
|
// Array of coordinates for each point
|
|
coordT *points = reinterpret_cast<coordT*> (calloc (cloud_transformed.size () * dim_, sizeof(coordT)));
|
|
|
|
for (std::size_t i = 0; i < cloud_transformed.size (); ++i)
|
|
{
|
|
points[i * dim_ + 0] = static_cast<coordT> (cloud_transformed[i].x);
|
|
points[i * dim_ + 1] = static_cast<coordT> (cloud_transformed[i].y);
|
|
|
|
if (dim_ > 2)
|
|
points[i * dim_ + 2] = static_cast<coordT> (cloud_transformed[i].z);
|
|
}
|
|
|
|
qhT qh_qh;
|
|
qhT* qh = &qh_qh;
|
|
QHULL_LIB_CHECK
|
|
qh_zero(qh, errfile);
|
|
|
|
// Compute concave hull
|
|
exitcode = qh_new_qhull (qh, dim_, static_cast<int> (cloud_transformed.size ()), points, ismalloc, flags, outfile, errfile);
|
|
|
|
if (exitcode != 0)
|
|
{
|
|
PCL_ERROR("[pcl::%s::performReconstrution] ERROR: qhull was unable to compute a "
|
|
"concave hull for the given point cloud (%zu)!\n",
|
|
getClassName().c_str(),
|
|
static_cast<std::size_t>(cloud_transformed.size()));
|
|
|
|
//check if it fails because of NaN values...
|
|
if (!cloud_transformed.is_dense)
|
|
{
|
|
bool NaNvalues = false;
|
|
for (std::size_t i = 0; i < cloud_transformed.size (); ++i)
|
|
{
|
|
if (!std::isfinite (cloud_transformed[i].x) ||
|
|
!std::isfinite (cloud_transformed[i].y) ||
|
|
!std::isfinite (cloud_transformed[i].z))
|
|
{
|
|
NaNvalues = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (NaNvalues)
|
|
PCL_ERROR ("[pcl::%s::performReconstruction] ERROR: point cloud contains NaN values, consider running pcl::PassThrough filter first to remove NaNs!\n", getClassName ().c_str ());
|
|
}
|
|
|
|
alpha_shape.resize (0);
|
|
alpha_shape.width = alpha_shape.height = 0;
|
|
polygons.resize (0);
|
|
|
|
qh_freeqhull (qh, !qh_ALL);
|
|
int curlong, totlong;
|
|
qh_memfreeshort (qh, &curlong, &totlong);
|
|
|
|
return;
|
|
}
|
|
|
|
qh_setvoronoi_all (qh);
|
|
|
|
int num_vertices = qh->num_vertices;
|
|
alpha_shape.resize (num_vertices);
|
|
|
|
vertexT *vertex;
|
|
// Max vertex id
|
|
int max_vertex_id = 0;
|
|
FORALLvertices
|
|
{
|
|
if (vertex->id + 1 > unsigned (max_vertex_id))
|
|
max_vertex_id = vertex->id + 1;
|
|
}
|
|
|
|
facetT *facet; // set by FORALLfacets
|
|
|
|
++max_vertex_id;
|
|
std::vector<int> qhid_to_pcidx (max_vertex_id);
|
|
|
|
int num_facets = qh->num_facets;
|
|
|
|
if (dim_ == 3)
|
|
{
|
|
setT *triangles_set = qh_settemp (qh, 4 * num_facets);
|
|
if (voronoi_centers_)
|
|
voronoi_centers_->points.resize (num_facets);
|
|
|
|
int non_upper = 0;
|
|
FORALLfacets
|
|
{
|
|
// Facets are tetrahedrons (3d)
|
|
if (!facet->upperdelaunay)
|
|
{
|
|
vertexT *anyVertex = static_cast<vertexT*> (facet->vertices->e[0].p);
|
|
double *center = facet->center;
|
|
double r = qh_pointdist (anyVertex->point,center,dim_);
|
|
|
|
if (voronoi_centers_)
|
|
{
|
|
(*voronoi_centers_)[non_upper].x = static_cast<float> (facet->center[0]);
|
|
(*voronoi_centers_)[non_upper].y = static_cast<float> (facet->center[1]);
|
|
(*voronoi_centers_)[non_upper].z = static_cast<float> (facet->center[2]);
|
|
}
|
|
|
|
non_upper++;
|
|
|
|
if (r <= alpha_)
|
|
{
|
|
// all triangles in tetrahedron are good, add them all to the alpha shape (triangles_set)
|
|
qh_makeridges (qh, facet);
|
|
facet->good = true;
|
|
facet->visitid = qh->visit_id;
|
|
ridgeT *ridge, **ridgep;
|
|
FOREACHridge_ (facet->ridges)
|
|
{
|
|
facetT *neighb = otherfacet_ (ridge, facet);
|
|
if ((neighb->visitid != qh->visit_id))
|
|
qh_setappend (qh, &triangles_set, ridge);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// consider individual triangles from the tetrahedron...
|
|
facet->good = false;
|
|
facet->visitid = qh->visit_id;
|
|
qh_makeridges (qh, facet);
|
|
ridgeT *ridge, **ridgep;
|
|
FOREACHridge_ (facet->ridges)
|
|
{
|
|
facetT *neighb;
|
|
neighb = otherfacet_ (ridge, facet);
|
|
if ((neighb->visitid != qh->visit_id))
|
|
{
|
|
// check if individual triangle is good and add it to triangles_set
|
|
|
|
PointInT a, b, c;
|
|
a.x = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[0].p))->point[0]);
|
|
a.y = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[0].p))->point[1]);
|
|
a.z = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[0].p))->point[2]);
|
|
b.x = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[1].p))->point[0]);
|
|
b.y = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[1].p))->point[1]);
|
|
b.z = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[1].p))->point[2]);
|
|
c.x = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[2].p))->point[0]);
|
|
c.y = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[2].p))->point[1]);
|
|
c.z = static_cast<float> ((static_cast<vertexT*>(ridge->vertices->e[2].p))->point[2]);
|
|
|
|
double r = pcl::getCircumcircleRadius (a, b, c);
|
|
if (r <= alpha_)
|
|
qh_setappend (qh, &triangles_set, ridge);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (voronoi_centers_)
|
|
voronoi_centers_->points.resize (non_upper);
|
|
|
|
// filter, add points to alpha_shape and create polygon structure
|
|
|
|
int num_good_triangles = 0;
|
|
ridgeT *ridge, **ridgep;
|
|
FOREACHridge_ (triangles_set)
|
|
{
|
|
if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
|
|
num_good_triangles++;
|
|
}
|
|
|
|
polygons.resize (num_good_triangles);
|
|
|
|
int vertices = 0;
|
|
std::vector<bool> added_vertices (max_vertex_id, false);
|
|
|
|
int triangles = 0;
|
|
FOREACHridge_ (triangles_set)
|
|
{
|
|
if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
|
|
{
|
|
polygons[triangles].vertices.resize (3);
|
|
int vertex_n, vertex_i;
|
|
FOREACHvertex_i_ (qh, (*ridge).vertices) //3 vertices per ridge!
|
|
{
|
|
if (!added_vertices[vertex->id])
|
|
{
|
|
alpha_shape[vertices].x = static_cast<float> (vertex->point[0]);
|
|
alpha_shape[vertices].y = static_cast<float> (vertex->point[1]);
|
|
alpha_shape[vertices].z = static_cast<float> (vertex->point[2]);
|
|
|
|
qhid_to_pcidx[vertex->id] = vertices; //map the vertex id of qhull to the point cloud index
|
|
added_vertices[vertex->id] = true;
|
|
vertices++;
|
|
}
|
|
|
|
polygons[triangles].vertices[vertex_i] = qhid_to_pcidx[vertex->id];
|
|
|
|
}
|
|
|
|
triangles++;
|
|
}
|
|
}
|
|
|
|
alpha_shape.resize (vertices);
|
|
alpha_shape.width = alpha_shape.size ();
|
|
alpha_shape.height = 1;
|
|
}
|
|
else
|
|
{
|
|
// Compute the alpha complex for the set of points
|
|
// Filters the delaunay triangles
|
|
setT *edges_set = qh_settemp (qh, 3 * num_facets);
|
|
if (voronoi_centers_)
|
|
voronoi_centers_->points.resize (num_facets);
|
|
|
|
int dd = 0;
|
|
FORALLfacets
|
|
{
|
|
// Facets are the delaunay triangles (2d)
|
|
if (!facet->upperdelaunay)
|
|
{
|
|
// Check if the distance from any vertex to the facet->center
|
|
// (center of the voronoi cell) is smaller than alpha
|
|
vertexT *anyVertex = static_cast<vertexT*>(facet->vertices->e[0].p);
|
|
double r = (sqrt ((anyVertex->point[0] - facet->center[0]) *
|
|
(anyVertex->point[0] - facet->center[0]) +
|
|
(anyVertex->point[1] - facet->center[1]) *
|
|
(anyVertex->point[1] - facet->center[1])));
|
|
if (r <= alpha_)
|
|
{
|
|
pcl::Vertices facet_vertices; //TODO: is not used!!
|
|
qh_makeridges (qh, facet);
|
|
facet->good = true;
|
|
|
|
ridgeT *ridge, **ridgep;
|
|
FOREACHridge_ (facet->ridges)
|
|
qh_setappend (qh, &edges_set, ridge);
|
|
|
|
if (voronoi_centers_)
|
|
{
|
|
(*voronoi_centers_)[dd].x = static_cast<float> (facet->center[0]);
|
|
(*voronoi_centers_)[dd].y = static_cast<float> (facet->center[1]);
|
|
(*voronoi_centers_)[dd].z = 0.0f;
|
|
}
|
|
|
|
++dd;
|
|
}
|
|
else
|
|
facet->good = false;
|
|
}
|
|
}
|
|
|
|
int vertices = 0;
|
|
std::vector<bool> added_vertices (max_vertex_id, false);
|
|
std::map<int, std::vector<int> > edges;
|
|
|
|
ridgeT *ridge, **ridgep;
|
|
FOREACHridge_ (edges_set)
|
|
{
|
|
if (ridge->bottom->upperdelaunay || ridge->top->upperdelaunay || !ridge->top->good || !ridge->bottom->good)
|
|
{
|
|
int vertex_n, vertex_i;
|
|
int vertices_in_ridge=0;
|
|
std::vector<int> pcd_indices;
|
|
pcd_indices.resize (2);
|
|
|
|
FOREACHvertex_i_ (qh, (*ridge).vertices) //in 2-dim, 2 vertices per ridge!
|
|
{
|
|
if (!added_vertices[vertex->id])
|
|
{
|
|
alpha_shape[vertices].x = static_cast<float> (vertex->point[0]);
|
|
alpha_shape[vertices].y = static_cast<float> (vertex->point[1]);
|
|
|
|
if (dim_ > 2)
|
|
alpha_shape[vertices].z = static_cast<float> (vertex->point[2]);
|
|
else
|
|
alpha_shape[vertices].z = 0;
|
|
|
|
qhid_to_pcidx[vertex->id] = vertices; //map the vertex id of qhull to the point cloud index
|
|
added_vertices[vertex->id] = true;
|
|
pcd_indices[vertices_in_ridge] = vertices;
|
|
vertices++;
|
|
}
|
|
else
|
|
{
|
|
pcd_indices[vertices_in_ridge] = qhid_to_pcidx[vertex->id];
|
|
}
|
|
|
|
vertices_in_ridge++;
|
|
}
|
|
|
|
// make edges bidirectional and pointing to alpha_shape pointcloud...
|
|
edges[pcd_indices[0]].push_back (pcd_indices[1]);
|
|
edges[pcd_indices[1]].push_back (pcd_indices[0]);
|
|
}
|
|
}
|
|
|
|
alpha_shape.resize (vertices);
|
|
|
|
PointCloud alpha_shape_sorted;
|
|
alpha_shape_sorted.resize (vertices);
|
|
|
|
// iterate over edges until they are empty!
|
|
std::map<int, std::vector<int> >::iterator curr = edges.begin ();
|
|
int next = - 1;
|
|
std::vector<bool> used (vertices, false); // used to decide which direction should we take!
|
|
std::vector<int> pcd_idx_start_polygons;
|
|
pcd_idx_start_polygons.push_back (0);
|
|
|
|
// start following edges and removing elements
|
|
int sorted_idx = 0;
|
|
while (!edges.empty ())
|
|
{
|
|
alpha_shape_sorted[sorted_idx] = alpha_shape[(*curr).first];
|
|
// check where we can go from (*curr).first
|
|
for (const auto &i : (*curr).second)
|
|
{
|
|
if (!used[i])
|
|
{
|
|
// we can go there
|
|
next = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
used[(*curr).first] = true;
|
|
edges.erase (curr); // remove edges starting from curr
|
|
|
|
sorted_idx++;
|
|
|
|
if (edges.empty ())
|
|
break;
|
|
|
|
// reassign current
|
|
curr = edges.find (next); // if next is not found, then we have unconnected polygons.
|
|
if (curr == edges.end ())
|
|
{
|
|
// set current to any of the remaining in edge!
|
|
curr = edges.begin ();
|
|
pcd_idx_start_polygons.push_back (sorted_idx);
|
|
}
|
|
}
|
|
|
|
pcd_idx_start_polygons.push_back (sorted_idx);
|
|
|
|
alpha_shape.points = alpha_shape_sorted.points;
|
|
|
|
polygons.reserve (pcd_idx_start_polygons.size () - 1);
|
|
|
|
for (std::size_t poly_id = 0; poly_id < pcd_idx_start_polygons.size () - 1; poly_id++)
|
|
{
|
|
// Check if we actually have a polygon, and not some degenerated output from QHull
|
|
if (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id] >= 3)
|
|
{
|
|
pcl::Vertices vertices;
|
|
vertices.vertices.resize (pcd_idx_start_polygons[poly_id + 1] - pcd_idx_start_polygons[poly_id]);
|
|
// populate points in the corresponding polygon
|
|
for (int j = pcd_idx_start_polygons[poly_id]; j < pcd_idx_start_polygons[poly_id + 1]; ++j)
|
|
vertices.vertices[j - pcd_idx_start_polygons[poly_id]] = static_cast<std::uint32_t> (j);
|
|
|
|
polygons.push_back (vertices);
|
|
}
|
|
}
|
|
|
|
if (voronoi_centers_)
|
|
voronoi_centers_->points.resize (dd);
|
|
}
|
|
|
|
qh_freeqhull (qh, !qh_ALL);
|
|
int curlong, totlong;
|
|
qh_memfreeshort (qh, &curlong, &totlong);
|
|
|
|
Eigen::Affine3d transInverse = transform1.inverse ();
|
|
pcl::transformPointCloud (alpha_shape, alpha_shape, transInverse);
|
|
xyz_centroid[0] = - xyz_centroid[0];
|
|
xyz_centroid[1] = - xyz_centroid[1];
|
|
xyz_centroid[2] = - xyz_centroid[2];
|
|
pcl::demeanPointCloud (alpha_shape, xyz_centroid, alpha_shape);
|
|
|
|
// also transform voronoi_centers_...
|
|
if (voronoi_centers_)
|
|
{
|
|
pcl::transformPointCloud (*voronoi_centers_, *voronoi_centers_, transInverse);
|
|
pcl::demeanPointCloud (*voronoi_centers_, xyz_centroid, *voronoi_centers_);
|
|
}
|
|
|
|
if (keep_information_)
|
|
{
|
|
// build a tree with the original points
|
|
pcl::KdTreeFLANN<PointInT> tree (true);
|
|
tree.setInputCloud (input_, indices_);
|
|
|
|
pcl::Indices neighbor;
|
|
std::vector<float> distances;
|
|
neighbor.resize (1);
|
|
distances.resize (1);
|
|
|
|
// for each point in the concave hull, search for the nearest neighbor in the original point cloud
|
|
hull_indices_.header = input_->header;
|
|
hull_indices_.indices.clear ();
|
|
hull_indices_.indices.reserve (alpha_shape.size ());
|
|
|
|
for (const auto& point: alpha_shape)
|
|
{
|
|
tree.nearestKSearch (point, 1, neighbor, distances);
|
|
hull_indices_.indices.push_back (neighbor[0]);
|
|
}
|
|
|
|
// replace point with the closest neighbor in the original point cloud
|
|
pcl::copyPointCloud (*input_, hull_indices_.indices, alpha_shape);
|
|
}
|
|
}
|
|
#ifdef __GNUC__
|
|
#pragma GCC diagnostic warning "-Wold-style-cast"
|
|
#endif
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointInT> void
|
|
pcl::ConcaveHull<PointInT>::performReconstruction (PolygonMesh &output)
|
|
{
|
|
// Perform reconstruction
|
|
pcl::PointCloud<PointInT> hull_points;
|
|
performReconstruction (hull_points, output.polygons);
|
|
|
|
// Convert the PointCloud into a PCLPointCloud2
|
|
pcl::toPCLPointCloud2 (hull_points, output.cloud);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointInT> void
|
|
pcl::ConcaveHull<PointInT>::performReconstruction (std::vector<pcl::Vertices> &polygons)
|
|
{
|
|
pcl::PointCloud<PointInT> hull_points;
|
|
performReconstruction (hull_points, polygons);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointInT> void
|
|
pcl::ConcaveHull<PointInT>::getHullPointIndices (pcl::PointIndices &hull_point_indices) const
|
|
{
|
|
hull_point_indices = hull_indices_;
|
|
}
|
|
|
|
#define PCL_INSTANTIATE_ConcaveHull(T) template class PCL_EXPORTS pcl::ConcaveHull<T>;
|
|
|
|
#endif // PCL_SURFACE_IMPL_CONCAVE_HULL_H_
|
|
#endif
|