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 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 " 108 "cloud dataset (%zu) than the input cloud (%zu)!\n",
110 static_cast<std::size_t>(cloud.
size()));
113 if (cloud.
size () != normals.
size ())
115 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point " 116 "cloud (%zu) different than normals (%zu)!\n",
117 static_cast<std::size_t>(cloud.
size()),
118 static_cast<std::size_t>(normals.
size()));
123 std::vector<bool> processed (cloud.
size (),
false);
125 std::vector<int> nn_indices;
126 std::vector<float> nn_distances;
128 for (std::size_t i = 0; i < cloud.
size (); ++i)
133 std::vector<unsigned int> seed_queue;
135 seed_queue.push_back (static_cast<int> (i));
139 while (sq_idx < static_cast<int> (seed_queue.size ()))
142 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
148 for (std::size_t j = 1; j < nn_indices.size (); ++j)
150 if (processed[nn_indices[j]])
155 double dot_p = normals[i].normal[0] * normals[nn_indices[j]].normal[0] +
156 normals[i].normal[1] * normals[nn_indices[j]].normal[1] +
157 normals[i].normal[2] * normals[nn_indices[j]].normal[2];
158 if ( std::acos (std::abs (dot_p)) < eps_angle )
160 processed[nn_indices[j]] =
true;
169 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
172 r.
indices.resize (seed_queue.size ());
173 for (std::size_t j = 0; j < seed_queue.size (); ++j)
181 clusters.push_back (r);
202 template <
typename Po
intT,
typename Normal>
206 float tolerance, std::vector<PointIndices> &clusters,
double eps_angle,
207 unsigned int min_pts_per_cluster = 1,
208 unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
213 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point " 214 "cloud dataset (%zu) than the input cloud (%zu)!\n",
216 static_cast<std::size_t>(cloud.
size()));
219 if (tree->
getIndices()->size() != indices.size()) {
220 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different set of " 221 "indices (%zu) than the input set (%zu)!\n",
222 static_cast<std::size_t>(tree->
getIndices()->size()),
226 if (cloud.
size() != normals.
size()) {
227 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point " 228 "cloud (%zu) different than normals (%zu)!\n",
229 static_cast<std::size_t>(cloud.
size()),
230 static_cast<std::size_t>(normals.
size()));
234 std::vector<bool> processed (cloud.
size (),
false);
236 std::vector<int> nn_indices;
237 std::vector<float> nn_distances;
239 for (std::size_t i = 0; i < indices.size (); ++i)
241 if (processed[indices[i]])
244 std::vector<int> seed_queue;
246 seed_queue.push_back (indices[i]);
248 processed[indices[i]] =
true;
250 while (sq_idx < static_cast<int> (seed_queue.size ()))
253 if (!tree->
radiusSearch (cloud[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances))
259 for (std::size_t j = 1; j < nn_indices.size (); ++j)
261 if (processed[nn_indices[j]])
267 normals[indices[i]].normal[0] * normals[indices[nn_indices[j]]].normal[0] +
268 normals[indices[i]].normal[1] * normals[indices[nn_indices[j]]].normal[1] +
269 normals[indices[i]].normal[2] * normals[indices[nn_indices[j]]].normal[2];
270 if ( std::acos (std::abs (dot_p)) < eps_angle )
272 processed[nn_indices[j]] =
true;
281 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
284 r.
indices.resize (seed_queue.size ());
285 for (std::size_t j = 0; j < seed_queue.size (); ++j)
293 clusters.push_back (r);
305 template <
typename Po
intT>
399 extract (std::vector<PointIndices> &clusters);
421 virtual std::string
getClassName ()
const {
return (
"EuclideanClusterExtraction"); }
435 #ifdef PCL_NO_PRECOMPILE 436 #include <pcl/segmentation/impl/extract_clusters.hpp> 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 comparePointClusters(const pcl::PointIndices &a, const pcl::PointIndices &b)
Sort clusters method (for std::sort).
shared_ptr< PointCloud< PointT > > Ptr
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...
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
bool initCompute()
This method should get called before starting the actual computation.
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.
IndicesConstPtr getIndices() const
Get a pointer to the vector of indices used.
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