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