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