42 #include <pcl/pcl_base.h>
44 #include <pcl/search/pcl_search.h>
59 template <
typename Po
intT>
void
62 float tolerance, std::vector<PointIndices> &clusters,
63 unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
77 template <
typename Po
intT>
void
80 const typename search::Search<PointT>::Ptr &tree,
float tolerance, std::vector<PointIndices> &clusters,
81 unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
97 template <
typename Po
intT,
typename Normal>
void
101 std::vector<PointIndices> &clusters,
double eps_angle,
102 unsigned int min_pts_per_cluster = 1,
103 unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
107 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 ());
112 PCL_ERROR (
"[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.
points.size (), normals.
points.size ());
117 std::vector<bool> processed (cloud.
points.size (),
false);
119 std::vector<int> nn_indices;
120 std::vector<float> nn_distances;
122 for (std::size_t i = 0; i < cloud.
points.size (); ++i)
127 std::vector<unsigned int> seed_queue;
129 seed_queue.push_back (static_cast<int> (i));
133 while (sq_idx < static_cast<int> (seed_queue.size ()))
136 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
142 for (std::size_t j = 1; j < nn_indices.size (); ++j)
144 if (processed[nn_indices[j]])
149 double dot_p = normals.
points[i].normal[0] * normals.
points[nn_indices[j]].normal[0] +
150 normals.
points[i].normal[1] * normals.
points[nn_indices[j]].normal[1] +
151 normals.
points[i].normal[2] * normals.
points[nn_indices[j]].normal[2];
152 if ( std::abs (std::acos (dot_p)) < eps_angle )
154 processed[nn_indices[j]] =
true;
155 seed_queue.push_back (nn_indices[j]);
163 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
166 r.
indices.resize (seed_queue.size ());
167 for (std::size_t j = 0; j < seed_queue.size (); ++j)
175 clusters.push_back (r);
196 template <
typename Po
intT,
typename Normal>
200 float tolerance, std::vector<PointIndices> &clusters,
double eps_angle,
201 unsigned int min_pts_per_cluster = 1,
202 unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
208 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 ());
211 if (tree->
getIndices ()->size () != indices.size ())
213 PCL_ERROR (
"[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->
getIndices ()->size (), indices.size ());
218 PCL_ERROR (
"[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.
points.size (), normals.
points.size ());
222 std::vector<bool> processed (cloud.
points.size (),
false);
224 std::vector<int> nn_indices;
225 std::vector<float> nn_distances;
227 for (std::size_t i = 0; i < indices.size (); ++i)
229 if (processed[indices[i]])
232 std::vector<int> seed_queue;
234 seed_queue.push_back (indices[i]);
236 processed[indices[i]] =
true;
238 while (sq_idx < static_cast<int> (seed_queue.size ()))
241 if (!tree->
radiusSearch (cloud.
points[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances))
247 for (std::size_t j = 1; j < nn_indices.size (); ++j)
249 if (processed[nn_indices[j]])
255 normals.
points[indices[i]].normal[0] * normals.
points[indices[nn_indices[j]]].normal[0] +
256 normals.
points[indices[i]].normal[1] * normals.
points[indices[nn_indices[j]]].normal[1] +
257 normals.
points[indices[i]].normal[2] * normals.
points[indices[nn_indices[j]]].normal[2];
258 if ( std::abs (std::acos (dot_p)) < eps_angle )
260 processed[nn_indices[j]] =
true;
261 seed_queue.push_back (nn_indices[j]);
269 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
272 r.
indices.resize (seed_queue.size ());
273 for (std::size_t j = 0; j < seed_queue.size (); ++j)
281 clusters.push_back (r);
293 template <
typename Po
intT>
387 extract (std::vector<PointIndices> &clusters);
409 virtual std::string
getClassName ()
const {
return (
"EuclideanClusterExtraction"); }
423 #ifdef PCL_NO_PRECOMPILE
424 #include <pcl/segmentation/impl/extract_clusters.hpp>
bool comparePointClusters(const pcl::PointIndices &a, const pcl::PointIndices &b)
Sort clusters method (for std::sort).
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
PointIndices::Ptr PointIndicesPtr
IndicesPtr indices_
A pointer to the vector of point indices to use.
void extractEuclideanClusters(const PointCloud< PointT > &cloud, const typename search::Search< PointT >::Ptr &tree, float tolerance, std::vector< PointIndices > &clusters, unsigned int min_pts_per_cluster=1, unsigned int max_pts_per_cluster=(std::numeric_limits< int >::max)())
Decompose a region of space into clusters based on the Euclidean distance between points...
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
virtual int radiusSearch(const PointT &p_q, double radius, std::vector< int > &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.
bool initCompute()
This method should get called before starting the actual computation.
IndicesConstPtr getIndices() const
Get a pointer to the vector of indices used.
shared_ptr< ::pcl::PointIndices > Ptr
typename PointCloud::Ptr PointCloudPtr
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. ...
pcl::PCLHeader header
The point cloud header.
shared_ptr< const ::pcl::PointIndices > ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
PointCloudConstPtr input_
The input point cloud dataset.
shared_ptr< pcl::search::Search< PointT > > Ptr
KdTree represents the base spatial locator class for kd-tree implementations.
PointIndices::ConstPtr PointIndicesConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr