diff --git a/common/include/pcl/common/utils.h b/common/include/pcl/common/utils.h index 2390ffeb21f..4a73e76eaa8 100644 --- a/common/include/pcl/common/utils.h +++ b/common/include/pcl/common/utils.h @@ -58,8 +58,8 @@ namespace pcl } /** \brief Utility function to eliminate unused variable warnings. */ - template void - ignore(const T&) + template void + ignore(const T&...) { } } // namespace utils diff --git a/examples/segmentation/example_extract_clusters_normals.cpp b/examples/segmentation/example_extract_clusters_normals.cpp index a3e664c9131..055e606a3cb 100644 --- a/examples/segmentation/example_extract_clusters_normals.cpp +++ b/examples/segmentation/example_extract_clusters_normals.cpp @@ -69,8 +69,8 @@ main (int, char **argv) std::cout << "Estimated the normals" << std::endl; // Creating the kdtree object for the search method of the extraction - pcl::KdTree::Ptr tree_ec (new pcl::KdTreeFLANN ()); - tree_ec->setInputCloud (cloud_ptr); + auto tree = std::make_shared>(); + tree->setInputCloud (cloud_ptr); // Extracting Euclidean clusters using cloud and its normals std::vector cluster_indices; @@ -78,7 +78,7 @@ main (int, char **argv) const double eps_angle = 5 * (M_PI / 180.0); // 5degree tolerance in normals const unsigned int min_cluster_size = 50; - pcl::extractEuclideanClusters (*cloud_ptr, *cloud_normals, tolerance, tree_ec, cluster_indices, eps_angle, min_cluster_size); + pcl::extractEuclideanClusters (*cloud_ptr, *cloud_normals, tolerance, tree, cluster_indices, eps_angle, min_cluster_size); std::cout << "No of clusters formed are " << cluster_indices.size () << std::endl; diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h index 190f2fd99a8..9371976f61b 100644 --- a/segmentation/include/pcl/segmentation/extract_clusters.h +++ b/segmentation/include/pcl/segmentation/extract_clusters.h @@ -41,10 +41,157 @@ #include +#include #include namespace pcl { + +namespace detail { +template +constexpr static bool is_functor_for_additional_filter_criteria_v = + pcl::is_invocable_r_v>&, + pcl::index_t, + const pcl::remove_cvref_t&, + pcl::index_t>; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief Decompose a region of space into clusters based on the Euclidean distance between points + * \param it cloud iterator for iterating over the points + * \param cloud the point cloud message + * \param additional_filter_criteria a functor specifying a criterion that must be satisfied by all point added to the cluster + * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching + * \note the tree has to be created as a spatial locator on \a cloud and \a indices + * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space + * \param clusters the resultant clusters containing point indices (as a vector of PointIndices) + * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) + * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) + * \warning The cloud/indices passed here must be the same ones used to build the tree. + * For performance reasons PCL on warns if the sizes are not equal and no checks are performed to verify if the content is the same + * \ingroup segmentation + */ + template void + extractEuclideanClusters( + ConstCloudIterator &it, const PointCloud &cloud, FunctorT additional_filter_criteria, + const typename search::Search::Ptr &tree, float tolerance, std::vector &clusters, + unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits::max()) + { + static_assert(detail::is_functor_for_additional_filter_criteria_v, + "Functor signature must be similar to `bool(const PointCloud&, " + "index_t, const Indices&, index_t)`"); + + if (tree->getInputCloud ()->points.size () != cloud.points.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built with a different point cloud size (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + return; + } + + // Check if the tree is sorted -- if it is we don't need to check the first element + index_t nn_start_idx = tree->getSortedResults () ? 1 : 0; + // Create a bool vector of processed point indices, and initialize it to false + std::vector processed (cloud.points.size (), false); + + for (; it.isValid(); ++it) { + if (processed[it.getCurrentIndex()]) + continue; + + clusters.emplace_back(); + auto& seed_queue = clusters.back(); + seed_queue.indices.push_back (it.getCurrentIndex()); + + processed[it.getCurrentIndex()] = true; + + for (index_t sq_idx = 0; sq_idx < static_cast (seed_queue.indices.size()); ++sq_idx) + { + Indices nn_indices; + std::vector nn_distances; + + // Search for sq_idx + if (!tree->radiusSearch (seed_queue.indices[sq_idx], tolerance, nn_indices, nn_distances)) + continue; + + for (index_t j = nn_start_idx; j < nn_indices.size (); ++j) // can't assume sorted (default isn't!) + { + if (processed[nn_indices[j]]) // Has this point been processed before ? + continue; + + if (additional_filter_criteria(cloud, it.getCurrentIndex(), nn_indices, j)) { + seed_queue.indices.push_back(nn_indices[j]); + processed[nn_indices[j]] = true; + } + } + } + + // If this queue is satisfactory, add to the clusters + if (seed_queue.indices.size () >= min_pts_per_cluster && seed_queue.indices.size () <= max_pts_per_cluster) + seed_queue.header = cloud.header; + else + clusters.pop_back(); + } + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Decompose a region of space into clusters based on the Euclidean distance between points + * \param cloud the point cloud message + * \param additional_filter_criteria a functor specifying a criterion that must be satisfied by all point added to the cluster + * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching + * \note the tree has to be created as a spatial locator on \a cloud and \a indices + * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space + * \param clusters the resultant clusters containing point indices (as a vector of PointIndices) + * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) + * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) + * \warning The cloud/indices passed here must be the same ones used to build the tree. + * For performance reasons PCL on warns if the sizes are not equal and no checks are performed to verify if the content is the same + * \ingroup segmentation + */ + template void + extractEuclideanClusters ( + const PointCloud &cloud, + FunctorT additional_filter_criteria, const typename search::Search::Ptr &tree, + float tolerance, std::vector &clusters, + unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits::max()) + { + auto it = ConstCloudIterator(cloud); + extractEuclideanClusters(it, cloud, additional_filter_criteria, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster); + } + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Decompose a region of space into clusters based on the Euclidean distance between points + * \param cloud the point cloud message + * \param indices a list of point indices to use from \a cloud + * \param additional_filter_criteria a functor specifying a criterion that must be satisfied by all point added to the cluster + * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching + * \note the tree has to be created as a spatial locator on \a cloud and \a indices + * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space + * \param clusters the resultant clusters containing point indices (as a vector of PointIndices) + * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) + * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) + * \warning The cloud/indices passed here must be the same ones used to build the tree. + * For performance reasons PCL on warns if the sizes are not equal and no checks are performed to verify if the content is the same + * \ingroup segmentation + */ + template void + extractEuclideanClusters ( + const PointCloud &cloud, const Indices &indices, + FunctorT additional_filter_criteria, const typename search::Search::Ptr &tree, + float tolerance, std::vector &clusters, + unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits::max()) + { + // \note If the tree was created over , we guarantee a 1-1 mapping between what the tree returns + //and indices[i] + if (tree->getIndices ()->size () != indices.size ()) + { + PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built with a different size of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ()); + return; + } + + auto it = ConstCloudIterator(cloud, indices); + extractEuclideanClusters(it, cloud, additional_filter_criteria, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster); + } + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Decompose a region of space into clusters based on the Euclidean distance between points * \param cloud the point cloud message @@ -56,11 +203,11 @@ namespace pcl * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) * \ingroup segmentation */ - template void + template void extractEuclideanClusters ( const PointCloud &cloud, const typename search::Search::Ptr &tree, float tolerance, std::vector &clusters, - unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits::max) ()); + unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits::max ()); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Decompose a region of space into clusters based on the Euclidean distance between points @@ -76,9 +223,9 @@ namespace pcl */ template void extractEuclideanClusters ( - const PointCloud &cloud, const std::vector &indices, + const PointCloud &cloud, const Indices &indices, const typename search::Search::Ptr &tree, float tolerance, std::vector &clusters, - unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits::max) ()); + unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits::max()); ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal @@ -89,7 +236,7 @@ namespace pcl * \note the tree has to be created as a spatial locator on \a cloud * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space * \param clusters the resultant clusters containing point indices (as a vector of PointIndices) - * \param eps_angle the maximum allowed difference between normals in radians for cluster/region growing + * \param max_angle the maximum allowed difference between normals in radians for cluster/region growing * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) * \ingroup segmentation @@ -97,84 +244,24 @@ namespace pcl template void extractEuclideanClusters ( const PointCloud &cloud, const PointCloud &normals, - float tolerance, const typename KdTree::Ptr &tree, - std::vector &clusters, double eps_angle, + float tolerance, const typename search::Search::Ptr &tree, + std::vector &clusters, double max_angle, unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits::max) ()) { - if (tree->getInputCloud ()->points.size () != cloud.points.size ()) - { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); - return; - } if (cloud.points.size () != normals.points.size ()) { PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ()); return; } - // Create a bool vector of processed point indices, and initialize it to false - std::vector processed (cloud.points.size (), false); - - std::vector nn_indices; - std::vector nn_distances; - // Process all points in the indices vector - for (std::size_t i = 0; i < cloud.points.size (); ++i) - { - if (processed[i]) - continue; - - std::vector seed_queue; - int sq_idx = 0; - seed_queue.push_back (static_cast (i)); - - processed[i] = true; - - while (sq_idx < static_cast (seed_queue.size ())) - { - // Search for sq_idx - if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances)) - { - sq_idx++; - continue; - } - - for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx - { - if (processed[nn_indices[j]]) // Has this point been processed before ? - continue; - - //processed[nn_indices[j]] = true; - // [-1;1] - double dot_p = normals[i].normal[0] * normals[nn_indices[j]].normal[0] + - normals[i].normal[1] * normals[nn_indices[j]].normal[1] + - normals[i].normal[2] * normals[nn_indices[j]].normal[2]; - if ( std::acos (std::abs (dot_p)) < eps_angle ) - { - processed[nn_indices[j]] = true; - seed_queue.push_back (nn_indices[j]); - } - } - - sq_idx++; - } - - // If this queue is satisfactory, add to the clusters - if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) - { - pcl::PointIndices r; - r.indices.resize (seed_queue.size ()); - for (std::size_t j = 0; j < seed_queue.size (); ++j) - r.indices[j] = seed_queue[j]; - - // These two lines should not be needed: (can anyone confirm?) -FF - std::sort (r.indices.begin (), r.indices.end ()); - r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); - - r.header = cloud.header; - clusters.push_back (r); // We could avoid a copy by working directly in the vector - } - } + max_angle = std::min(std::abs(max_angle), M_PI); + auto cos_max_angle = std::cos(max_angle); + auto normal_deviation_filter = [&](const PointCloud &cloud, index_t i, const Indices& nn_indices, index_t j) -> bool { + double dot_p = normals[i].getNormalVector3fMap().dot(normals[nn_indices[j]].getNormalVector3fMap()); + return std::abs(dot_p) < cos_max_angle; + }; + pcl::extractEuclideanClusters(cloud, normal_deviation_filter, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster); } @@ -188,7 +275,7 @@ namespace pcl * \note the tree has to be created as a spatial locator on \a cloud * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space * \param clusters the resultant clusters containing point indices (as PointIndices) - * \param eps_angle the maximum allowed difference between normals in degrees for cluster/region growing + * \param max_angle the maximum allowed difference between normals in degrees for cluster/region growing * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) * \ingroup segmentation @@ -196,91 +283,29 @@ namespace pcl template void extractEuclideanClusters ( const PointCloud &cloud, const PointCloud &normals, - const std::vector &indices, const typename KdTree::Ptr &tree, - float tolerance, std::vector &clusters, double eps_angle, + const Indices &indices, const typename search::Search::Ptr &tree, + float tolerance, std::vector &clusters, double max_angle, unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits::max) ()) { - // \note If the tree was created over , we guarantee a 1-1 mapping between what the tree returns - //and indices[i] - if (tree->getInputCloud ()->points.size () != cloud.points.size ()) - { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); - return; - } - if (tree->getIndices ()->size () != indices.size ()) - { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ()); - return; - } if (cloud.points.size () != normals.points.size ()) { PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%lu) different than normals (%lu)!\n", cloud.points.size (), normals.points.size ()); return; } - // Create a bool vector of processed point indices, and initialize it to false - std::vector processed (cloud.points.size (), false); - - std::vector nn_indices; - std::vector nn_distances; - // Process all points in the indices vector - for (std::size_t i = 0; i < indices.size (); ++i) - { - if (processed[indices[i]]) - continue; - - std::vector seed_queue; - int sq_idx = 0; - seed_queue.push_back (indices[i]); - - processed[indices[i]] = true; - - while (sq_idx < static_cast (seed_queue.size ())) - { - // Search for sq_idx - if (!tree->radiusSearch (cloud[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances)) - { - sq_idx++; - continue; - } - - for (std::size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx - { - if (processed[nn_indices[j]]) // Has this point been processed before ? - continue; - - //processed[nn_indices[j]] = true; - // [-1;1] - double dot_p = - normals[indices[i]].normal[0] * normals[indices[nn_indices[j]]].normal[0] + - normals[indices[i]].normal[1] * normals[indices[nn_indices[j]]].normal[1] + - normals[indices[i]].normal[2] * normals[indices[nn_indices[j]]].normal[2]; - if ( std::acos (std::abs (dot_p)) < eps_angle ) - { - processed[nn_indices[j]] = true; - seed_queue.push_back (nn_indices[j]); - } - } - sq_idx++; - } - - // If this queue is satisfactory, add to the clusters - if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) - { - pcl::PointIndices r; - r.indices.resize (seed_queue.size ()); - for (std::size_t j = 0; j < seed_queue.size (); ++j) - r.indices[j] = seed_queue[j]; + if (indices.empty()) + return; - // These two lines should not be needed: (can anyone confirm?) -FF - std::sort (r.indices.begin (), r.indices.end ()); - r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); + max_angle = std::min(std::abs(max_angle), M_PI); + auto cos_max_angle = std::cos(max_angle); + auto normal_deviation_filter = [&](const PointCloud &cloud, index_t i, const Indices& nn_indices, index_t j) -> bool { + double dot_p = + normals[indices[i]].getNormalVector3fMap().dot(normals[indices[nn_indices[j]]].getNormalVector3fMap()); + return std::abs(dot_p) < cos_max_angle; + }; - r.header = cloud.header; - clusters.push_back (r); - } - } + pcl::extractEuclideanClusters(cloud, indices, normal_deviation_filter, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster); } ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp index 929fd0d7312..be717e0765d 100644 --- a/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp +++ b/segmentation/include/pcl/segmentation/impl/extract_clusters.hpp @@ -48,158 +48,31 @@ pcl::extractEuclideanClusters (const PointCloud &cloud, unsigned int min_pts_per_cluster, unsigned int max_pts_per_cluster) { - if (tree->getInputCloud ()->points.size () != cloud.points.size ()) - { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); - return; - } - // Check if the tree is sorted -- if it is we don't need to check the first element - int nn_start_idx = tree->getSortedResults () ? 1 : 0; - // Create a bool vector of processed point indices, and initialize it to false - std::vector processed (cloud.points.size (), false); - - std::vector nn_indices; - std::vector nn_distances; - // Process all points in the indices vector - for (int i = 0; i < static_cast (cloud.points.size ()); ++i) - { - if (processed[i]) - continue; - - std::vector seed_queue; - int sq_idx = 0; - seed_queue.push_back (i); - - processed[i] = true; - - while (sq_idx < static_cast (seed_queue.size ())) - { - // Search for sq_idx - if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances)) - { - sq_idx++; - continue; - } - - for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j) // can't assume sorted (default isn't!) - { - if (nn_indices[j] == -1 || processed[nn_indices[j]]) // Has this point been processed before ? - continue; - - // Perform a simple Euclidean clustering - seed_queue.push_back (nn_indices[j]); - processed[nn_indices[j]] = true; - } - - sq_idx++; - } - - // If this queue is satisfactory, add to the clusters - if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) - { - pcl::PointIndices r; - r.indices.resize (seed_queue.size ()); - for (std::size_t j = 0; j < seed_queue.size (); ++j) - r.indices[j] = seed_queue[j]; - - // These two lines should not be needed: (can anyone confirm?) -FF - std::sort (r.indices.begin (), r.indices.end ()); - r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); - - r.header = cloud.header; - clusters.push_back (r); // We could avoid a copy by working directly in the vector - } - } + auto extract_all = [&](const PointCloud &cloud, index_t i, const Indices& nn_indices, index_t j) { + utils::ignore(cloud, i, nn_indices, j); + return true; + }; + pcl::extractEuclideanClusters(cloud, extract_all, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster); } ////////////////////////////////////////////////////////////////////////////////////////////// /** @todo: fix the return value, make sure the exit is not needed anymore*/ template void pcl::extractEuclideanClusters (const PointCloud &cloud, - const std::vector &indices, + const Indices &indices, const typename search::Search::Ptr &tree, float tolerance, std::vector &clusters, unsigned int min_pts_per_cluster, unsigned int max_pts_per_cluster) { - // \note If the tree was created over , we guarantee a 1-1 mapping between what the tree returns - //and indices[i] - if (tree->getInputCloud ()->points.size () != cloud.points.size ()) - { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + if (indices.empty()) return; - } - if (tree->getIndices ()->size () != indices.size ()) - { - PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", tree->getIndices ()->size (), indices.size ()); - return; - } - // Check if the tree is sorted -- if it is we don't need to check the first element - int nn_start_idx = tree->getSortedResults () ? 1 : 0; - - // Create a bool vector of processed point indices, and initialize it to false - std::vector processed (cloud.points.size (), false); - - std::vector nn_indices; - std::vector nn_distances; - // Process all points in the indices vector - for (const int &index : indices) - { - if (processed[index]) - continue; - - std::vector seed_queue; - int sq_idx = 0; - seed_queue.push_back (index); - processed[index] = true; - - while (sq_idx < static_cast (seed_queue.size ())) - { - // Search for sq_idx - int ret = tree->radiusSearch (cloud[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances); - if( ret == -1) - { - PCL_ERROR("[pcl::extractEuclideanClusters] Received error code -1 from radiusSearch\n"); - exit(0); - } - if (!ret) - { - sq_idx++; - continue; - } - - for (std::size_t j = nn_start_idx; j < nn_indices.size (); ++j) // can't assume sorted (default isn't!) - { - if (nn_indices[j] == -1 || processed[nn_indices[j]]) // Has this point been processed before ? - continue; - - // Perform a simple Euclidean clustering - seed_queue.push_back (nn_indices[j]); - processed[nn_indices[j]] = true; - } - - sq_idx++; - } - - // If this queue is satisfactory, add to the clusters - if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster) - { - pcl::PointIndices r; - r.indices.resize (seed_queue.size ()); - for (std::size_t j = 0; j < seed_queue.size (); ++j) - // This is the only place where indices come into play - r.indices[j] = seed_queue[j]; - - // These two lines should not be needed: (can anyone confirm?) -FF - //r.indices.assign(seed_queue.begin(), seed_queue.end()); - std::sort (r.indices.begin (), r.indices.end ()); - r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); - - r.header = cloud.header; - clusters.push_back (r); // We could avoid a copy by working directly in the vector - } - } + auto extract_all = [&](const PointCloud &cloud, index_t i, const Indices& nn_indices, index_t j) { + utils::ignore(cloud, i, nn_indices, j); + return true; + }; + pcl::extractEuclideanClusters(cloud, indices, extract_all, tree, tolerance, clusters, min_pts_per_cluster, max_pts_per_cluster); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -241,6 +114,6 @@ pcl::EuclideanClusterExtraction::extract (std::vector &clu #define PCL_INSTANTIATE_EuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::EuclideanClusterExtraction; #define PCL_INSTANTIATE_extractEuclideanClusters(T) template void PCL_EXPORTS pcl::extractEuclideanClusters(const pcl::PointCloud &, const typename pcl::search::Search::Ptr &, float , std::vector &, unsigned int, unsigned int); -#define PCL_INSTANTIATE_extractEuclideanClusters_indices(T) template void PCL_EXPORTS pcl::extractEuclideanClusters(const pcl::PointCloud &, const std::vector &, const typename pcl::search::Search::Ptr &, float , std::vector &, unsigned int, unsigned int); +#define PCL_INSTANTIATE_extractEuclideanClusters_indices(T) template void PCL_EXPORTS pcl::extractEuclideanClusters(const pcl::PointCloud &, const Indices &, const typename pcl::search::Search::Ptr &, float , std::vector &, unsigned int, unsigned int); #endif // PCL_EXTRACT_CLUSTERS_IMPL_H_