39 #ifndef PCL_SEGMENTATION_IMPL_SEEDED_HUE_SEGMENTATION_H_
40 #define PCL_SEGMENTATION_IMPL_SEEDED_HUE_SEGMENTATION_H_
42 #include <pcl/segmentation/seeded_hue_segmentation.h>
55 PCL_ERROR(
"[pcl::seededHueSegmentation] Tree built for a different point cloud "
56 "dataset (%zu) than the input cloud (%zu)!\n",
58 static_cast<std::size_t
>(cloud.
size()));
62 std::vector<bool> processed (cloud.
size (),
false);
64 std::vector<int> nn_indices;
65 std::vector<float> nn_distances;
68 for (
const int &i : indices_in.
indices)
75 std::vector<int> seed_queue;
77 seed_queue.push_back (i);
84 while (sq_idx <
static_cast<int> (seed_queue.size ()))
86 int ret = tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
88 PCL_ERROR(
"[pcl::seededHueSegmentation] radiusSearch returned error code -1");
96 for (std::size_t j = 1; j < nn_indices.size (); ++j)
98 if (processed[nn_indices[j]])
102 p_l = cloud[nn_indices[j]];
106 if (std::fabs(h_l.
h - h.
h) < delta_hue)
108 seed_queue.push_back (nn_indices[j]);
109 processed[nn_indices[j]] =
true;
116 for (
const int &l : seed_queue)
117 indices_out.
indices.push_back(l);
120 std::sort (indices_out.
indices.begin (), indices_out.
indices.end ());
133 PCL_ERROR(
"[pcl::seededHueSegmentation] Tree built for a different point cloud "
134 "dataset (%zu) than the input cloud (%zu)!\n",
136 static_cast<std::size_t
>(cloud.
size()));
140 std::vector<bool> processed (cloud.
size (),
false);
142 std::vector<int> nn_indices;
143 std::vector<float> nn_distances;
146 for (
const int &i : indices_in.
indices)
153 std::vector<int> seed_queue;
155 seed_queue.push_back (i);
162 while (sq_idx <
static_cast<int> (seed_queue.size ()))
164 int ret = tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
166 PCL_ERROR(
"[pcl::seededHueSegmentation] radiusSearch returned error code -1");
173 for (std::size_t j = 1; j < nn_indices.size (); ++j)
175 if (processed[nn_indices[j]])
179 p_l = cloud[nn_indices[j]];
183 if (std::fabs(h_l.
h - h.
h) < delta_hue)
185 seed_queue.push_back (nn_indices[j]);
186 processed[nn_indices[j]] =
true;
193 for (
const int &l : seed_queue)
194 indices_out.
indices.push_back(l);
197 std::sort (indices_out.
indices.begin (), indices_out.
indices.end ());
216 if (
input_->isOrganized ())
228 #endif // PCL_EXTRACT_CLUSTERS_IMPL_H_