38 #ifndef PCL_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
39 #define PCL_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
41 #include <pcl/segmentation/extract_clusters.h>
42 #include <pcl/search/organized.h>
45 template <
typename Po
intT>
void
48 float tolerance, std::vector<PointIndices> &clusters,
49 unsigned int min_pts_per_cluster,
50 unsigned int max_pts_per_cluster)
54 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud "
55 "dataset (%zu) than the input cloud (%zu)!\n",
57 static_cast<std::size_t
>(cloud.
size()));
63 std::vector<bool> processed (cloud.
size (),
false);
66 std::vector<float> nn_distances;
68 for (
int i = 0; i < static_cast<int> (cloud.
size ()); ++i)
75 seed_queue.push_back (i);
79 while (sq_idx <
static_cast<int> (seed_queue.size ()))
82 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
88 for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
90 if (nn_indices[j] == UNAVAILABLE || processed[nn_indices[j]])
94 seed_queue.push_back (nn_indices[j]);
95 processed[nn_indices[j]] =
true;
102 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
105 r.
indices.resize (seed_queue.size ());
106 for (std::size_t j = 0; j < seed_queue.size (); ++j)
114 clusters.push_back (r);
118 PCL_DEBUG(
"[pcl::extractEuclideanClusters] This cluster has %zu points, which is not between %u and %u points, so it is not a final cluster\n",
119 seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
125 template <
typename Po
intT>
void
129 float tolerance, std::vector<PointIndices> &clusters,
130 unsigned int min_pts_per_cluster,
131 unsigned int max_pts_per_cluster)
136 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud "
137 "dataset (%zu) than the input cloud (%zu)!\n",
139 static_cast<std::size_t
>(cloud.
size()));
142 if (tree->
getIndices()->size() != indices.size()) {
143 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different set of "
144 "indices (%zu) than the input set (%zu)!\n",
145 static_cast<std::size_t
>(tree->
getIndices()->size()),
153 std::vector<bool> processed (cloud.
size (),
false);
156 std::vector<float> nn_distances;
158 for (
const auto &index : indices)
160 if (processed[index])
165 seed_queue.push_back (index);
167 processed[index] =
true;
169 while (sq_idx <
static_cast<int> (seed_queue.size ()))
172 int ret = tree->
radiusSearch (cloud[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances);
175 PCL_ERROR(
"[pcl::extractEuclideanClusters] Received error code -1 from radiusSearch\n");
184 for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
186 if (nn_indices[j] == UNAVAILABLE || processed[nn_indices[j]])
190 seed_queue.push_back (nn_indices[j]);
191 processed[nn_indices[j]] =
true;
198 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
201 r.
indices.resize (seed_queue.size ());
202 for (std::size_t j = 0; j < seed_queue.size (); ++j)
212 clusters.push_back (r);
216 PCL_DEBUG(
"[pcl::extractEuclideanClusters] This cluster has %zu points, which is not between %u and %u points, so it is not a final cluster\n",
217 seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
226 template <
typename Po
intT>
void
229 if (!initCompute () ||
230 (input_ && input_->points.empty ()) ||
231 (indices_ && indices_->empty ()))
240 if (input_->isOrganized ())
247 tree_->setInputCloud (input_, indices_);
248 extractEuclideanClusters (*input_, *indices_, tree_,
static_cast<float> (cluster_tolerance_), clusters, min_pts_per_cluster_, max_pts_per_cluster_);
259 #define PCL_INSTANTIATE_EuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::EuclideanClusterExtraction<T>;
260 #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);
261 #define PCL_INSTANTIATE_extractEuclideanClusters_indices(T) template void PCL_EXPORTS pcl::extractEuclideanClusters<T>(const pcl::PointCloud<T> &, const pcl::Indices &, 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.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
OrganizedNeighbor is a class for optimized nearest neighbor 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).
IndicesAllocator<> Indices
Type used for indices in PCL.