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 (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.
points.size ());
113 PCL_ERROR (
"[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\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 (%zu) than the input cloud (%zu)!\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 (%zu) than the input set (%zu)!\n", tree->getIndices ()->size (), indices.size ());
219 PCL_ERROR (
"[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\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_
bool comparePointClusters(const pcl::PointIndices &a, const pcl::PointIndices &b)
Sort clusters method (for std::sort).
PointCloud::ConstPtr PointCloudConstPtr
void setMaxClusterSize(int max_cluster_size)
Set the maximum number of points that a cluster needs to contain in order to be considered valid...
void setMinClusterSize(int min_cluster_size)
Set the minimum number of points that a cluster needs to contain in order to be considered valid...
EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sen...
PointCloud::Ptr PointCloudPtr
void extract(std::vector< PointIndices > &clusters)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
KdTreePtr getSearchMethod() const
Get a pointer to the search method used.
PointIndices::Ptr PointIndicesPtr
pcl::PCLHeader header
The point cloud header.
boost::shared_ptr< ::pcl::PointIndices const > ConstPtr
void setClusterTolerance(double tolerance)
Set the spatial cluster tolerance as a measure in the L2 Euclidean space.
pcl::search::Search< PointT > KdTree
int getMinClusterSize() const
Get the minimum number of points that a cluster needs to contain in order to be considered valid...
virtual std::string getClassName() const
Class getName method.
boost::shared_ptr< ::pcl::PointIndices > Ptr
double getClusterTolerance() const
Get the spatial cluster tolerance as a measure in the L2 Euclidean space.
EuclideanClusterExtraction()
Empty constructor.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
pcl::search::Search< PointT >::Ptr KdTreePtr
bool initCompute()
This method should get called before starting the actual computation.
boost::shared_ptr< PointCloud< PointT > > Ptr
KdTree represents the base spatial locator class for kd-tree implementations.
IndicesPtr indices_
A pointer to the vector of point indices to use.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
int getMaxClusterSize() const
Get the maximum number of points that a cluster needs to contain in order to be considered valid...
void extractEuclideanClusters(const PointCloud< PointT > &cloud, const boost::shared_ptr< search::Search< PointT > > &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...
KdTreePtr tree_
A pointer to the spatial search object.
pcl::PointCloud< PointT > PointCloud
int max_pts_per_cluster_
The maximum number of points that a cluster needs to contain in order to be considered valid (default...
double cluster_tolerance_
The spatial cluster tolerance as a measure in the L2 Euclidean space.
std::vector< int > indices
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
PointIndices::ConstPtr PointIndicesConstPtr
PointCloudConstPtr input_
The input point cloud dataset.
bool deinitCompute()
This method should get called after finishing the actual computation.
int min_pts_per_cluster_
The minimum number of points that a cluster needs to contain in order to be considered valid (default...