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>
53 if (tree->getInputCloud ()->points.size () != cloud.
points.size ())
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 (
size_t k = 0; k < indices_in.
indices.size (); ++k)
73 std::vector<int> seed_queue;
75 seed_queue.push_back (i);
82 while (sq_idx < static_cast<int> (seed_queue.size ()))
84 int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
86 PCL_ERROR(
"[pcl::seededHueSegmentation] radiusSearch returned error code -1");
94 for (
size_t j = 1; j < nn_indices.size (); ++j)
96 if (processed[nn_indices[j]])
100 p_l = cloud.
points[nn_indices[j]];
104 if (fabs(h_l.
h - h.
h) < delta_hue)
106 seed_queue.push_back (nn_indices[j]);
107 processed[nn_indices[j]] =
true;
114 for (
size_t l = 0; l < seed_queue.size (); ++l)
115 indices_out.
indices.push_back(seed_queue[l]);
118 std::sort (indices_out.
indices.begin (), indices_out.
indices.end ());
129 if (tree->getInputCloud ()->points.size () != cloud.
points.size ())
131 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 ());
135 std::vector<bool> processed (cloud.
points.size (),
false);
137 std::vector<int> nn_indices;
138 std::vector<float> nn_distances;
141 for (
size_t k = 0; k < indices_in.
indices.size (); ++k)
149 std::vector<int> seed_queue;
151 seed_queue.push_back (i);
158 while (sq_idx < static_cast<int> (seed_queue.size ()))
160 int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits<int>::max());
162 PCL_ERROR(
"[pcl::seededHueSegmentation] radiusSearch returned error code -1");
169 for (
size_t j = 1; j < nn_indices.size (); ++j)
171 if (processed[nn_indices[j]])
175 p_l = cloud.
points[nn_indices[j]];
179 if (fabs(h_l.
h - h.
h) < delta_hue)
181 seed_queue.push_back (nn_indices[j]);
182 processed[nn_indices[j]] =
true;
189 for (
size_t l = 0; l < seed_queue.size (); ++l)
190 indices_out.
indices.push_back(seed_queue[l]);
193 std::sort (indices_out.
indices.begin (), indices_out.
indices.end ());
212 if (
input_->isOrganized ())
224 #endif // PCL_EXTRACT_CLUSTERS_IMPL_H_
float delta_hue_
The allowed difference on the hue.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
IndicesPtr indices_
A pointer to the vector of point indices to use.
std::vector< int > indices
void seededHueSegmentation(const PointCloud< PointXYZRGB > &cloud, const boost::shared_ptr< search::Search< PointXYZRGB > > &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...
bool initCompute()
This method should get called before starting the actual computation.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void segment(PointIndices &indices_in, PointIndices &indices_out)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
KdTreePtr tree_
A pointer to the spatial search object.
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. ...
void PointXYZRGBtoXYZHSV(PointXYZRGB &in, PointXYZHSV &out)
Convert a XYZRGB point type to a XYZHSV.
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
PointCloudConstPtr input_
The input point cloud dataset.
A point structure representing Euclidean xyz coordinates, and the RGB color.
double cluster_tolerance_
The spatial cluster tolerance as a measure in the L2 Euclidean space.