Point Cloud Library (PCL)  1.11.0-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 
44 //////////////////////////////////////////////////////////////////////////////////////////////
45 void
48  float tolerance,
49  PointIndices &indices_in,
50  PointIndices &indices_out,
51  float delta_hue)
52 {
53  if (tree->getInputCloud ()->size () != cloud.size ())
54  {
55  PCL_ERROR("[pcl::seededHueSegmentation] Tree built for a different point cloud "
56  "dataset (%zu) than the input cloud (%zu)!\n",
57  static_cast<std::size_t>(tree->getInputCloud()->size()),
58  static_cast<std::size_t>(cloud.size()));
59  return;
60  }
61  // Create a bool vector of processed point indices, and initialize it to false
62  std::vector<bool> processed (cloud.size (), false);
63 
64  std::vector<int> nn_indices;
65  std::vector<float> nn_distances;
66 
67  // Process all points in the indices vector
68  for (const int &i : indices_in.indices)
69  {
70  if (processed[i])
71  continue;
72 
73  processed[i] = true;
74 
75  std::vector<int> seed_queue;
76  int sq_idx = 0;
77  seed_queue.push_back (i);
78 
79  PointXYZRGB p;
80  p = cloud[i];
81  PointXYZHSV h;
82  PointXYZRGBtoXYZHSV(p, h);
83 
84  while (sq_idx < static_cast<int> (seed_queue.size ()))
85  {
86  int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
87  if(ret == -1)
88  PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1");
89  // Search for sq_idx
90  if (!ret)
91  {
92  sq_idx++;
93  continue;
94  }
95 
96  for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
97  {
98  if (processed[nn_indices[j]]) // Has this point been processed before ?
99  continue;
100 
101  PointXYZRGB p_l;
102  p_l = cloud[nn_indices[j]];
103  PointXYZHSV h_l;
104  PointXYZRGBtoXYZHSV(p_l, h_l);
105 
106  if (std::fabs(h_l.h - h.h) < delta_hue)
107  {
108  seed_queue.push_back (nn_indices[j]);
109  processed[nn_indices[j]] = true;
110  }
111  }
112 
113  sq_idx++;
114  }
115  // Copy the seed queue into the output indices
116  for (const int &l : seed_queue)
117  indices_out.indices.push_back(l);
118  }
119  // This is purely esthetical, can be removed for speed purposes
120  std::sort (indices_out.indices.begin (), indices_out.indices.end ());
121 }
122 //////////////////////////////////////////////////////////////////////////////////////////////
123 void
126  float tolerance,
127  PointIndices &indices_in,
128  PointIndices &indices_out,
129  float delta_hue)
130 {
131  if (tree->getInputCloud ()->size () != cloud.size ())
132  {
133  PCL_ERROR("[pcl::seededHueSegmentation] Tree built for a different point cloud "
134  "dataset (%zu) than the input cloud (%zu)!\n",
135  static_cast<std::size_t>(tree->getInputCloud()->size()),
136  static_cast<std::size_t>(cloud.size()));
137  return;
138  }
139  // Create a bool vector of processed point indices, and initialize it to false
140  std::vector<bool> processed (cloud.size (), false);
141 
142  std::vector<int> nn_indices;
143  std::vector<float> nn_distances;
144 
145  // Process all points in the indices vector
146  for (const int &i : indices_in.indices)
147  {
148  if (processed[i])
149  continue;
150 
151  processed[i] = true;
152 
153  std::vector<int> seed_queue;
154  int sq_idx = 0;
155  seed_queue.push_back (i);
156 
157  PointXYZRGB p;
158  p = cloud[i];
159  PointXYZHSV h;
160  PointXYZRGBtoXYZHSV(p, h);
161 
162  while (sq_idx < static_cast<int> (seed_queue.size ()))
163  {
164  int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
165  if(ret == -1)
166  PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1");
167  // Search for sq_idx
168  if (!ret)
169  {
170  sq_idx++;
171  continue;
172  }
173  for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
174  {
175  if (processed[nn_indices[j]]) // Has this point been processed before ?
176  continue;
177 
178  PointXYZRGB p_l;
179  p_l = cloud[nn_indices[j]];
180  PointXYZHSV h_l;
181  PointXYZRGBtoXYZHSV(p_l, h_l);
182 
183  if (std::fabs(h_l.h - h.h) < delta_hue)
184  {
185  seed_queue.push_back (nn_indices[j]);
186  processed[nn_indices[j]] = true;
187  }
188  }
189 
190  sq_idx++;
191  }
192  // Copy the seed queue into the output indices
193  for (const int &l : seed_queue)
194  indices_out.indices.push_back(l);
195  }
196  // This is purely esthetical, can be removed for speed purposes
197  std::sort (indices_out.indices.begin (), indices_out.indices.end ());
198 }
199 //////////////////////////////////////////////////////////////////////////////////////////////
200 //////////////////////////////////////////////////////////////////////////////////////////////
201 
202 void
204 {
205  if (!initCompute () ||
206  (input_ && input_->points.empty ()) ||
207  (indices_ && indices_->empty ()))
208  {
209  indices_out.indices.clear ();
210  return;
211  }
212 
213  // Initialize the spatial locator
214  if (!tree_)
215  {
216  if (input_->isOrganized ())
218  else
219  tree_.reset (new pcl::search::KdTree<PointXYZRGB> (false));
220  }
221 
222  // Send the input dataset to the spatial locator
223  tree_->setInputCloud (input_);
224  seededHueSegmentation (*input_, tree_, static_cast<float> (cluster_tolerance_), indices_in, indices_out, delta_hue_);
225  deinitCompute ();
226 }
227 
228 #endif // PCL_EXTRACT_CLUSTERS_IMPL_H_
pcl::PCLBase< PointXYZRGB >::input_
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:150
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:23
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:13
pcl::search::OrganizedNeighbor
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
Definition: organized.h:63
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:459
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:46
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:153
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:203
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