42 #include <pcl/console/print.h>
43 #include <pcl/pcl_base.h>
45 #include <pcl/search/search.h>
46 #include <pcl/search/kdtree.h>
61 template <
typename Po
intT>
void
64 float tolerance, std::vector<PointIndices> &clusters,
65 unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
79 template <
typename Po
intT>
void
83 unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
102 template <
typename Po
intT,
typename Normal>
void
106 std::vector<PointIndices> &clusters,
double eps_angle,
107 unsigned int min_pts_per_cluster = 1,
108 unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
112 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point "
113 "cloud dataset (%zu) than the input cloud (%zu)!\n",
115 static_cast<std::size_t
>(cloud.
size()));
118 if (cloud.
size () != normals.
size ())
120 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point "
121 "cloud (%zu) different than normals (%zu)!\n",
122 static_cast<std::size_t
>(cloud.
size()),
123 static_cast<std::size_t
>(normals.
size()));
128 std::vector<bool> processed (cloud.
size (),
false);
131 std::vector<float> nn_distances;
133 for (std::size_t i = 0; i < cloud.
size (); ++i)
140 seed_queue.push_back (
static_cast<index_t> (i));
144 while (sq_idx <
static_cast<int> (seed_queue.size ()))
147 if (!tree->
radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
153 for (std::size_t j = 1; j < nn_indices.size (); ++j)
155 if (processed[nn_indices[j]])
160 double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0] +
161 normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] +
162 normals[seed_queue[sq_idx]].normal[2] * normals[nn_indices[j]].normal[2];
163 if ( std::acos (std::abs (dot_p)) < eps_angle )
165 processed[nn_indices[j]] =
true;
174 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
177 r.
indices.resize (seed_queue.size ());
178 for (std::size_t j = 0; j < seed_queue.size (); ++j)
186 clusters.push_back (r);
210 template <
typename Po
intT,
typename Normal>
214 float tolerance, std::vector<PointIndices> &clusters,
double eps_angle,
215 unsigned int min_pts_per_cluster = 1,
216 unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
221 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different point "
222 "cloud dataset (%zu) than the input cloud (%zu)!\n",
224 static_cast<std::size_t
>(cloud.
size()));
227 if (tree->
getIndices()->size() != indices.size()) {
228 PCL_ERROR(
"[pcl::extractEuclideanClusters] Tree built for a different set of "
229 "indices (%zu) than the input set (%zu)!\n",
230 static_cast<std::size_t
>(tree->
getIndices()->size()),
234 if (cloud.
size() != normals.
size()) {
235 PCL_ERROR(
"[pcl::extractEuclideanClusters] Number of points in the input point "
236 "cloud (%zu) different than normals (%zu)!\n",
237 static_cast<std::size_t
>(cloud.
size()),
238 static_cast<std::size_t
>(normals.
size()));
242 std::vector<bool> processed (cloud.
size (),
false);
245 std::vector<float> nn_distances;
247 for (std::size_t i = 0; i < indices.size (); ++i)
249 if (processed[indices[i]])
254 seed_queue.push_back (indices[i]);
256 processed[indices[i]] =
true;
258 while (sq_idx <
static_cast<int> (seed_queue.size ()))
261 if (!tree->
radiusSearch (cloud[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances))
267 for (std::size_t j = 1; j < nn_indices.size (); ++j)
269 if (processed[nn_indices[j]])
274 double dot_p = normals[seed_queue[sq_idx]].normal[0] * normals[nn_indices[j]].normal[0] +
275 normals[seed_queue[sq_idx]].normal[1] * normals[nn_indices[j]].normal[1] +
276 normals[seed_queue[sq_idx]].normal[2] * normals[nn_indices[j]].normal[2];
277 if ( std::acos (std::abs (dot_p)) < eps_angle )
279 processed[nn_indices[j]] =
true;
288 if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
291 r.
indices.resize (seed_queue.size ());
292 for (std::size_t j = 0; j < seed_queue.size (); ++j)
300 clusters.push_back (r);
312 template <
typename Po
intT>
406 extract (std::vector<PointIndices> &clusters);
428 virtual std::string
getClassName ()
const {
return (
"EuclideanClusterExtraction"); }
442 #ifdef PCL_NO_PRECOMPILE
443 #include <pcl/segmentation/impl/extract_clusters.hpp>