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 (size_t k = 0; k < indices_in.indices.size (); ++k)
66  {
67  int i = indices_in.indices[k];
68  if (processed[i])
69  continue;
70 
71  processed[i] = true;
72 
73  std::vector<int> seed_queue;
74  int sq_idx = 0;
75  seed_queue.push_back (i);
76 
77  PointXYZRGB p;
78  p = cloud.points[i];
79  PointXYZHSV h;
80  PointXYZRGBtoXYZHSV(p, h);
81 
82  while (sq_idx < static_cast<int> (seed_queue.size ()))
83  {
84  int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
85  if(ret == -1)
86  PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1");
87  // Search for sq_idx
88  if (!ret)
89  {
90  sq_idx++;
91  continue;
92  }
93 
94  for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
95  {
96  if (processed[nn_indices[j]]) // Has this point been processed before ?
97  continue;
98 
99  PointXYZRGB p_l;
100  p_l = cloud.points[nn_indices[j]];
101  PointXYZHSV h_l;
102  PointXYZRGBtoXYZHSV(p_l, h_l);
103 
104  if (fabs(h_l.h - h.h) < delta_hue)
105  {
106  seed_queue.push_back (nn_indices[j]);
107  processed[nn_indices[j]] = true;
108  }
109  }
110 
111  sq_idx++;
112  }
113  // Copy the seed queue into the output indices
114  for (size_t l = 0; l < seed_queue.size (); ++l)
115  indices_out.indices.push_back(seed_queue[l]);
116  }
117  // This is purely esthetical, can be removed for speed purposes
118  std::sort (indices_out.indices.begin (), indices_out.indices.end ());
119 }
120 //////////////////////////////////////////////////////////////////////////////////////////////
121 void
124  float tolerance,
125  PointIndices &indices_in,
126  PointIndices &indices_out,
127  float delta_hue)
128 {
129  if (tree->getInputCloud ()->points.size () != cloud.points.size ())
130  {
131  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 ());
132  return;
133  }
134  // Create a bool vector of processed point indices, and initialize it to false
135  std::vector<bool> processed (cloud.points.size (), false);
136 
137  std::vector<int> nn_indices;
138  std::vector<float> nn_distances;
139 
140  // Process all points in the indices vector
141  for (size_t k = 0; k < indices_in.indices.size (); ++k)
142  {
143  int i = indices_in.indices[k];
144  if (processed[i])
145  continue;
146 
147  processed[i] = true;
148 
149  std::vector<int> seed_queue;
150  int sq_idx = 0;
151  seed_queue.push_back (i);
152 
153  PointXYZRGB p;
154  p = cloud.points[i];
155  PointXYZHSV h;
156  PointXYZRGBtoXYZHSV(p, h);
157 
158  while (sq_idx < static_cast<int> (seed_queue.size ()))
159  {
160  int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
161  if(ret == -1)
162  PCL_ERROR("[pcl::seededHueSegmentation] radiusSearch returned error code -1");
163  // Search for sq_idx
164  if (!ret)
165  {
166  sq_idx++;
167  continue;
168  }
169  for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
170  {
171  if (processed[nn_indices[j]]) // Has this point been processed before ?
172  continue;
173 
174  PointXYZRGB p_l;
175  p_l = cloud.points[nn_indices[j]];
176  PointXYZHSV h_l;
177  PointXYZRGBtoXYZHSV(p_l, h_l);
178 
179  if (fabs(h_l.h - h.h) < delta_hue)
180  {
181  seed_queue.push_back (nn_indices[j]);
182  processed[nn_indices[j]] = true;
183  }
184  }
185 
186  sq_idx++;
187  }
188  // Copy the seed queue into the output indices
189  for (size_t l = 0; l < seed_queue.size (); ++l)
190  indices_out.indices.push_back(seed_queue[l]);
191  }
192  // This is purely esthetical, can be removed for speed purposes
193  std::sort (indices_out.indices.begin (), indices_out.indices.end ());
194 }
195 //////////////////////////////////////////////////////////////////////////////////////////////
196 //////////////////////////////////////////////////////////////////////////////////////////////
197 
198 void
200 {
201  if (!initCompute () ||
202  (input_ != 0 && input_->points.empty ()) ||
203  (indices_ != 0 && indices_->empty ()))
204  {
205  indices_out.indices.clear ();
206  return;
207  }
208 
209  // Initialize the spatial locator
210  if (!tree_)
211  {
212  if (input_->isOrganized ())
214  else
215  tree_.reset (new pcl::search::KdTree<PointXYZRGB> (false));
216  }
217 
218  // Send the input dataset to the spatial locator
219  tree_->setInputCloud (input_);
220  seededHueSegmentation (*input_, tree_, static_cast<float> (cluster_tolerance_), indices_in, indices_out, delta_hue_);
221  deinitCompute ();
222 }
223 
224 #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:409
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:153
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.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:80
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:61
PointCloudConstPtr input_
The input point cloud dataset.
Definition: pcl_base.h:150
A point structure representing Euclidean xyz coordinates, and the RGB color.
double cluster_tolerance_
The spatial cluster tolerance as a measure in the L2 Euclidean space.