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.
typename PointCloud::Ptr PointCloudPtr
typename PointCloud::ConstPtr PointCloudConstPtr
IndicesPtr indices_
A pointer to the vector of point indices to use.
bool initCompute()
This method should get called before starting the actual computation.
PointIndices::ConstPtr PointIndicesConstPtr
bool deinitCompute()
This method should get called after finishing the actual computation.
PointIndices::Ptr PointIndicesPtr
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