40 #ifndef PCL_EXTRACT_CLUSTERS_H_
41 #define PCL_EXTRACT_CLUSTERS_H_
43 #include <pcl/pcl_base.h>
45 #include <pcl/search/pcl_search.h>
60 template <
typename Po
intT>
void
62 const PointCloud<PointT> &cloud,
const boost::shared_ptr<search::Search<PointT> > &tree,
63 float tolerance, std::vector<PointIndices> &clusters,
64 unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
78 template <
typename Po
intT>
void
81 const boost::shared_ptr<search::Search<PointT> > &tree,
float tolerance, std::vector<PointIndices> &clusters,
82 unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
98 template <
typename Po
intT,
typename Normal>
void
102 std::vector<PointIndices> &clusters,
double eps_angle,
103 unsigned int min_pts_per_cluster = 1,
104 unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
106 if (tree->getInputCloud ()->points.size () != cloud.
points.size ())
108 PCL_ERROR (
"[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.
points.size ());
113 PCL_ERROR (
"[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.
points.size (), normals.
points.size ());
118 std::vector<bool> processed (cloud.
points.size (),
false);
120 std::vector<int> nn_indices;
121 std::vector<float> nn_distances;
123 for (
size_t i = 0; i < cloud.
points.size (); ++i)
128 std::vector<unsigned int> seed_queue;
130 seed_queue.push_back (
static_cast<int> (i));
134 while (sq_idx <
static_cast<int> (seed_queue.size ()))
137 if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
143 for (
size_t j = 1; j < nn_indices.size (); ++j)
145 if (processed[nn_indices[j]])
150 double dot_p = normals.
points[i].normal[0] * normals.
points[nn_indices[j]].normal[0] +
151 normals.
points[i].normal[1] * normals.
points[nn_indices[j]].normal[1] +
152 normals.
points[i].normal[2] * normals.
points[nn_indices[j]].normal[2];
153 if ( fabs (acos (dot_p)) < eps_angle )
155 processed[nn_indices[j]] =
true;
156 seed_queue.push_back (nn_indices[j]);
164 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
167 r.
indices.resize (seed_queue.size ());
168 for (
size_t j = 0; j < seed_queue.size (); ++j)
176 clusters.push_back (r);
197 template <
typename Po
intT,
typename Normal>
200 const std::vector<int> &indices,
const boost::shared_ptr<
KdTree<PointT> > &tree,
201 float tolerance, std::vector<PointIndices> &clusters,
double eps_angle,
202 unsigned int min_pts_per_cluster = 1,
203 unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
207 if (tree->getInputCloud ()->points.size () != cloud.
points.size ())
209 PCL_ERROR (
"[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.
points.size ());
212 if (tree->getIndices ()->size () != indices.size ())
214 PCL_ERROR (
"[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ());
219 PCL_ERROR (
"[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.
points.size (), normals.
points.size ());
223 std::vector<bool> processed (cloud.
points.size (),
false);
225 std::vector<int> nn_indices;
226 std::vector<float> nn_distances;
228 for (
size_t i = 0; i < indices.size (); ++i)
230 if (processed[indices[i]])
233 std::vector<int> seed_queue;
235 seed_queue.push_back (indices[i]);
237 processed[indices[i]] =
true;
239 while (sq_idx <
static_cast<int> (seed_queue.size ()))
242 if (!tree->radiusSearch (cloud.
points[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances))
248 for (
size_t j = 1; j < nn_indices.size (); ++j)
250 if (processed[nn_indices[j]])
256 normals.
points[indices[i]].normal[0] * normals.
points[indices[nn_indices[j]]].normal[0] +
257 normals.
points[indices[i]].normal[1] * normals.
points[indices[nn_indices[j]]].normal[1] +
258 normals.
points[indices[i]].normal[2] * normals.
points[indices[nn_indices[j]]].normal[2];
259 if ( fabs (acos (dot_p)) < eps_angle )
261 processed[nn_indices[j]] =
true;
262 seed_queue.push_back (nn_indices[j]);
270 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
273 r.
indices.resize (seed_queue.size ());
274 for (
size_t j = 0; j < seed_queue.size (); ++j)
282 clusters.push_back (r);
294 template <
typename Po
intT>
388 extract (std::vector<PointIndices> &clusters);
410 virtual std::string
getClassName ()
const {
return (
"EuclideanClusterExtraction"); }
424 #ifdef PCL_NO_PRECOMPILE
425 #include <pcl/segmentation/impl/extract_clusters.hpp>
428 #endif //#ifndef PCL_EXTRACT_CLUSTERS_H_