Point Cloud Library (PCL)  1.8.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 #ifndef PCL_EXTRACT_CLUSTERS_H_
41 #define PCL_EXTRACT_CLUSTERS_H_
42 
43 #include <pcl/pcl_base.h>
44 
45 #include <pcl/search/pcl_search.h>
46 
47 namespace pcl
48 {
49  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
50  /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
51  * \param cloud the point cloud message
52  * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
53  * \note the tree has to be created as a spatial locator on \a cloud
54  * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
55  * \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
56  * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
57  * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
58  * \ingroup segmentation
59  */
60  template <typename PointT> void
62  const PointCloud<PointT> &cloud, const boost::shared_ptr<search::Search<PointT> > &tree,
63  float tolerance, std::vector<PointIndices> &clusters,
64  unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
65 
66  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
67  /** \brief Decompose a region of space into clusters based on the Euclidean distance between points
68  * \param cloud the point cloud message
69  * \param indices a list of point indices to use from \a cloud
70  * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
71  * \note the tree has to be created as a spatial locator on \a cloud and \a indices
72  * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space
73  * \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
74  * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
75  * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
76  * \ingroup segmentation
77  */
78  template <typename PointT> void
80  const PointCloud<PointT> &cloud, const std::vector<int> &indices,
81  const boost::shared_ptr<search::Search<PointT> > &tree, float tolerance, std::vector<PointIndices> &clusters,
82  unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
83 
84  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
85  /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal
86  * angular deviation
87  * \param cloud the point cloud message
88  * \param normals the point cloud message containing normal information
89  * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
90  * \note the tree has to be created as a spatial locator on \a cloud
91  * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
92  * \param clusters the resultant clusters containing point indices (as a vector of PointIndices)
93  * \param eps_angle the maximum allowed difference between normals in radians for cluster/region growing
94  * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
95  * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
96  * \ingroup segmentation
97  */
98  template <typename PointT, typename Normal> void
100  const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals,
101  float tolerance, const boost::shared_ptr<KdTree<PointT> > &tree,
102  std::vector<PointIndices> &clusters, double eps_angle,
103  unsigned int min_pts_per_cluster = 1,
104  unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
105  {
106  if (tree->getInputCloud ()->points.size () != cloud.points.size ())
107  {
108  PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
109  return;
110  }
111  if (cloud.points.size () != normals.points.size ())
112  {
113  PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ());
114  return;
115  }
116 
117  // Create a bool vector of processed point indices, and initialize it to false
118  std::vector<bool> processed (cloud.points.size (), false);
119 
120  std::vector<int> nn_indices;
121  std::vector<float> nn_distances;
122  // Process all points in the indices vector
123  for (size_t i = 0; i < cloud.points.size (); ++i)
124  {
125  if (processed[i])
126  continue;
127 
128  std::vector<unsigned int> seed_queue;
129  int sq_idx = 0;
130  seed_queue.push_back (static_cast<int> (i));
131 
132  processed[i] = true;
133 
134  while (sq_idx < static_cast<int> (seed_queue.size ()))
135  {
136  // Search for sq_idx
137  if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
138  {
139  sq_idx++;
140  continue;
141  }
142 
143  for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
144  {
145  if (processed[nn_indices[j]]) // Has this point been processed before ?
146  continue;
147 
148  //processed[nn_indices[j]] = true;
149  // [-1;1]
150  double dot_p = normals.points[i].normal[0] * normals.points[nn_indices[j]].normal[0] +
151  normals.points[i].normal[1] * normals.points[nn_indices[j]].normal[1] +
152  normals.points[i].normal[2] * normals.points[nn_indices[j]].normal[2];
153  if ( fabs (acos (dot_p)) < eps_angle )
154  {
155  processed[nn_indices[j]] = true;
156  seed_queue.push_back (nn_indices[j]);
157  }
158  }
159 
160  sq_idx++;
161  }
162 
163  // If this queue is satisfactory, add to the clusters
164  if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
165  {
167  r.indices.resize (seed_queue.size ());
168  for (size_t j = 0; j < seed_queue.size (); ++j)
169  r.indices[j] = seed_queue[j];
170 
171  // These two lines should not be needed: (can anyone confirm?) -FF
172  std::sort (r.indices.begin (), r.indices.end ());
173  r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
174 
175  r.header = cloud.header;
176  clusters.push_back (r); // We could avoid a copy by working directly in the vector
177  }
178  }
179  }
180 
181 
182  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
183  /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal
184  * angular deviation
185  * \param cloud the point cloud message
186  * \param normals the point cloud message containing normal information
187  * \param indices a list of point indices to use from \a cloud
188  * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching
189  * \note the tree has to be created as a spatial locator on \a cloud
190  * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
191  * \param clusters the resultant clusters containing point indices (as PointIndices)
192  * \param eps_angle the maximum allowed difference between normals in degrees for cluster/region growing
193  * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1)
194  * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int)
195  * \ingroup segmentation
196  */
197  template <typename PointT, typename Normal>
199  const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals,
200  const std::vector<int> &indices, const boost::shared_ptr<KdTree<PointT> > &tree,
201  float tolerance, std::vector<PointIndices> &clusters, double eps_angle,
202  unsigned int min_pts_per_cluster = 1,
203  unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
204  {
205  // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
206  //and indices[i]
207  if (tree->getInputCloud ()->points.size () != cloud.points.size ())
208  {
209  PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
210  return;
211  }
212  if (tree->getIndices ()->size () != indices.size ())
213  {
214  PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ());
215  return;
216  }
217  if (cloud.points.size () != normals.points.size ())
218  {
219  PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ());
220  return;
221  }
222  // Create a bool vector of processed point indices, and initialize it to false
223  std::vector<bool> processed (cloud.points.size (), false);
224 
225  std::vector<int> nn_indices;
226  std::vector<float> nn_distances;
227  // Process all points in the indices vector
228  for (size_t i = 0; i < indices.size (); ++i)
229  {
230  if (processed[indices[i]])
231  continue;
232 
233  std::vector<int> seed_queue;
234  int sq_idx = 0;
235  seed_queue.push_back (indices[i]);
236 
237  processed[indices[i]] = true;
238 
239  while (sq_idx < static_cast<int> (seed_queue.size ()))
240  {
241  // Search for sq_idx
242  if (!tree->radiusSearch (cloud.points[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances))
243  {
244  sq_idx++;
245  continue;
246  }
247 
248  for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
249  {
250  if (processed[nn_indices[j]]) // Has this point been processed before ?
251  continue;
252 
253  //processed[nn_indices[j]] = true;
254  // [-1;1]
255  double dot_p =
256  normals.points[indices[i]].normal[0] * normals.points[indices[nn_indices[j]]].normal[0] +
257  normals.points[indices[i]].normal[1] * normals.points[indices[nn_indices[j]]].normal[1] +
258  normals.points[indices[i]].normal[2] * normals.points[indices[nn_indices[j]]].normal[2];
259  if ( fabs (acos (dot_p)) < eps_angle )
260  {
261  processed[nn_indices[j]] = true;
262  seed_queue.push_back (nn_indices[j]);
263  }
264  }
265 
266  sq_idx++;
267  }
268 
269  // If this queue is satisfactory, add to the clusters
270  if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
271  {
273  r.indices.resize (seed_queue.size ());
274  for (size_t j = 0; j < seed_queue.size (); ++j)
275  r.indices[j] = seed_queue[j];
276 
277  // These two lines should not be needed: (can anyone confirm?) -FF
278  std::sort (r.indices.begin (), r.indices.end ());
279  r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
280 
281  r.header = cloud.header;
282  clusters.push_back (r);
283  }
284  }
285  }
286 
287  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
288  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
289  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
290  /** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense.
291  * \author Radu Bogdan Rusu
292  * \ingroup segmentation
293  */
294  template <typename PointT>
295  class EuclideanClusterExtraction: public PCLBase<PointT>
296  {
298 
299  public:
301  typedef typename PointCloud::Ptr PointCloudPtr;
303 
306 
309 
310  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
311  /** \brief Empty constructor. */
313  cluster_tolerance_ (0),
315  max_pts_per_cluster_ (std::numeric_limits<int>::max ())
316  {};
317 
318  /** \brief Provide a pointer to the search object.
319  * \param[in] tree a pointer to the spatial search object.
320  */
321  inline void
322  setSearchMethod (const KdTreePtr &tree)
323  {
324  tree_ = tree;
325  }
326 
327  /** \brief Get a pointer to the search method used.
328  * @todo fix this for a generic search tree
329  */
330  inline KdTreePtr
331  getSearchMethod () const
332  {
333  return (tree_);
334  }
335 
336  /** \brief Set the spatial cluster tolerance as a measure in the L2 Euclidean space
337  * \param[in] tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space
338  */
339  inline void
340  setClusterTolerance (double tolerance)
341  {
342  cluster_tolerance_ = tolerance;
343  }
344 
345  /** \brief Get the spatial cluster tolerance as a measure in the L2 Euclidean space. */
346  inline double
348  {
349  return (cluster_tolerance_);
350  }
351 
352  /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid.
353  * \param[in] min_cluster_size the minimum cluster size
354  */
355  inline void
356  setMinClusterSize (int min_cluster_size)
357  {
358  min_pts_per_cluster_ = min_cluster_size;
359  }
360 
361  /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */
362  inline int
364  {
365  return (min_pts_per_cluster_);
366  }
367 
368  /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid.
369  * \param[in] max_cluster_size the maximum cluster size
370  */
371  inline void
372  setMaxClusterSize (int max_cluster_size)
373  {
374  max_pts_per_cluster_ = max_cluster_size;
375  }
376 
377  /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */
378  inline int
380  {
381  return (max_pts_per_cluster_);
382  }
383 
384  /** \brief Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
385  * \param[out] clusters the resultant point clusters
386  */
387  void
388  extract (std::vector<PointIndices> &clusters);
389 
390  protected:
391  // Members derived from the base class
392  using BasePCLBase::input_;
393  using BasePCLBase::indices_;
396 
397  /** \brief A pointer to the spatial search object. */
399 
400  /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */
402 
403  /** \brief The minimum number of points that a cluster needs to contain in order to be considered valid (default = 1). */
405 
406  /** \brief The maximum number of points that a cluster needs to contain in order to be considered valid (default = MAXINT). */
408 
409  /** \brief Class getName method. */
410  virtual std::string getClassName () const { return ("EuclideanClusterExtraction"); }
411 
412  };
413 
414  /** \brief Sort clusters method (for std::sort).
415  * \ingroup segmentation
416  */
417  inline bool
419  {
420  return (a.indices.size () < b.indices.size ());
421  }
422 }
423 
424 #ifdef PCL_NO_PRECOMPILE
425 #include <pcl/segmentation/impl/extract_clusters.hpp>
426 #endif
427 
428 #endif //#ifndef PCL_EXTRACT_CLUSTERS_H_
int min_pts_per_cluster_
The minimum number of points that a cluster needs to contain in order to be considered valid (default...
int getMaxClusterSize() const
Get the maximum number of points that a cluster needs to contain in order to be considered valid...
bool comparePointClusters(const pcl::PointIndices &a, const pcl::PointIndices &b)
Sort clusters method (for std::sort).
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
void extractEuclideanClusters(const PointCloud< PointT > &cloud, const boost::shared_ptr< search::Search< PointT > > &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...
pcl::search::Search< PointT > KdTree
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
pcl::search::Search< PointT >::Ptr KdTreePtr
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: pcl_base.h:153
std::vector< int > indices
Definition: PointIndices.h:19
void setClusterTolerance(double tolerance)
Set the spatial cluster tolerance as a measure in the L2 Euclidean space.
PointIndices::ConstPtr PointIndicesConstPtr
KdTreePtr tree_
A pointer to the spatial search object.
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
bool initCompute()
This method should get called before starting the actual computation.
Definition: pcl_base.hpp:139
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
void setMaxClusterSize(int max_cluster_size)
Set the maximum number of points that a cluster needs to contain in order to be considered valid...
int getMinClusterSize() const
Get the minimum number of points that a cluster needs to contain in order to be considered valid...
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
PCL base class.
Definition: pcl_base.h:68
void extract(std::vector< PointIndices > &clusters)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
::pcl::PCLHeader header
Definition: PointIndices.h:17
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition: pcl_base.hpp:174
virtual std::string getClassName() const
Class getName method.
EuclideanClusterExtraction()
Empty constructor.
boost::shared_ptr< ::pcl::PointIndices > Ptr
Definition: PointIndices.h:22
void setMinClusterSize(int min_cluster_size)
Set the minimum number of points that a cluster needs to contain in order to be considered valid...
KdTreePtr getSearchMethod() const
Get a pointer to the search method used.
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:150
EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sen...
int max_pts_per_cluster_
The maximum number of points that a cluster needs to contain in order to be considered valid (default...
double cluster_tolerance_
The spatial cluster tolerance as a measure in the L2 Euclidean space.
double getClusterTolerance() const
Get the spatial cluster tolerance as a measure in the L2 Euclidean space.
PointCloud::ConstPtr PointCloudConstPtr
KdTree represents the base spatial locator class for kd-tree implementations.
Definition: kdtree.h:56
pcl::PointCloud< PointT > PointCloud
boost::shared_ptr< ::pcl::PointIndices const > ConstPtr
Definition: PointIndices.h:23
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
Generic search class.
Definition: search.h:74