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 dataset (%lu) than the input cloud (%lu)!\n", tree->
getInputCloud ()->points.size (), cloud.
points.size ());
59 std::vector<bool> processed (cloud.
points.size (),
false);
61 std::vector<int> nn_indices;
62 std::vector<float> nn_distances;
65 for (
const int &i : indices_in.
indices)
72 std::vector<int> seed_queue;
74 seed_queue.push_back (i);
81 while (sq_idx < static_cast<int> (seed_queue.size ()))
83 int ret = tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
85 PCL_ERROR(
"[pcl::seededHueSegmentation] radiusSearch returned error code -1");
93 for (std::size_t j = 1; j < nn_indices.size (); ++j)
95 if (processed[nn_indices[j]])
99 p_l = cloud.
points[nn_indices[j]];
103 if (std::fabs(h_l.
h - h.
h) < delta_hue)
105 seed_queue.push_back (nn_indices[j]);
106 processed[nn_indices[j]] =
true;
113 for (
const int &l : seed_queue)
114 indices_out.
indices.push_back(l);
117 std::sort (indices_out.
indices.begin (), indices_out.
indices.end ());
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 ());
134 std::vector<bool> processed (cloud.
points.size (),
false);
136 std::vector<int> nn_indices;
137 std::vector<float> nn_distances;
140 for (
const int &i : indices_in.
indices)
147 std::vector<int> seed_queue;
149 seed_queue.push_back (i);
156 while (sq_idx < static_cast<int> (seed_queue.size ()))
158 int ret = tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
160 PCL_ERROR(
"[pcl::seededHueSegmentation] radiusSearch returned error code -1");
167 for (std::size_t j = 1; j < nn_indices.size (); ++j)
169 if (processed[nn_indices[j]])
173 p_l = cloud.
points[nn_indices[j]];
177 if (std::fabs(h_l.
h - h.
h) < delta_hue)
179 seed_queue.push_back (nn_indices[j]);
180 processed[nn_indices[j]] =
true;
187 for (
const int &l : seed_queue)
188 indices_out.
indices.push_back(l);
191 std::sort (indices_out.
indices.begin (), indices_out.
indices.end ());
210 if (
input_->isOrganized ())
222 #endif // PCL_EXTRACT_CLUSTERS_IMPL_H_