Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
48namespace 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>
328 {
330
331 public:
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
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
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
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
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
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. */
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...
typename PointCloud::Ptr PointCloudPtr
typename PointCloud::ConstPtr PointCloudConstPtr
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.
PointIndices::ConstPtr PointIndicesConstPtr
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::PointCloud< PointT > PointCloud
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
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
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition pcl_base.hpp:175
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.
std::size_t size() const
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
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
::pcl::PCLHeader header
shared_ptr< const ::pcl::PointIndices > ConstPtr