48 float tolerance, std::vector<PointIndices> &clusters,
49 unsigned int min_pts_per_cluster,
50 unsigned int max_pts_per_cluster)
52 if (tree->getInputCloud ()->size () != cloud.
size ())
54 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud "
55 "dataset (%zu) than the input cloud (%zu)!\n",
56 static_cast<std::size_t
>(tree->getInputCloud()->size()),
57 static_cast<std::size_t
>(cloud.
size()));
61 int nn_start_idx = tree->getSortedResults () ? 1 : 0;
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);
130 float tolerance, std::vector<PointIndices> &clusters,
131 unsigned int min_pts_per_cluster,
132 unsigned int max_pts_per_cluster)
136 if (tree->getInputCloud()->size() != cloud.
size()) {
137 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point cloud "
138 "dataset (%zu) than the input cloud (%zu)!\n",
139 static_cast<std::size_t
>(tree->getInputCloud()->size()),
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()),
151 int nn_start_idx = tree->getSortedResults () ? 1 : 0;
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);
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_);
255 std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters);
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.