Point Cloud Library (PCL)  1.9.0-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 
42 template<typename PointT> void
44 {
45  // Prepare output (going to use push_back)
46  clusters.clear ();
47  if (extract_removed_clusters_)
48  {
49  small_clusters_->clear ();
50  large_clusters_->clear ();
51  }
52 
53  // Validity checks
54  if (!initCompute () || input_->points.empty () || indices_->empty () || !condition_function_)
55  return;
56 
57  // Initialize the search class
58  if (!searcher_)
59  {
60  if (input_->isOrganized ())
61  searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
62  else
63  searcher_.reset (new pcl::search::KdTree<PointT> ());
64  }
65  searcher_->setInputCloud (input_, indices_);
66 
67  // Temp variables used by search class
68  std::vector<int> nn_indices;
69  std::vector<float> nn_distances;
70 
71  // Create a bool vector of processed point indices, and initialize it to false
72  // Need to have it contain all possible points because radius search can not return indices into indices
73  std::vector<bool> processed (input_->points.size (), false);
74 
75  // Process all points indexed by indices_
76  for (int iii = 0; iii < static_cast<int> (indices_->size ()); ++iii) // iii = input indices iterator
77  {
78  // Has this point been processed before?
79  if ((*indices_)[iii] == -1 || processed[(*indices_)[iii]])
80  continue;
81 
82  // Set up a new growing cluster
83  std::vector<int> current_cluster;
84  int cii = 0; // cii = cluster indices iterator
85 
86  // Add the point to the cluster
87  current_cluster.push_back ((*indices_)[iii]);
88  processed[(*indices_)[iii]] = true;
89 
90  // Process the current cluster (it can be growing in size as it is being processed)
91  while (cii < static_cast<int> (current_cluster.size ()))
92  {
93  // Search for neighbors around the current seed point of the current cluster
94  if (searcher_->radiusSearch (input_->points[current_cluster[cii]], cluster_tolerance_, nn_indices, nn_distances) < 1)
95  {
96  cii++;
97  continue;
98  }
99 
100  // Process the neighbors
101  for (int nii = 1; nii < static_cast<int> (nn_indices.size ()); ++nii) // nii = neighbor indices iterator
102  {
103  // Has this point been processed before?
104  if (nn_indices[nii] == -1 || processed[nn_indices[nii]])
105  continue;
106 
107  // Validate if condition holds
108  if (condition_function_ (input_->points[current_cluster[cii]], input_->points[nn_indices[nii]], nn_distances[nii]))
109  {
110  // Add the point to the cluster
111  current_cluster.push_back (nn_indices[nii]);
112  processed[nn_indices[nii]] = true;
113  }
114  }
115  cii++;
116  }
117 
118  // If extracting removed clusters, all clusters need to be saved, otherwise only the ones within the given cluster size range
119  if (extract_removed_clusters_ ||
120  (static_cast<int> (current_cluster.size ()) >= min_cluster_size_ &&
121  static_cast<int> (current_cluster.size ()) <= max_cluster_size_))
122  {
124  pi.header = input_->header;
125  pi.indices.resize (current_cluster.size ());
126  for (int ii = 0; ii < static_cast<int> (current_cluster.size ()); ++ii) // ii = indices iterator
127  pi.indices[ii] = current_cluster[ii];
128 
129  if (extract_removed_clusters_ && static_cast<int> (current_cluster.size ()) < min_cluster_size_)
130  small_clusters_->push_back (pi);
131  else if (extract_removed_clusters_ && static_cast<int> (current_cluster.size ()) > max_cluster_size_)
132  large_clusters_->push_back (pi);
133  else
134  clusters.push_back (pi);
135  }
136  }
137 
138  deinitCompute ();
139 }
140 
141 #define PCL_INSTANTIATE_ConditionalEuclideanClustering(T) template class PCL_EXPORTS pcl::ConditionalEuclideanClustering<T>;
142 
143 #endif // PCL_SEGMENTATION_IMPL_CONDITIONAL_EUCLIDEAN_CLUSTERING_HPP_
144 
void segment(IndicesClusters &clusters)
Segment the input into separate clusters.
std::vector< int > indices
Definition: PointIndices.h:19
::pcl::PCLHeader header
Definition: PointIndices.h:17
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
Definition: organized.h:62
std::vector< pcl::PointIndices > IndicesClusters