Point Cloud Library (PCL)  1.14.1-dev
extract_clusters.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #pragma once
41 
42 #include <pcl/console/print.h> // for PCL_ERROR
43 #include <pcl/pcl_base.h>
44 
45 #include <pcl/search/search.h> // for Search
46 #include <pcl/search/kdtree.h> // for KdTree
47 
48 namespace pcl
49 {
50  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
51  /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
52  * \param cloud the point cloud message
53  * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
54  * \note the tree has to be created as a spatial locator on \a cloud
55  * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
56  * \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
57  * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
58  * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
59  * \ingroup segmentation
60  */
61  template <typename PointT> void
63  const PointCloud<PointT> &cloud, const typename search::Search<PointT>::Ptr &tree,
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) ());
66 
67  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
68  /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
69  * \param cloud the point cloud message
70  * \param indices a list of point indices to use from \a cloud
71  * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
72  * \note the tree has to be created as a spatial locator on \a cloud and \a indices
73  * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
74  * \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
75  * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
76  * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
77  * \ingroup segmentation
78  */
79  template <typename PointT> void
81  const PointCloud<PointT> &cloud, const Indices &indices,
82  const typename search::Search<PointT>::Ptr &tree, float tolerance, std::vector<PointIndices> &clusters,
83  unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
84 
85  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
86  /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal
87  * angular deviation between points. Each point added to the cluster is origin to another radius search. Each point
88  * within radius range will be compared to the origin in respect to normal angle and euclidean distance. If both
89  * are under their respective threshold the point will be added to the cluster. Generally speaking the cluster
90  * algorithm will not stop on smooth surfaces but on surfaces with sharp edges.
91  * \param cloud the point cloud message
92  * \param normals the point cloud message containing normal information
93  * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
94  * \note the tree has to be created as a spatial locator on \a cloud
95  * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
96  * \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
97  * \param eps_angle the maximum allowed difference between normals in radians for cluster/region growing
98  * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
99  * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
100  * \ingroup segmentation
101  */
102  template <typename PointT, typename Normal> void
104  const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals,
105  float tolerance, const typename KdTree<PointT>::Ptr &tree,
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) ())
109  {
110  if (tree->getInputCloud ()->size () != cloud.size ())
111  {
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()));
116  return;
117  }
118  if (cloud.size () != normals.size ())
119  {
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()));
124  return;
125  }
126  // If tree gives sorted results, we can skip the first one because it is the query point itself
127  const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;
128  const double cos_eps_angle = std::cos (eps_angle); // compute this once instead of acos many times (faster)
129 
130  // Create a bool vector of processed point indices, and initialize it to false
131  std::vector<bool> processed (cloud.size (), false);
132 
133  Indices nn_indices;
134  std::vector<float> nn_distances;
135  // Process all points in the indices vector
136  for (std::size_t i = 0; i < cloud.size (); ++i)
137  {
138  if (processed[i])
139  continue;
140 
141  Indices seed_queue;
142  int sq_idx = 0;
143  seed_queue.push_back (static_cast<index_t> (i));
144 
145  processed[i] = true;
146 
147  while (sq_idx < static_cast<int> (seed_queue.size ()))
148  {
149  // Search for sq_idx
150  if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
151  {
152  sq_idx++;
153  continue;
154  }
155 
156  for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
157  {
158  if (processed[nn_indices[j]]) // Has this point been processed before ?
159  continue;
160 
161  //processed[nn_indices[j]] = true;
162  // [-1;1]
163  double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0] +
164  normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] +
165  normals[seed_queue[sq_idx]].normal[2] * normals[nn_indices[j]].normal[2];
166  if ( std::abs (dot_p) > cos_eps_angle )
167  {
168  processed[nn_indices[j]] = true;
169  seed_queue.push_back (nn_indices[j]);
170  }
171  }
172 
173  sq_idx++;
174  }
175 
176  // If this queue is satisfactory, add to the clusters
177  if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
178  {
180  r.indices.resize (seed_queue.size ());
181  for (std::size_t j = 0; j < seed_queue.size (); ++j)
182  r.indices[j] = seed_queue[j];
183 
184  // After clustering, indices are out of order, so sort them
185  std::sort (r.indices.begin (), r.indices.end ());
186 
187  r.header = cloud.header;
188  clusters.push_back (r); // We could avoid a copy by working directly in the vector
189  }
190  else
191  {
192  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",
193  seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
194  }
195  }
196  }
197 
198 
199  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
200  /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal
201  * angular deviation between points. Each point added to the cluster is origin to another radius search. Each point
202  * within radius range will be compared to the origin in respect to normal angle and euclidean distance. If both
203  * are under their respective threshold the point will be added to the cluster. Generally speaking the cluster
204  * algorithm will not stop on smooth surfaces but on surfaces with sharp edges.
205  * \param cloud the point cloud message
206  * \param normals the point cloud message containing normal information
207  * \param indices a list of point indices to use from \a cloud
208  * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
209  * \note the tree has to be created as a spatial locator on \a cloud
210  * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
211  * \param clusters the resultant clusters containing point indices (as PointIndices)
212  * \param eps_angle the maximum allowed difference between normals in radians for cluster/region growing
213  * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
214  * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
215  * \ingroup segmentation
216  */
217  template <typename PointT, typename Normal>
219  const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals,
220  const Indices &indices, const typename KdTree<PointT>::Ptr &tree,
221  float tolerance, std::vector<PointIndices> &clusters, double eps_angle,
222  unsigned int min_pts_per_cluster = 1,
223  unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
224  {
225  // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
226  //and indices[i]
227  if (tree->getInputCloud()->size() != cloud.size()) {
228  PCL_ERROR("[pcl::extractEuclideanClusters] Tree built for a different point "
229  "cloud dataset (%zu) than the input cloud (%zu)!\n",
230  static_cast<std::size_t>(tree->getInputCloud()->size()),
231  static_cast<std::size_t>(cloud.size()));
232  return;
233  }
234  if (tree->getIndices()->size() != indices.size()) {
235  PCL_ERROR("[pcl::extractEuclideanClusters] Tree built for a different set of "
236  "indices (%zu) than the input set (%zu)!\n",
237  static_cast<std::size_t>(tree->getIndices()->size()),
238  indices.size());
239  return;
240  }
241  if (cloud.size() != normals.size()) {
242  PCL_ERROR("[pcl::extractEuclideanClusters] Number of points in the input point "
243  "cloud (%zu) different than normals (%zu)!\n",
244  static_cast<std::size_t>(cloud.size()),
245  static_cast<std::size_t>(normals.size()));
246  return;
247  }
248  // If tree gives sorted results, we can skip the first one because it is the query point itself
249  const std::size_t nn_start_idx = tree->getSortedResults () ? 1 : 0;
250  const double cos_eps_angle = std::cos (eps_angle); // compute this once instead of acos many times (faster)
251  // Create a bool vector of processed point indices, and initialize it to false
252  std::vector<bool> processed (cloud.size (), false);
253 
254  Indices nn_indices;
255  std::vector<float> nn_distances;
256  // Process all points in the indices vector
257  for (const auto& point_idx : indices)
258  {
259  if (processed[point_idx])
260  continue;
261 
262  Indices seed_queue;
263  int sq_idx = 0;
264  seed_queue.push_back (point_idx);
265 
266  processed[point_idx] = true;
267 
268  while (sq_idx < static_cast<int> (seed_queue.size ()))
269  {
270  // Search for sq_idx
271  if (!tree->radiusSearch (cloud[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances))
272  {
273  sq_idx++;
274  continue;
275  }
276 
277  for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
278  {
279  if (processed[nn_indices[j]]) // Has this point been processed before ?
280  continue;
281 
282  //processed[nn_indices[j]] = true;
283  // [-1;1]
284  double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0] +
285  normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] +
286  normals[seed_queue[sq_idx]].normal[2] * normals[nn_indices[j]].normal[2];
287  if ( std::abs (dot_p) > cos_eps_angle )
288  {
289  processed[nn_indices[j]] = true;
290  seed_queue.push_back (nn_indices[j]);
291  }
292  }
293 
294  sq_idx++;
295  }
296 
297  // If this queue is satisfactory, add to the clusters
298  if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
299  {
301  r.indices.resize (seed_queue.size ());
302  for (std::size_t j = 0; j < seed_queue.size (); ++j)
303  r.indices[j] = seed_queue[j];
304 
305  // After clustering, indices are out of order, so sort them
306  std::sort (r.indices.begin (), r.indices.end ());
307 
308  r.header = cloud.header;
309  clusters.push_back (r);
310  }
311  else
312  {
313  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",
314  seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
315  }
316  }
317  }
318 
319  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
320  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
321  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
322  /** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense.
323  * \author Radu Bogdan Rusu
324  * \ingroup segmentation
325  */
326  template <typename PointT>
327  class EuclideanClusterExtraction: public PCLBase<PointT>
328  {
330 
331  public:
333  using PointCloudPtr = typename PointCloud::Ptr;
335 
337  using KdTreePtr = typename KdTree::Ptr;
338 
341 
342  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
343  /** \brief Empty constructor. */
345 
346  /** \brief Provide a pointer to the search object.
347  * \param[in] tree a pointer to the spatial search object.
348  */
349  inline void
350  setSearchMethod (const KdTreePtr &tree)
351  {
352  tree_ = tree;
353  }
354 
355  /** \brief Get a pointer to the search method used.
356  * @todo fix this for a generic search tree
357  */
358  inline KdTreePtr
359  getSearchMethod () const
360  {
361  return (tree_);
362  }
363 
364  /** \brief Set the spatial cluster tolerance as a measure in the L2 Euclidean space
365  * \param[in] tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
366  */
367  inline void
368  setClusterTolerance (double tolerance)
369  {
370  cluster_tolerance_ = tolerance;
371  }
372 
373  /** \brief Get the spatial cluster tolerance as a measure in the L2 Euclidean space. */
374  inline double
376  {
377  return (cluster_tolerance_);
378  }
379 
380  /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid.
381  * \param[in] min_cluster_size the minimum cluster size
382  */
383  inline void
384  setMinClusterSize (pcl::uindex_t min_cluster_size)
385  {
386  min_pts_per_cluster_ = min_cluster_size;
387  }
388 
389  /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */
390  inline pcl::uindex_t
392  {
393  return (min_pts_per_cluster_);
394  }
395 
396  /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid.
397  * \param[in] max_cluster_size the maximum cluster size
398  */
399  inline void
400  setMaxClusterSize (pcl::uindex_t max_cluster_size)
401  {
402  max_pts_per_cluster_ = max_cluster_size;
403  }
404 
405  /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */
406  inline pcl::uindex_t
408  {
409  return (max_pts_per_cluster_);
410  }
411 
412  /** \brief Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
413  * \param[out] clusters the resultant point clusters
414  */
415  void
416  extract (std::vector<PointIndices> &clusters);
417 
418  protected:
419  // Members derived from the base class
420  using BasePCLBase::input_;
421  using BasePCLBase::indices_;
424 
425  /** \brief A pointer to the spatial search object. */
426  KdTreePtr tree_{nullptr};
427 
428  /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */
429  double cluster_tolerance_{0.0};
430 
431  /** \brief The minimum number of points that a cluster needs to contain in order to be considered valid (default = 1). */
433 
434  /** \brief The maximum number of points that a cluster needs to contain in order to be considered valid (default = MAXINT). */
435  pcl::uindex_t max_pts_per_cluster_{std::numeric_limits<pcl::uindex_t>::max()};
436 
437  /** \brief Class getName method. */
438  virtual std::string getClassName () const { return ("EuclideanClusterExtraction"); }
439 
440  };
441 
442  /** \brief Sort clusters method (for std::sort).
443  * \ingroup segmentation
444  */
445  inline bool
447  {
448  return (a.indices.size () < b.indices.size ());
449  }
450 }
451 
452 #ifdef PCL_NO_PRECOMPILE
453 #include <pcl/segmentation/impl/extract_clusters.hpp>
454 #endif
EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sen...
pcl::uindex_t getMaxClusterSize() const
Get the maximum number of points that a cluster needs to contain in order to be considered valid.
double cluster_tolerance_
The spatial cluster tolerance as a measure in the L2 Euclidean space.
pcl::uindex_t max_pts_per_cluster_
The maximum number of points that a cluster needs to contain in order to be considered valid (default...
void extract(std::vector< PointIndices > &clusters)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
EuclideanClusterExtraction()=default
Empty constructor.
double getClusterTolerance() const
Get the spatial cluster tolerance as a measure in the L2 Euclidean space.
virtual std::string getClassName() const
Class getName method.
void setClusterTolerance(double tolerance)
Set the spatial cluster tolerance as a measure in the L2 Euclidean space.
pcl::uindex_t min_pts_per_cluster_
The minimum number of points that a cluster needs to contain in order to be considered valid (default...
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
KdTreePtr getSearchMethod() const
Get a pointer to the search method used.
void setMaxClusterSize(pcl::uindex_t max_cluster_size)
Set the maximum number of points that a cluster needs to contain in order to be considered valid.
void setMinClusterSize(pcl::uindex_t min_cluster_size)
Set the minimum number of points that a cluster needs to contain in order to be considered valid.
KdTreePtr tree_
A pointer to the spatial search object.
pcl::uindex_t getMinClusterSize() const
Get the minimum number of points that a cluster needs to contain in order to be considered valid.
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: kdtree.h:101
IndicesConstPtr getIndices() const
Get a pointer to the vector of indices used.
Definition: kdtree.h:94
bool getSortedResults() const
Gets whether the results should be sorted (ascending in the distance) or not Otherwise the results ma...
Definition: kdtree.h:339
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
Definition: kdtree.h:69
PCL base class.
Definition: pcl_base.h:70
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:147
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: pcl_base.h:150
bool initCompute()
This method should get called before starting the actual computation.
Definition: pcl_base.hpp:138
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_base.h:77
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition: pcl_base.hpp:175
PointIndices::Ptr PointIndicesPtr
Definition: pcl_base.h:76
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:663
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
std::size_t size() const
Definition: point_cloud.h:443
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
Generic search class.
Definition: search.h:75
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
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.
Definition: types.h:120
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:13
::pcl::PCLHeader header
Definition: PointIndices.h:18
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition: PointIndices.h:14