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>
43 #include <pcl/console/print.h>
44 #include <pcl/search/organized.h>
45 #include <pcl/search/kdtree.h>
58 PCL_ERROR(
"[pcl::seededHueSegmentation] Tree built for a different point cloud "
59 "dataset (%zu) than the input cloud (%zu)!\n",
61 static_cast<std::size_t
>(cloud.
size()));
65 std::vector<bool> processed (cloud.
size (),
false);
68 std::vector<float> nn_distances;
71 for (
const auto &i : indices_in.
indices)
80 seed_queue.push_back (i);
87 while (sq_idx <
static_cast<int> (seed_queue.size ()))
89 int ret = tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
91 PCL_ERROR(
"[pcl::seededHueSegmentation] radiusSearch returned error code -1\n");
99 for (std::size_t j = 1; j < nn_indices.size (); ++j)
101 if (processed[nn_indices[j]])
105 p_l = cloud[nn_indices[j]];
109 if (std::fabs(h_l.
h - h.
h) < delta_hue)
111 seed_queue.push_back (nn_indices[j]);
112 processed[nn_indices[j]] =
true;
119 for (
const auto &l : seed_queue)
120 indices_out.
indices.push_back(l);
123 std::sort (indices_out.
indices.begin (), indices_out.
indices.end ());
136 PCL_ERROR(
"[pcl::seededHueSegmentation] Tree built for a different point cloud "
137 "dataset (%zu) than the input cloud (%zu)!\n",
139 static_cast<std::size_t
>(cloud.
size()));
143 std::vector<bool> processed (cloud.
size (),
false);
146 std::vector<float> nn_distances;
149 for (
const auto &i : indices_in.
indices)
158 seed_queue.push_back (i);
165 while (sq_idx <
static_cast<int> (seed_queue.size ()))
167 int ret = tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
169 PCL_ERROR(
"[pcl::seededHueSegmentation] radiusSearch returned error code -1\n");
176 for (std::size_t j = 1; j < nn_indices.size (); ++j)
178 if (processed[nn_indices[j]])
182 p_l = cloud[nn_indices[j]];
186 if (std::fabs(h_l.
h - h.
h) < delta_hue)
188 seed_queue.push_back (nn_indices[j]);
189 processed[nn_indices[j]] =
true;
196 for (
const auto &l : seed_queue)
197 indices_out.
indices.push_back(l);
200 std::sort (indices_out.
indices.begin (), indices_out.
indices.end ());
219 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 neighbor search in organized projectable point clo...
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.
IndicesAllocator<> Indices
Type used for indices in PCL.
A point structure representing Euclidean xyz coordinates, and the RGB color.