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);
126 template <
typename Po
intT>
void
130 float tolerance, std::vector<PointIndices> &clusters,
131 unsigned int min_pts_per_cluster,
132 unsigned int max_pts_per_cluster)
137 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud "
138 "dataset (%zu) than the input cloud (%zu)!\n",
140 static_cast<std::size_t
>(cloud.
size()));
143 if (tree->
getIndices()->size() != indices.size()) {
144 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different set of "
145 "indices (%zu) than the input set (%zu)!\n",
146 static_cast<std::size_t
>(tree->
getIndices()->size()),
154 std::vector<bool> processed (cloud.
size (),
false);
157 std::vector<float> nn_distances;
159 for (
const auto &index : indices)
161 if (processed[index])
166 seed_queue.push_back (index);
168 processed[index] =
true;
170 while (sq_idx <
static_cast<int> (seed_queue.size ()))
173 int ret = tree->
radiusSearch (cloud[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances);
176 PCL_ERROR(
"[pcl::extractEuclideanClusters] Received error code -1 from radiusSearch\n");
185 for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
187 if (nn_indices[j] == UNAVAILABLE || processed[nn_indices[j]])
191 seed_queue.push_back (nn_indices[j]);
192 processed[nn_indices[j]] =
true;
199 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
202 r.
indices.resize (seed_queue.size ());
203 for (std::size_t j = 0; j < seed_queue.size (); ++j)
213 clusters.push_back (r);
217 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",
218 seed_queue.size (), min_pts_per_cluster, max_pts_per_cluster);
227 template <
typename Po
intT>
void
230 if (!initCompute () ||
231 (input_ && input_->points.empty ()) ||
232 (indices_ && indices_->empty ()))
241 if (input_->isOrganized ())
248 tree_->setInputCloud (input_, indices_);
249 extractEuclideanClusters (*input_, *indices_, tree_,
static_cast<float> (cluster_tolerance_), clusters, min_pts_per_cluster_, max_pts_per_cluster_);
260 #define PCL_INSTANTIATE_EuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::EuclideanClusterExtraction<T>;
261 #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);
262 #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 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).
IndicesAllocator<> Indices
Type used for indices in PCL.