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