diff --git a/segmentation/include/pcl/segmentation/extract_labeled_clusters.h b/segmentation/include/pcl/segmentation/extract_labeled_clusters.h index e1a9b52220c..6b85cbe0c17 100644 --- a/segmentation/include/pcl/segmentation/extract_labeled_clusters.h +++ b/segmentation/include/pcl/segmentation/extract_labeled_clusters.h @@ -40,6 +40,12 @@ namespace pcl { + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Map of label and associated clusters + */ + using labeled_cluster_map_t = std::map>; + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Decompose a region of space into clusters based on the Euclidean distance between points * \param[in] cloud the point cloud message @@ -59,6 +65,25 @@ namespace pcl unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits::max (), unsigned int max_label = std::numeric_limits::max ()); + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Decompose a region of space into clusters based on the Euclidean distance between points + * \param[in] cloud the point cloud message + * \param[in] 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 + * \param[in] tolerance the spatial cluster tolerance as a measure in L2 Euclidean space + * \param[out] labeled_clusters a map of the resultant clusters, grouped by label + * \param[in] min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) + * \param[in] max_pts_per_cluster maximum number of points that a cluster may contain (default: max uint) + * \param[in] max_label the maximum number of labels to extract (default: max uint) + * \ingroup segmentation + */ + template void + extractLabeledEuclideanClusters ( + const PointCloud &cloud, const typename search::Search::Ptr &tree, + float tolerance, labeled_cluster_map_t &labeled_clusters, + unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = std::numeric_limits::max (), + unsigned int max_labels = std::numeric_limits::max ()); + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -148,6 +173,12 @@ namespace pcl void extract (std::vector > &labeled_clusters); + /** \brief Cluster extraction in a PointCloud given by + * \param[out] labeled_clusters the resultant point clusters keyed by label + */ + void + extract ( labeled_cluster_map_t &labeled_clusters); + protected: // Members derived from the base class using BasePCLBase::input_; diff --git a/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp index 92aa157a396..5439b865237 100644 --- a/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp +++ b/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp @@ -39,45 +39,99 @@ #include +namespace impl +{ + // convert labeled_cluster_map_t + void + labeled_cluster_map_to_labeled_cluster_vector( const pcl::PCLHeader& header, pcl::labeled_cluster_map_t labeled_map, std::vector>& vec ) + { + if ( labeled_map.empty() ) + return; + + // get max label + const auto extracted_max_label = std::max_element( labeled_map.begin(), labeled_map.end(), + []( const pcl::labeled_cluster_map_t::value_type& lhs, const pcl::labeled_cluster_map_t::value_type& rhs ) { return lhs.first < rhs.first; } + ) + ->first; + + // if the user didn't provide a preallocated vector, create it for them based on max label + if ( vec.empty() ) + vec.resize(extracted_max_label+1); + else if ( vec.size() <= extracted_max_label ) + { + PCL_ERROR ("[pcl::extractLabeledEuclideanClusters] labeled_clusters size (%lu) less than extracted max_label (%lu)!\n", vec.size (), (unsigned int)extracted_max_label ); + return; + } + + // move clusters to output vector + // convert pcl::Indices to pcl:PointIndices + for ( auto& label_group : labeled_map ) + { + for ( auto& cluster : label_group.second ) // cluster is pcl::Indices + { + pcl::PointIndices pi = {}; + pi.header = header; + pi.indices = std::move(cluster); + vec[label_group.first].emplace_back(std::move(pi)); + } + } + + } +} + ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::extractLabeledEuclideanClusters (const PointCloud &cloud, const typename search::Search::Ptr &tree, float tolerance, - std::vector > &labeled_clusters, + labeled_cluster_map_t &labeled_clusters, unsigned int min_pts_per_cluster, unsigned int max_pts_per_cluster, - unsigned int) + unsigned int max_labels ) { - if (tree->getInputCloud ()->points.size () != cloud.points.size ()) + assert(tree); // ensure we have a tree + + if (tree->getInputCloud ()->size () != cloud.size ()) { - PCL_ERROR ("[pcl::extractLabeledEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ()); + PCL_ERROR ("[pcl::extractLabeledEuclideanClusters] Tree built for a different point cloud dataset (%lu) than the input cloud (%lu)!\n", tree->getInputCloud ()->size (), cloud.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; + // Create a bool vector of processed point indices, and initialize it to false + std::vector processed (cloud.size (), false); - // Process all points in the indices vector - for (int i = 0; i < static_cast (cloud.points.size ()); ++i) + // Process all points in the cloud + for ( index_t i = 0; i < static_cast(cloud.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 ())) + // label for the current point + const auto current_label = cloud[i].label; + + // iterator in labeled_clusters for the current label + const auto current_label_cluster_it = labeled_clusters.find( current_label ); + + // max labels check + // if we would have to add a new label and we're already at max, no need to proceed + if ( current_label_cluster_it == labeled_clusters.end() && labeled_clusters.size() >= max_labels ) + continue; + + pcl::Indices seed_queue = {i}; + + index_t sq_idx = 0; + while (sq_idx < static_cast (seed_queue.size ())) { // Search for sq_idx - int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, std::numeric_limits::max()); + pcl::Indices nn_indices = {}; + std::vector nn_distances = {}; + + const int ret = tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances, max_pts_per_cluster ); if(ret == -1) PCL_ERROR("radiusSearch on tree came back with error -1"); + if (!ret) { sq_idx++; @@ -86,12 +140,10 @@ pcl::extractLabeledEuclideanClusters (const PointCloud &cloud, 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; - if (cloud.points[i].label == cloud.points[nn_indices[j]].label) + // labels match, and nn point not processed before + if ( !processed[nn_indices[j]] && current_label == cloud[nn_indices[j]].label ) { - // Perform a simple Euclidean clustering - seed_queue.push_back (nn_indices[j]); + seed_queue.push_back ( nn_indices[j] ) ; processed[nn_indices[j]] = true; } } @@ -101,26 +153,54 @@ pcl::extractLabeledEuclideanClusters (const PointCloud &cloud, // 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]; - - std::sort (r.indices.begin (), r.indices.end ()); - r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ()); + { + if ( current_label_cluster_it == labeled_clusters.end() ) // label not found in map, add + { + std::vector v = {}; + v.emplace_back(std::move(seed_queue)); + labeled_clusters.emplace(current_label, std::move(v)); + } + else // label already exists in map; append seed_queue to label vector + current_label_cluster_it->second.emplace_back(std::move(seed_queue)); - r.header = cloud.header; - labeled_clusters[cloud.points[i].label].push_back (r); // We could avoid a copy by working directly in the vector } } } + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::extractLabeledEuclideanClusters (const PointCloud &cloud, + const typename search::Search::Ptr &tree, + float tolerance, + std::vector > &labeled_clusters, + unsigned int min_pts_per_cluster, + unsigned int max_pts_per_cluster, + unsigned int max_label ) +{ + labeled_cluster_map_t label_map = {}; + extractLabeledEuclideanClusters( cloud, tree, tolerance, label_map, min_pts_per_cluster, max_pts_per_cluster, max_label ); + + impl::labeled_cluster_map_to_labeled_cluster_vector( cloud.header, std::move(label_map), labeled_clusters ); +} + ////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::LabeledEuclideanClusterExtraction::extract (std::vector > &labeled_clusters) +{ + labeled_cluster_map_t label_map = {}; + extract( label_map ); + impl::labeled_cluster_map_to_labeled_cluster_vector( input_->header, std::move(label_map), labeled_clusters ); + + // Sort the clusters based on their size (largest one first) + for (auto &labeled_cluster : labeled_clusters) + std::sort (labeled_cluster.rbegin (), labeled_cluster.rend (), comparePointClusters); +} + +template void +pcl::LabeledEuclideanClusterExtraction::extract ( pcl::labeled_cluster_map_t &labeled_clusters) { if (!initCompute () || (input_ && input_->points.empty ()) || @@ -143,14 +223,11 @@ pcl::LabeledEuclideanClusterExtraction::extract (std::vectorsetInputCloud (input_); extractLabeledEuclideanClusters (*input_, tree_, static_cast (cluster_tolerance_), labeled_clusters, min_pts_per_cluster_, max_pts_per_cluster_, max_label_); - // Sort the clusters based on their size (largest one first) - for (auto &labeled_cluster : labeled_clusters) - std::sort (labeled_cluster.rbegin (), labeled_cluster.rend (), comparePointClusters); - deinitCompute (); } #define PCL_INSTANTIATE_LabeledEuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::LabeledEuclideanClusterExtraction; #define PCL_INSTANTIATE_extractLabeledEuclideanClusters(T) template void PCL_EXPORTS pcl::extractLabeledEuclideanClusters(const pcl::PointCloud &, const typename pcl::search::Search::Ptr &, float , std::vector > &, unsigned int, unsigned int, unsigned int); +#define PCL_INSTANTIATE_extractLabeledEuclideanClustersMap(T) template void PCL_EXPORTS pcl::extractLabeledEuclideanClusters(const pcl::PointCloud &, const typename pcl::search::Search::Ptr &, float , pcl::labeled_cluster_map_t &, unsigned int, unsigned int, unsigned int); #endif // PCL_EXTRACT_CLUSTERS_IMPL_H_ diff --git a/test/segmentation/test_segmentation.cpp b/test/segmentation/test_segmentation.cpp index 5121ecc57bb..94c2fdc30fd 100644 --- a/test/segmentation/test_segmentation.cpp +++ b/test/segmentation/test_segmentation.cpp @@ -49,6 +49,7 @@ #include #include #include +#include using namespace pcl; using namespace pcl::io; @@ -62,6 +63,10 @@ pcl::PointCloud::Ptr another_cloud_; pcl::PointCloud::Ptr normals_; pcl::PointCloud::Ptr another_normals_; +pcl::PointCloud::Ptr labeled_cloud_; +static const std::size_t LABELED_CLOUD_NUM_LABELS_ = 5; +static const std::size_t LABELED_CLOUD_NUM_PTS_PER_CLUSTER_ = 5; + //////////////////////////////////////////////////////////////////////////////////////////////// TEST (RegionGrowingRGBTest, Segment) { @@ -385,6 +390,79 @@ TEST (ExtractPolygonalPrism, Segmentation) EXPECT_EQ (static_cast (output.indices.size ()), 0); } +////////////////////////////////////////////////////////////////////////////////////////////// +TEST (ExtractLabeledEuclideanClusters, SegmentEmptyCloud) +{ + pcl::PointCloud::Ptr empty_cloud (new pcl::PointCloud); + + pcl::LabeledEuclideanClusterExtraction ec; + ec.setInputCloud (empty_cloud); + + std::vector < std::vector< pcl::PointIndices > > labeled_clusters; + ec.extract (labeled_clusters); + int num_of_labels = static_cast (labeled_clusters.size ()); + EXPECT_EQ (0, num_of_labels); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +TEST (ExtractLabeledEuclideanClusters, SegmentFromPointsVec) +{ + pcl::LabeledEuclideanClusterExtraction ec; + + ec.setInputCloud (labeled_cloud_); + ec.setClusterTolerance( std::numeric_limits::max() ); // no distance limit + + std::vector < std::vector< pcl::PointIndices > > labeled_clusters; + ec.extract (labeled_clusters); + + const auto num_of_labels = labeled_clusters.size (); + EXPECT_EQ (LABELED_CLOUD_NUM_LABELS_, num_of_labels); + + for ( std::size_t i = 0; i < labeled_clusters.size(); ++i ) + { + const auto& label = labeled_clusters[i]; + + EXPECT_EQ( 1, label.size() ); // 1 cluster per label + + const auto& cluster = label.front(); + EXPECT_EQ( LABELED_CLOUD_NUM_PTS_PER_CLUSTER_, cluster.indices.size() ); // n points per cluster + + for ( const auto idx : cluster.indices ) // check all labels in the cluster + EXPECT_EQ( i, (*labeled_cloud_)[idx].label ); // get point from cloud, check its label + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +TEST (ExtractLabeledEuclideanClusters, SegmentFromPointsMap) +{ + pcl::LabeledEuclideanClusterExtraction ec; + + ec.setInputCloud (labeled_cloud_); + ec.setClusterTolerance( std::numeric_limits::max() ); // no distance limit + + pcl::labeled_cluster_map_t labeled_clusters = {}; + ec.extract (labeled_clusters); + + const auto num_of_labels = labeled_clusters.size (); + EXPECT_EQ (LABELED_CLOUD_NUM_LABELS_, num_of_labels); + + for ( const auto& label : labeled_clusters ) + { + const auto& clusters = label.second; + EXPECT_EQ( 1, clusters.size() ); // 1 cluster per label + + for ( const auto& cluster : clusters ) + { + EXPECT_EQ( LABELED_CLOUD_NUM_PTS_PER_CLUSTER_, cluster.size() ); // n points per cluster + + for ( const auto idx : cluster ) // check all labels in the cluster + EXPECT_EQ( label.first, (*labeled_cloud_)[idx].label ); // get point from cloud, check its label + } + + } +} + + /* ---[ */ int main (int argc, char** argv) @@ -438,6 +516,20 @@ main (int argc, char** argv) normal_estimator.setKSearch(30); normal_estimator.compute(*another_normals_); + // load labeled cloud + labeled_cloud_ = (new pcl::PointCloud)->makeShared(); + + // create LABELED_CLOUD_NUM_LABELS_ labels, each with 1 cluster of LABELED_CLOUD_NUM_PTS_PER_CLUSTER_ points + for (std::size_t i = 0; i < LABELED_CLOUD_NUM_LABELS_; ++i) + { + for (std::size_t j = 0; j < LABELED_CLOUD_NUM_PTS_PER_CLUSTER_; ++j) { + auto pt = pcl::PointXYZL(); + pt.x = pt.y = pt.z = static_cast( j ); + pt.label = static_cast (i); + labeled_cloud_->points.emplace_back(std::move(pt)); + } + } + testing::InitGoogleTest (&argc, argv); return (RUN_ALL_TESTS ()); }