42#include <pcl/console/print.h>
43#include <pcl/pcl_base.h>
45#include <pcl/search/search.h>
46#include <pcl/search/kdtree.h>
61 template <
typename Po
intT>
void
64 float tolerance, std::vector<PointIndices> &clusters,
65 unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
79 template <
typename Po
intT>
void
83 unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
102 template <
typename Po
intT,
typename Normal>
void
106 std::vector<PointIndices> &clusters,
double eps_angle,
107 unsigned int min_pts_per_cluster = 1,
108 unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
112 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point "
113 "cloud dataset (%zu) than the input cloud (%zu)!\n",
115 static_cast<std::size_t
>(cloud.
size()));
118 if (cloud.
size () != normals.
size ())
120 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point "
121 "cloud (%zu) different than normals (%zu)!\n",
122 static_cast<std::size_t
>(cloud.
size()),
123 static_cast<std::size_t
>(normals.
size()));
126 const double cos_eps_angle = std::cos (eps_angle);
129 std::vector<bool> processed (cloud.
size (),
false);
132 std::vector<float> nn_distances;
134 for (std::size_t i = 0; i < cloud.
size (); ++i)
141 seed_queue.push_back (
static_cast<index_t> (i));
145 while (sq_idx <
static_cast<int> (seed_queue.size ()))
148 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
154 for (std::size_t j = 1; j < nn_indices.size (); ++j)
156 if (processed[nn_indices[j]])
161 double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0] +
162 normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] +
163 normals[seed_queue[sq_idx]].normal[2] * normals[nn_indices[j]].normal[2];
164 if ( std::abs (dot_p) > cos_eps_angle )
166 processed[nn_indices[j]] =
true;
175 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
178 r.
indices.resize (seed_queue.size ());
179 for (std::size_t j = 0; j < seed_queue.size (); ++j)
186 clusters.push_back (r);
190 PCL_DEBUG(
"[pcl::extractEuclideanClusters] This cluster has %zu points, which is not between %u and %u points, so it is not a final cluster\n",
191 seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
215 template <
typename Po
intT,
typename Normal>
219 float tolerance, std::vector<PointIndices> &clusters,
double eps_angle,
220 unsigned int min_pts_per_cluster = 1,
221 unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
226 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point "
227 "cloud dataset (%zu) than the input cloud (%zu)!\n",
229 static_cast<std::size_t
>(cloud.
size()));
232 if (tree->
getIndices()->size() != indices.size()) {
233 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different set of "
234 "indices (%zu) than the input set (%zu)!\n",
235 static_cast<std::size_t
>(tree->
getIndices()->size()),
239 if (cloud.
size() != normals.
size()) {
240 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point "
241 "cloud (%zu) different than normals (%zu)!\n",
242 static_cast<std::size_t
>(cloud.
size()),
243 static_cast<std::size_t
>(normals.
size()));
246 const double cos_eps_angle = std::cos (eps_angle);
248 std::vector<bool> processed (cloud.
size (),
false);
251 std::vector<float> nn_distances;
253 for (
const auto& point_idx : indices)
255 if (processed[point_idx])
260 seed_queue.push_back (point_idx);
262 processed[point_idx] =
true;
264 while (sq_idx <
static_cast<int> (seed_queue.size ()))
267 if (!tree->
radiusSearch (cloud[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances))
273 for (std::size_t j = 1; j < nn_indices.size (); ++j)
275 if (processed[nn_indices[j]])
280 double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0] +
281 normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] +
282 normals[seed_queue[sq_idx]].normal[2] * normals[nn_indices[j]].normal[2];
283 if ( std::abs (dot_p) > cos_eps_angle )
285 processed[nn_indices[j]] =
true;
294 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
297 r.
indices.resize (seed_queue.size ());
298 for (std::size_t j = 0; j < seed_queue.size (); ++j)
305 clusters.push_back (r);
309 PCL_DEBUG(
"[pcl::extractEuclideanClusters] This cluster has %zu points, which is not between %u and %u points, so it is not a final cluster\n",
310 seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
322 template <
typename Po
intT>
412 extract (std::vector<PointIndices> &clusters);
434 virtual std::string
getClassName ()
const {
return (
"EuclideanClusterExtraction"); }
448#ifdef PCL_NO_PRECOMPILE
449#include <pcl/segmentation/impl/extract_clusters.hpp>
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
IndicesConstPtr getIndices() const
Get a pointer to the vector of indices used.
virtual int radiusSearch(const PointT &p_q, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const =0
Search for all the nearest neighbors of the query point in a given radius.
shared_ptr< KdTree< PointT > > Ptr
PointCloudConstPtr input_
The input point cloud dataset.
IndicesPtr indices_
A pointer to the vector of point indices to use.
bool initCompute()
This method should get called before starting the actual computation.
bool deinitCompute()
This method should get called after finishing the actual computation.
PointCloud represents the base class in PCL for storing collections of 3D points.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
pcl::PCLHeader header
The point cloud header.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
shared_ptr< pcl::search::Search< PointT > > Ptr
void extractEuclideanClusters(const PointCloud< PointT > &cloud, const typename search::Search< PointT >::Ptr &tree, float tolerance, std::vector< PointIndices > &clusters, unsigned int min_pts_per_cluster=1, unsigned int max_pts_per_cluster=(std::numeric_limits< int >::max)())
Decompose a region of space into clusters based on the Euclidean distance between points.
bool comparePointClusters(const pcl::PointIndices &a, const pcl::PointIndices &b)
Sort clusters method (for std::sort).
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
IndicesAllocator<> Indices
Type used for indices in PCL.
shared_ptr< ::pcl::PointIndices > Ptr
shared_ptr< const ::pcl::PointIndices > ConstPtr