Point Cloud Library (PCL)  1.11.1-dev
seeded_hue_segmentation.hpp
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 #ifndef PCL_SEGMENTATION_IMPL_SEEDED_HUE_SEGMENTATION_H_
40 #define PCL_SEGMENTATION_IMPL_SEEDED_HUE_SEGMENTATION_H_
41 
42 #include <pcl/segmentation/seeded_hue_segmentation.h>
43 #include <pcl/console/print.h> // for PCL_ERROR
44 #include <pcl/search/organized.h> // for OrganizedNeighbor
45 #include <pcl/search/kdtree.h> // for KdTree
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////
48 void
51  float tolerance,
52  PointIndices &indices_in,
53  PointIndices &indices_out,
54  float delta_hue)
55 {
56  if (tree->getInputCloud ()->size () != cloud.size ())
57  {
58  PCL_ERROR("[pcl::seededHueSegmentation] Tree built for a different point cloud "
59  "dataset (%zu) than the input cloud (%zu)!\n",
60  static_cast<std::size_t>(tree->getInputCloud()->size()),
61  static_cast<std::size_t>(cloud.size()));
62  return;
63  }
64  // Create a bool vector of processed point indices, and initialize it to false
65  std::vector<bool> processed (cloud.size (), false);
66 
67  Indices nn_indices;
68  std::vector<float> nn_distances;
69 
70  // Process all points in the indices vector
71  for (const auto &i : indices_in.indices)
72  {
73  if (processed[i])
74  continue;
75 
76  processed[i] = true;
77 
78  Indices seed_queue;
79  int sq_idx = 0;
80  seed_queue.push_back (i);
81 
82  PointXYZRGB p;
83  p = cloud[i];
84  PointXYZHSV h;
85  PointXYZRGBtoXYZHSV(p, h);
86 
87  while (sq_idx < static_cast<int> (seed_queue.size ()))
88  {
89  int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
90  if(ret == -1)
91  PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1");
92  // Search for sq_idx
93  if (!ret)
94  {
95  sq_idx++;
96  continue;
97  }
98 
99  for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
100  {
101  if (processed[nn_indices[j]]) // Has this point been processed before ?
102  continue;
103 
104  PointXYZRGB p_l;
105  p_l = cloud[nn_indices[j]];
106  PointXYZHSV h_l;
107  PointXYZRGBtoXYZHSV(p_l, h_l);
108 
109  if (std::fabs(h_l.h - h.h) < delta_hue)
110  {
111  seed_queue.push_back (nn_indices[j]);
112  processed[nn_indices[j]] = true;
113  }
114  }
115 
116  sq_idx++;
117  }
118  // Copy the seed queue into the output indices
119  for (const auto &l : seed_queue)
120  indices_out.indices.push_back(l);
121  }
122  // This is purely esthetical, can be removed for speed purposes
123  std::sort (indices_out.indices.begin (), indices_out.indices.end ());
124 }
125 //////////////////////////////////////////////////////////////////////////////////////////////
126 void
129  float tolerance,
130  PointIndices &indices_in,
131  PointIndices &indices_out,
132  float delta_hue)
133 {
134  if (tree->getInputCloud ()->size () != cloud.size ())
135  {
136  PCL_ERROR("[pcl::seededHueSegmentation] Tree built for a different point cloud "
137  "dataset (%zu) than the input cloud (%zu)!\n",
138  static_cast<std::size_t>(tree->getInputCloud()->size()),
139  static_cast<std::size_t>(cloud.size()));
140  return;
141  }
142  // Create a bool vector of processed point indices, and initialize it to false
143  std::vector<bool> processed (cloud.size (), false);
144 
145  Indices nn_indices;
146  std::vector<float> nn_distances;
147 
148  // Process all points in the indices vector
149  for (const auto &i : indices_in.indices)
150  {
151  if (processed[i])
152  continue;
153 
154  processed[i] = true;
155 
156  Indices seed_queue;
157  int sq_idx = 0;
158  seed_queue.push_back (i);
159 
160  PointXYZRGB p;
161  p = cloud[i];
162  PointXYZHSV h;
163  PointXYZRGBtoXYZHSV(p, h);
164 
165  while (sq_idx < static_cast<int> (seed_queue.size ()))
166  {
167  int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
168  if(ret == -1)
169  PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1");
170  // Search for sq_idx
171  if (!ret)
172  {
173  sq_idx++;
174  continue;
175  }
176  for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
177  {
178  if (processed[nn_indices[j]]) // Has this point been processed before ?
179  continue;
180 
181  PointXYZRGB p_l;
182  p_l = cloud[nn_indices[j]];
183  PointXYZHSV h_l;
184  PointXYZRGBtoXYZHSV(p_l, h_l);
185 
186  if (std::fabs(h_l.h - h.h) < delta_hue)
187  {
188  seed_queue.push_back (nn_indices[j]);
189  processed[nn_indices[j]] = true;
190  }
191  }
192 
193  sq_idx++;
194  }
195  // Copy the seed queue into the output indices
196  for (const auto &l : seed_queue)
197  indices_out.indices.push_back(l);
198  }
199  // This is purely esthetical, can be removed for speed purposes
200  std::sort (indices_out.indices.begin (), indices_out.indices.end ());
201 }
202 //////////////////////////////////////////////////////////////////////////////////////////////
203 //////////////////////////////////////////////////////////////////////////////////////////////
204 
205 void
207 {
208  if (!initCompute () ||
209  (input_ && input_->points.empty ()) ||
210  (indices_ && indices_->empty ()))
211  {
212  indices_out.indices.clear ();
213  return;
214  }
215 
216  // Initialize the spatial locator
217  if (!tree_)
218  {
219  if (input_->isOrganized ())
221  else
222  tree_.reset (new pcl::search::KdTree<PointXYZRGB> (false));
223  }
224 
225  // Send the input dataset to the spatial locator
226  tree_->setInputCloud (input_);
227  seededHueSegmentation (*input_, tree_, static_cast<float> (cluster_tolerance_), indices_in, indices_out, delta_hue_);
228  deinitCompute ();
229 }
230 
231 #endif // PCL_EXTRACT_CLUSTERS_IMPL_H_
pcl::PCLBase< PointXYZRGB >::input_
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:147
pcl::search::Search::getInputCloud
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: search.h:125
pcl::PointIndices::indices
Indices indices
Definition: PointIndices.h:21
pcl::search::Search::radiusSearch
virtual int radiusSearch(const PointT &point, 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.
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: distances.h:55
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::PointXYZRGBtoXYZHSV
void PointXYZRGBtoXYZHSV(const PointXYZRGB &in, PointXYZHSV &out)
Convert a XYZRGB point type to a XYZHSV.
Definition: point_types_conversion.h:105
pcl::PointXYZHSV
Definition: point_types.hpp:710
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:61
pcl::_PointXYZHSV::h
float h
Definition: point_types.hpp:700
pcl::search::Search::Ptr
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
pcl::PointIndices
Definition: PointIndices.h:11
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:131
pcl::search::OrganizedNeighbor
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
Definition: organized.h:62
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:436
pcl::seededHueSegmentation
void seededHueSegmentation(const PointCloud< PointXYZRGB > &cloud, const search::Search< PointXYZRGB >::Ptr &tree, float tolerance, PointIndices &indices_in, PointIndices &indices_out, float delta_hue=0.0)
Decompose a region of space into clusters based on the Euclidean distance between points.
Definition: seeded_hue_segmentation.hpp:49
pcl::PCLBase< PointXYZRGB >::deinitCompute
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition: pcl_base.hpp:171
pcl::PCLBase< PointXYZRGB >::indices_
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: pcl_base.h:150
pcl::SeededHueSegmentation::segment
void segment(PointIndices &indices_in, PointIndices &indices_out)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
Definition: seeded_hue_segmentation.hpp:206
pcl::PCLBase< PointXYZRGB >::initCompute
bool initCompute()
This method should get called before starting the actual computation.
Definition: pcl_base.hpp:138
pcl::SeededHueSegmentation::cluster_tolerance_
double cluster_tolerance_
The spatial cluster tolerance as a measure in the L2 Euclidean space.
Definition: seeded_hue_segmentation.h:161
pcl::SeededHueSegmentation::tree_
KdTreePtr tree_
A pointer to the spatial search object.
Definition: seeded_hue_segmentation.h:158
pcl::SeededHueSegmentation::delta_hue_
float delta_hue_
The allowed difference on the hue.
Definition: seeded_hue_segmentation.h:164