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 ())
PointCloudConstPtr input_
The input point cloud dataset.
IndicesPtr indices_
A pointer to the vector of point indices to use.
bool initCompute()
This method should get called before starting the actual computation.
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.
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...
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
shared_ptr< pcl::search::Search< PointT > > Ptr
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
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.
A point structure representing Euclidean xyz coordinates, and the RGB color.