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 "
54 "dataset (%zu) than the input cloud (%zu)!\n",
56 static_cast<std::size_t
>(cloud.
size()));
62 std::vector<bool> processed (cloud.
size (),
false);
64 std::vector<int> nn_indices;
65 std::vector<float> nn_distances;
67 for (
int i = 0; i < static_cast<int> (cloud.
size ()); ++i)
72 std::vector<int> seed_queue;
74 seed_queue.push_back (i);
78 while (sq_idx <
static_cast<int> (seed_queue.size ()))
81 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
87 for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
89 if (nn_indices[j] == -1 || processed[nn_indices[j]])
93 seed_queue.push_back (nn_indices[j]);
94 processed[nn_indices[j]] =
true;
101 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
104 r.
indices.resize (seed_queue.size ());
105 for (std::size_t j = 0; j < seed_queue.size (); ++j)
113 clusters.push_back (r);
120 template <
typename Po
intT>
void
122 const std::vector<int> &indices,
124 float tolerance, std::vector<PointIndices> &clusters,
125 unsigned int min_pts_per_cluster,
126 unsigned int max_pts_per_cluster)
131 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud "
132 "dataset (%zu) than the input cloud (%zu)!\n",
134 static_cast<std::size_t
>(cloud.
size()));
137 if (tree->
getIndices()->size() != indices.size()) {
138 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different set of "
139 "indices (%zu) than the input set (%zu)!\n",
140 static_cast<std::size_t
>(tree->
getIndices()->size()),
148 std::vector<bool> processed (cloud.
size (),
false);
150 std::vector<int> nn_indices;
151 std::vector<float> nn_distances;
153 for (
const int &index : indices)
155 if (processed[index])
158 std::vector<int> seed_queue;
160 seed_queue.push_back (index);
162 processed[index] =
true;
164 while (sq_idx <
static_cast<int> (seed_queue.size ()))
167 int ret = tree->
radiusSearch (cloud[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances);
170 PCL_ERROR(
"[pcl::extractEuclideanClusters] Received error code -1 from radiusSearch\n");
179 for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j)
181 if (nn_indices[j] == -1 || processed[nn_indices[j]])
185 seed_queue.push_back (nn_indices[j]);
186 processed[nn_indices[j]] =
true;
193 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
196 r.
indices.resize (seed_queue.size ());
197 for (std::size_t j = 0; j < seed_queue.size (); ++j)
207 clusters.push_back (r);
216 template <
typename Po
intT>
void
219 if (!initCompute () ||
220 (input_ && input_->points.empty ()) ||
221 (indices_ && indices_->empty ()))
230 if (input_->isOrganized ())
237 tree_->setInputCloud (input_, indices_);
238 extractEuclideanClusters (*input_, *indices_, tree_,
static_cast<float> (cluster_tolerance_), clusters, min_pts_per_cluster_, max_pts_per_cluster_);
249 #define PCL_INSTANTIATE_EuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::EuclideanClusterExtraction<T>;
250 #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);
251 #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.
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).