Point Cloud Library (PCL)  1.12.1-dev
conditional_euclidean_clustering.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the copyright holder(s) nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  */
36 
37 #ifndef PCL_SEGMENTATION_IMPL_CONDITIONAL_EUCLIDEAN_CLUSTERING_HPP_
38 #define PCL_SEGMENTATION_IMPL_CONDITIONAL_EUCLIDEAN_CLUSTERING_HPP_
39 
40 #include <pcl/segmentation/conditional_euclidean_clustering.h>
41 #include <pcl/search/organized.h> // for OrganizedNeighbor
42 #include <pcl/search/kdtree.h> // for KdTree
43 
44 template<typename PointT> void
46 {
47  // Prepare output (going to use push_back)
48  clusters.clear ();
49  if (extract_removed_clusters_)
50  {
51  small_clusters_->clear ();
52  large_clusters_->clear ();
53  }
54 
55  // Validity checks
56  if (!initCompute () || input_->points.empty () || indices_->empty () || !condition_function_)
57  return;
58 
59  // Initialize the search class
60  if (!searcher_)
61  {
62  if (input_->isOrganized ())
63  searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
64  else
65  searcher_.reset (new pcl::search::KdTree<PointT> ());
66  }
67  searcher_->setInputCloud (input_, indices_);
68 
69  // Temp variables used by search class
70  Indices nn_indices;
71  std::vector<float> nn_distances;
72 
73  // Create a bool vector of processed point indices, and initialize it to false
74  // Need to have it contain all possible points because radius search can not return indices into indices
75  std::vector<bool> processed (input_->size (), false);
76 
77  // Process all points indexed by indices_
78  for (const auto& iindex : (*indices_)) // iindex = input index
79  {
80  // Has this point been processed before?
81  if (iindex == UNAVAILABLE || processed[iindex])
82  continue;
83 
84  // Set up a new growing cluster
85  Indices current_cluster;
86  int cii = 0; // cii = cluster indices iterator
87 
88  // Add the point to the cluster
89  current_cluster.push_back (iindex);
90  processed[iindex] = true;
91 
92  // Process the current cluster (it can be growing in size as it is being processed)
93  while (cii < static_cast<int> (current_cluster.size ()))
94  {
95  // Search for neighbors around the current seed point of the current cluster
96  if (searcher_->radiusSearch ((*input_)[current_cluster[cii]], cluster_tolerance_, nn_indices, nn_distances) < 1)
97  {
98  cii++;
99  continue;
100  }
101 
102  // Process the neighbors
103  for (int nii = 1; nii < static_cast<int> (nn_indices.size ()); ++nii) // nii = neighbor indices iterator
104  {
105  // Has this point been processed before?
106  if (nn_indices[nii] == UNAVAILABLE || processed[nn_indices[nii]])
107  continue;
108 
109  // Validate if condition holds
110  if (condition_function_ ((*input_)[current_cluster[cii]], (*input_)[nn_indices[nii]], nn_distances[nii]))
111  {
112  // Add the point to the cluster
113  current_cluster.push_back (nn_indices[nii]);
114  processed[nn_indices[nii]] = true;
115  }
116  }
117  cii++;
118  }
119 
120  // If extracting removed clusters, all clusters need to be saved, otherwise only the ones within the given cluster size range
121  if (extract_removed_clusters_ ||
122  (static_cast<int> (current_cluster.size ()) >= min_cluster_size_ &&
123  static_cast<int> (current_cluster.size ()) <= max_cluster_size_))
124  {
126  pi.header = input_->header;
127  pi.indices.resize (current_cluster.size ());
128  for (int ii = 0; ii < static_cast<int> (current_cluster.size ()); ++ii) // ii = indices iterator
129  pi.indices[ii] = current_cluster[ii];
130 
131  if (extract_removed_clusters_ && static_cast<int> (current_cluster.size ()) < min_cluster_size_)
132  small_clusters_->push_back (pi);
133  else if (extract_removed_clusters_ && static_cast<int> (current_cluster.size ()) > max_cluster_size_)
134  large_clusters_->push_back (pi);
135  else
136  clusters.push_back (pi);
137  }
138  }
139 
140  deinitCompute ();
141 }
142 
143 #define PCL_INSTANTIATE_ConditionalEuclideanClustering(T) template class PCL_EXPORTS pcl::ConditionalEuclideanClustering<T>;
144 
145 #endif // PCL_SEGMENTATION_IMPL_CONDITIONAL_EUCLIDEAN_CLUSTERING_HPP_
146 
void segment(IndicesClusters &clusters)
Segment the input into separate clusters.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
Definition: organized.h:61
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
std::vector< pcl::PointIndices > IndicesClusters
::pcl::PCLHeader header
Definition: PointIndices.h:18