38 #ifndef PCL_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
39 #define PCL_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
41 #include <pcl/segmentation/extract_clusters.h>
44 template <
typename Po
intT>
void
47 float tolerance, std::vector<PointIndices> &clusters,
48 unsigned int min_pts_per_cluster,
49 unsigned int max_pts_per_cluster)
53 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 ());
59 std::vector<bool> processed (cloud.
points.size (),
false);
61 std::vector<int> nn_indices;
62 std::vector<float> nn_distances;
64 for (
int i = 0; i < static_cast<int> (cloud.
points.size ()); ++i)
69 std::vector<int> seed_queue;
71 seed_queue.push_back (i);
75 while (sq_idx <
static_cast<int> (seed_queue.size ()))
78 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
84 for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
86 if (nn_indices[j] == -1 || processed[nn_indices[j]])
90 seed_queue.push_back (nn_indices[j]);
91 processed[nn_indices[j]] =
true;
98 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
101 r.
indices.resize (seed_queue.size ());
102 for (std::size_t j = 0; j < seed_queue.size (); ++j)
110 clusters.push_back (r);
117 template <
typename Po
intT>
void
119 const std::vector<int> &indices,
121 float tolerance, std::vector<PointIndices> &clusters,
122 unsigned int min_pts_per_cluster,
123 unsigned int max_pts_per_cluster)
129 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 ());
132 if (tree->
getIndices ()->size () != indices.size ())
134 PCL_ERROR (
"[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->
getIndices ()->size (), indices.size ());
141 std::vector<bool> processed (cloud.
points.size (),
false);
143 std::vector<int> nn_indices;
144 std::vector<float> nn_distances;
146 for (
const int &index : indices)
148 if (processed[index])
151 std::vector<int> seed_queue;
153 seed_queue.push_back (index);
155 processed[index] =
true;
157 while (sq_idx <
static_cast<int> (seed_queue.size ()))
160 int ret = tree->
radiusSearch (cloud.
points[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances);
163 PCL_ERROR(
"[pcl::extractEuclideanClusters] Received error code -1 from radiusSearch\n");
172 for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
174 if (nn_indices[j] == -1 || processed[nn_indices[j]])
178 seed_queue.push_back (nn_indices[j]);
179 processed[nn_indices[j]] =
true;
186 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
189 r.
indices.resize (seed_queue.size ());
190 for (std::size_t j = 0; j < seed_queue.size (); ++j)
200 clusters.push_back (r);
209 template <
typename Po
intT>
void
212 if (!initCompute () ||
213 (input_ && input_->points.empty ()) ||
214 (indices_ && indices_->empty ()))
223 if (input_->isOrganized ())
230 tree_->setInputCloud (input_, indices_);
231 extractEuclideanClusters (*input_, *indices_, tree_,
static_cast<float> (cluster_tolerance_), clusters, min_pts_per_cluster_, max_pts_per_cluster_);
242 #define PCL_INSTANTIATE_EuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::EuclideanClusterExtraction<T>;
243 #define PCL_INSTANTIATE_extractEuclideanClusters(T) template void PCL_EXPORTS pcl::extractEuclideanClusters<T>(const pcl::PointCloud<T> &, const typename pcl::search::Search<T>::Ptr &, float , std::vector<pcl::PointIndices> &, unsigned int, unsigned int);
244 #define PCL_INSTANTIATE_extractEuclideanClusters_indices(T) template void PCL_EXPORTS pcl::extractEuclideanClusters<T>(const pcl::PointCloud<T> &, const std::vector<int> &, const typename pcl::search::Search<T>::Ptr &, float , std::vector<pcl::PointIndices> &, unsigned int, unsigned int);
PointCloud represents the base class in PCL for storing collections of 3D points.
pcl::PCLHeader header
The point cloud header.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds.
virtual bool getSortedResults()
Gets whether the results should be sorted (ascending in the distance) or not Otherwise the results ma...
virtual IndicesConstPtr getIndices() const
Get a pointer to the vector of indices used.
shared_ptr< pcl::search::Search< PointT > > Ptr
virtual PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
virtual int radiusSearch(const PointT &point, double radius, Indices &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.
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.
bool comparePointClusters(const pcl::PointIndices &a, const pcl::PointIndices &b)
Sort clusters method (for std::sort).