Point Cloud Library (PCL)  1.8.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
47  const boost::shared_ptr<search::Search<PointXYZRGB> > &tree,
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
123  const boost::shared_ptr<search::Search<PointXYZRGBL> > &tree,
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:62
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
void seededHueSegmentation(const PointCloud< PointXYZRGB > &cloud, const boost::shared_ptr< search::Search< PointXYZRGB > > &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...
bool initCompute()
This method should get called before starting the actual computation.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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.
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: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.