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) ())
110 if (tree->getInputCloud ()->size () != cloud.
size ())
112 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point "
113 "cloud dataset (%zu) than the input cloud (%zu)!\n",
114 static_cast<std::size_t
>(tree->getInputCloud()->size()),
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;
167 seed_queue.push_back (nn_indices[j]);
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)
187 clusters.push_back (r);
191 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",
192 seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
216 template <
typename Po
intT,
typename Normal>
220 float tolerance, std::vector<PointIndices> &clusters,
double eps_angle,
221 unsigned int min_pts_per_cluster = 1,
222 unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
226 if (tree->getInputCloud()->size() != cloud.
size()) {
227 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point "
228 "cloud dataset (%zu) than the input cloud (%zu)!\n",
229 static_cast<std::size_t
>(tree->getInputCloud()->size()),
230 static_cast<std::size_t
>(cloud.
size()));
233 if (tree->getIndices()->size() != indices.size()) {
234 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different set of "
235 "indices (%zu) than the input set (%zu)!\n",
236 static_cast<std::size_t
>(tree->getIndices()->size()),
240 if (cloud.
size() != normals.
size()) {
241 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point "
242 "cloud (%zu) different than normals (%zu)!\n",
243 static_cast<std::size_t
>(cloud.
size()),
244 static_cast<std::size_t
>(normals.
size()));
247 const double cos_eps_angle = std::cos (eps_angle);
249 std::vector<bool> processed (cloud.
size (),
false);
252 std::vector<float> nn_distances;
254 for (
const auto& point_idx : indices)
256 if (processed[point_idx])
261 seed_queue.push_back (point_idx);
263 processed[point_idx] =
true;
265 while (sq_idx <
static_cast<int> (seed_queue.size ()))
268 if (!tree->radiusSearch (cloud[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances))
274 for (std::size_t j = 1; j < nn_indices.size (); ++j)
276 if (processed[nn_indices[j]])
281 double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0] +
282 normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] +
283 normals[seed_queue[sq_idx]].normal[2] * normals[nn_indices[j]].normal[2];
284 if ( std::abs (dot_p) > cos_eps_angle )
286 processed[nn_indices[j]] =
true;
287 seed_queue.push_back (nn_indices[j]);
295 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
298 r.
indices.resize (seed_queue.size ());
299 for (std::size_t j = 0; j < seed_queue.size (); ++j)
307 clusters.push_back (r);
311 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",
312 seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
324 template <
typename Po
intT>
418 extract (std::vector<PointIndices> &clusters);
440 virtual std::string
getClassName ()
const {
return (
"EuclideanClusterExtraction"); }
454#ifdef PCL_NO_PRECOMPILE
455#include <pcl/segmentation/impl/extract_clusters.hpp>
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.
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, detail::index_type_signed > index_t
Type used for an index in PCL.
IndicesAllocator<> Indices
Type used for indices in PCL.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
shared_ptr< ::pcl::PointIndices > Ptr
shared_ptr< const ::pcl::PointIndices > ConstPtr