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 ())
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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.