40#include <pcl/pcl_config.h>
43#ifndef PCL_SURFACE_IMPL_CONVEX_HULL_H_
44#define PCL_SURFACE_IMPL_CONVEX_HULL_H_
46#include <pcl/surface/convex_hull.h>
48#include <pcl/common/eigen.h>
49#include <pcl/common/transforms.h>
50#include <pcl/common/io.h>
53#include <pcl/surface/qhull.h>
56template <
typename Po
intInT>
void
59 PCL_DEBUG (
"[pcl::%s::calculateInputDimension] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().
c_str ());
60 Eigen::Vector4d xyz_centroid;
68 if (std::abs (eigen_values[0]) < std::numeric_limits<double>::epsilon () || std::abs (eigen_values[0] / eigen_values[2]) < 1.0e-3)
75template <
typename Po
intInT>
void
86 PointInT p1 = (*input_)[(*indices_)[indices_->size () - 1]];
87 PointInT p2 = (*input_)[(*indices_)[indices_->size () / 2]];
88 Eigen::Array4f
dy1dy2 = (p1.getArray4fMap () -
p0.getArray4fMap ()) / (p2.getArray4fMap () -
p0.getArray4fMap ());
91 p0 = (*input_)[(*indices_)[
rand () % indices_->
size ()]];
92 p1 = (*input_)[(*indices_)[
rand () % indices_->
size ()]];
93 p2 = (*input_)[(*indices_)[
rand () % indices_->
size ()]];
94 dy1dy2 = (p1.getArray4fMap () -
p0.getArray4fMap ()) / (p2.getArray4fMap () -
p0.getArray4fMap ());
118 if (
theta_z > projection_angle_thresh_)
123 if (
theta_x > projection_angle_thresh_)
128 if (
theta_y > projection_angle_thresh_)
143 const char* flags = qhull_flags.c_str ();
154 for (std::size_t i = 0; i < indices_->size (); ++i, j+=
dimension)
156 points[j + 0] =
static_cast<coordT> ((*input_)[(*indices_)[i]].x);
157 points[j + 1] =
static_cast<coordT> ((*input_)[(*indices_)[i]].y);
162 for (std::size_t i = 0; i < indices_->size (); ++i, j+=
dimension)
164 points[j + 0] =
static_cast<coordT> ((*input_)[(*indices_)[i]].y);
165 points[j + 1] =
static_cast<coordT> ((*input_)[(*indices_)[i]].z);
170 for (std::size_t i = 0; i < indices_->size (); ++i, j+=
dimension)
172 points[j + 0] =
static_cast<coordT> ((*input_)[(*indices_)[i]].x);
173 points[j + 1] =
static_cast<coordT> ((*input_)[(*indices_)[i]].z);
179 PCL_ERROR (
"[pcl::%s::performReconstruction2D] Invalid input!\n", getClassName ().
c_str ());
197 PCL_ERROR (
"[pcl::%s::performReconstrution2D] ERROR: qhull was unable to compute a convex hull for the given point cloud (%lu)!\n", getClassName ().
c_str (), indices_->
size ());
213 total_area_ =
qh->totvol;
235 Eigen::Vector4f centroid;
239 for (std::size_t j = 0; j <
hull.
size (); j++)
247 for (std::size_t j = 0; j <
hull.
size (); j++)
255 for (std::size_t j = 0; j <
hull.
size (); j++)
264 polygons[0].vertices.resize (
hull.
size ());
266 hull_indices_.header = input_->header;
267 hull_indices_.indices.clear ();
268 hull_indices_.indices.reserve (
hull.
size ());
272 hull_indices_.indices.push_back ((*indices_)[
idx_points[j].first]);
274 polygons[0].vertices[j] =
static_cast<unsigned int> (j);
283 hull.is_dense =
false;
288#pragma GCC diagnostic ignored "-Wold-style-cast"
291template <
typename Po
intInT>
void
306 const char *flags = qhull_flags.c_str ();
314 for (std::size_t i = 0; i < indices_->size (); ++i, j+=
dimension)
316 points[j + 0] =
static_cast<coordT> ((*input_)[(*indices_)[i]].x);
317 points[j + 1] =
static_cast<coordT> ((*input_)[(*indices_)[i]].y);
318 points[j + 2] =
static_cast<coordT> ((*input_)[(*indices_)[i]].z);
336 PCL_ERROR(
"[pcl::%s::performReconstrution3D] ERROR: qhull was unable to compute a "
337 "convex hull for the given point cloud (%zu)!\n",
338 getClassName().
c_str(),
339 static_cast<std::size_t
>(input_->size()));
372 hull_indices_.header = input_->header;
373 hull_indices_.indices.clear ();
380 hull[i] = (*input_)[hull_indices_.indices.back ()];
388 total_area_ =
qh->totarea;
389 total_volume_ =
qh->totvol;
400 polygons[
dd].vertices.resize (3);
417 hull.is_dense =
false;
420#pragma GCC diagnostic warning "-Wold-style-cast"
424template <
typename Po
intInT>
void
429 calculateInputDimension ();
432 else if (dimension_ == 3)
435 PCL_ERROR (
"[pcl::%s::performReconstruction] Error: invalid input dimension requested: %d\n",getClassName ().
c_str (),dimension_);
439template <
typename Po
intInT>
void
442 points.
header = input_->header;
443 if (!initCompute () || input_->points.empty () || indices_->empty ())
450 std::vector<pcl::Vertices> polygons;
451 performReconstruction (points, polygons,
false);
462template <
typename Po
intInT>
void
474template <
typename Po
intInT>
void
478 performReconstruction (
hull_points, polygons,
true);
482template <
typename Po
intInT>
void
485 points.
header = input_->header;
486 if (!initCompute () || input_->points.empty () || indices_->empty ())
493 performReconstruction (points, polygons,
true);
502template <
typename Po
intInT>
void
508#define PCL_INSTANTIATE_ConvexHull(T) template class PCL_EXPORTS pcl::ConvexHull<T>;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
void calculateInputDimension()
Automatically determines the dimension of input data - 2D or 3D.
void performReconstruction2D(PointCloud &points, std::vector< pcl::Vertices > &polygons, bool fill_polygon_data=false)
The reconstruction method for 2D data.
void getHullPointIndices(pcl::PointIndices &hull_point_indices) const
Retrieve the indices of the input point cloud that for the convex hull.
void performReconstruction(PointCloud &points, std::vector< pcl::Vertices > &polygons, bool fill_polygon_data=false)
The actual reconstruction method.
void reconstruct(PointCloud &points, std::vector< pcl::Vertices > &polygons)
Compute a convex hull for all points given.
void performReconstruction3D(PointCloud &points, std::vector< pcl::Vertices > &polygons, bool fill_polygon_data=false)
The reconstruction method for 3D data.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
void clear()
Removes all points in a cloud and sets the width and height to 0.
Define standard C methods and C++ classes that are common to all methods.
unsigned int computeCovarianceMatrixNormalized(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute normalized the 3x3 covariance matrix of a given set of points.
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
bool comparePoints2D(const std::pair< int, Eigen::Vector4f > &p1, const std::pair< int, Eigen::Vector4f > &p2)
Sort 2D points in a vector structure.
void toPCLPointCloud2(const pcl::PointCloud< PointT > &cloud, pcl::PCLPointCloud2 &msg)
Convert a pcl::PointCloud<T> object to a PCLPointCloud2 binary data blob.