From 02579915f0a98d70988beeb53b15d7c7b632bd65 Mon Sep 17 00:00:00 2001 From: Koen Buys Date: Mon, 31 Oct 2011 17:17:53 +0000 Subject: [PATCH] Added labeled cluster euclidean segmentation for use in the people stack git-svn-id: svn+ssh://svn.pointclouds.org/pcl/trunk@2993 a9d63959-f2ad-4865-b262-bf0e56cfafb6 --- segmentation/CMakeLists.txt | 2 + .../pcl/segmentation/extract_clusters.h | 4 +- .../segmentation/extract_labeled_clusters.h | 169 ++++++++++++++++++ .../impl/extract_labeled_clusters.hpp | 153 ++++++++++++++++ segmentation/src/extract_clusters.cpp | 4 + 5 files changed, 330 insertions(+), 2 deletions(-) create mode 100644 segmentation/include/pcl/segmentation/extract_labeled_clusters.h create mode 100644 segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp diff --git a/segmentation/CMakeLists.txt b/segmentation/CMakeLists.txt index af961bd3477..9116bccd50a 100644 --- a/segmentation/CMakeLists.txt +++ b/segmentation/CMakeLists.txt @@ -14,12 +14,14 @@ if(build) ) set(incs include/pcl/${SUBSYS_NAME}/extract_clusters.h + include/pcl/${SUBSYS_NAME}/extract_labeled_clusters.h include/pcl/${SUBSYS_NAME}/extract_polygonal_prism_data.h include/pcl/${SUBSYS_NAME}/sac_segmentation.h include/pcl/${SUBSYS_NAME}/segment_differences.h ) set(impl_incs include/pcl/${SUBSYS_NAME}/impl/extract_clusters.hpp + include/pcl/${SUBSYS_NAME}/impl/extract_labeled_clusters.hpp include/pcl/${SUBSYS_NAME}/impl/extract_polygonal_prism_data.hpp include/pcl/${SUBSYS_NAME}/impl/sac_segmentation.hpp include/pcl/${SUBSYS_NAME}/impl/segment_differences.hpp diff --git a/segmentation/include/pcl/segmentation/extract_clusters.h b/segmentation/include/pcl/segmentation/extract_clusters.h index fabd20a9c4c..db8e258ffb5 100644 --- a/segmentation/include/pcl/segmentation/extract_clusters.h +++ b/segmentation/include/pcl/segmentation/extract_clusters.h @@ -289,8 +289,8 @@ namespace pcl typedef typename PointCloud::Ptr PointCloudPtr; typedef typename PointCloud::ConstPtr PointCloudConstPtr; - typedef typename pcl::search::Search KdTree; - typedef typename pcl::search::Search::Ptr KdTreePtr; + typedef typename pcl::search::Search KdTree; + typedef typename pcl::search::Search::Ptr KdTreePtr; typedef PointIndices::Ptr PointIndicesPtr; typedef PointIndices::ConstPtr PointIndicesConstPtr; diff --git a/segmentation/include/pcl/segmentation/extract_labeled_clusters.h b/segmentation/include/pcl/segmentation/extract_labeled_clusters.h new file mode 100644 index 00000000000..4d3c2e991ff --- /dev/null +++ b/segmentation/include/pcl/segmentation/extract_labeled_clusters.h @@ -0,0 +1,169 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_EXTRACT_LABELED_CLUSTERS_H_ +#define PCL_EXTRACT_LABELED_CLUSTERS_H_ + +#include +#include "pcl/search/pcl_search.h" + +namespace pcl +{ + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Decompose a region of space into clusters based on the Euclidean distance between points + * \param cloud the point cloud message + * \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 + * \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) + * \ingroup segmentation + */ + template void extractLabeledEuclideanClusters (const PointCloud &cloud, const boost::shared_ptr > &tree, float tolerance, std::vector > &labeled_clusters, 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 @b LabeledEuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense, with label info. + * \author Koen Buys + * \ingroup segmentation + */ + template + class LabeledEuclideanClusterExtraction: public PCLBase + { + typedef PCLBase BasePCLBase; + + public: + typedef pcl::PointCloud PointCloud; + typedef typename PointCloud::Ptr PointCloudPtr; + typedef typename PointCloud::ConstPtr PointCloudConstPtr; + + typedef typename pcl::search::Search KdTree; + typedef typename pcl::search::Search::Ptr KdTreePtr; + + typedef PointIndices::Ptr PointIndicesPtr; + typedef PointIndices::ConstPtr PointIndicesConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /** \brief Empty constructor. */ + LabeledEuclideanClusterExtraction () : tree_ (), min_pts_per_cluster_ (1), + max_pts_per_cluster_ (std::numeric_limits::max ()), + max_label_ (std::numeric_limits::max ()) + {}; + + /** \brief Provide a pointer to the search object. + * \param tree a pointer to the spatial search object. + */ + inline void setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } + + /** \brief Get a pointer to the search method used. */ + inline KdTreePtr getSearchMethod () { return (tree_); } + + /** \brief Set the spatial cluster tolerance as a measure in the L2 Euclidean space + * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space + */ + inline void setClusterTolerance (double tolerance) { cluster_tolerance_ = tolerance; } + + /** \brief Get the spatial cluster tolerance as a measure in the L2 Euclidean space. */ + inline double getClusterTolerance () { return (cluster_tolerance_); } + + /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid. + * \param min_cluster_size the minimum cluster size + */ + inline void setMinClusterSize (int min_cluster_size) { min_pts_per_cluster_ = min_cluster_size; } + + /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */ + inline int getMinClusterSize () { return (min_pts_per_cluster_); } + + /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid. + * \param max_cluster_size the maximum cluster size + */ + inline void setMaxClusterSize (int max_cluster_size) { max_pts_per_cluster_ = max_cluster_size; } + + /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */ + inline int getMaxClusterSize () { return (max_pts_per_cluster_); } + + /** \brief Set the maximum number of labels in the cloud. + * \param max_label the maximum + */ + inline void setMaxLabels (unsigned int max_label) { max_label_ = max_label; } + + /** \brief Get the maximum number of labels */ + inline unsigned int getMaxLabels () { return (max_label_); } + + /** \brief Cluster extraction in a PointCloud given by + * \param clusters the resultant point clusters + */ + void extract (std::vector > &labeled_clusters); + + protected: + // Members derived from the base class + using BasePCLBase::input_; + using BasePCLBase::indices_; + using BasePCLBase::initCompute; + using BasePCLBase::deinitCompute; + + /** \brief A pointer to the spatial search object. */ + KdTreePtr tree_; + + /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */ + double cluster_tolerance_; + + /** \brief The minimum number of points that a cluster needs to contain in order to be considered valid (default = 1). */ + int min_pts_per_cluster_; + + /** \brief The maximum number of points that a cluster needs to contain in order to be considered valid (default = MAXINT). */ + int max_pts_per_cluster_; + + /** \brief The maximum number of labels we can find in this pointcloud (default = MAXINT)*/ + unsigned int max_label_; + + /** \brief Class getName method. */ + virtual std::string getClassName () const { return ("LabeledEuclideanClusterExtraction"); } + + }; + + /** \brief Sort clusters method (for std::sort). + * \ingroup segmentation + */ + inline bool + compareLabeledPointClusters (const pcl::PointIndices &a, const pcl::PointIndices &b) + { + return (a.indices.size () < b.indices.size ()); + } +} + +#endif //#ifndef PCL_EXTRACT_LABELED_CLUSTERS_H_ diff --git a/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp b/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp new file mode 100644 index 00000000000..3ffe5b94d3e --- /dev/null +++ b/segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp @@ -0,0 +1,153 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * + */ + +#ifndef PCL_SEGMENTATION_IMPL_EXTRACT_LABELED_CLUSTERS_H_ +#define PCL_SEGMENTATION_IMPL_EXTRACT_LABELED_CLUSTERS_H_ + +#include "pcl/segmentation/extract_labeled_clusters.h" + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::extractLabeledEuclideanClusters (const PointCloud &cloud, + const boost::shared_ptr > &tree, + float tolerance, + std::vector > &labeled_clusters, + unsigned int min_pts_per_cluster, + unsigned int max_pts_per_cluster, + unsigned int max_label) +{ + if (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", (unsigned long)tree->getInputCloud ()->points.size (), (unsigned long)cloud.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 (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 (i); + + processed[i] = true; + + while (sq_idx < (int)seed_queue.size ()) + { + // Search for sq_idx + if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances)) + { + sq_idx++; + continue; + } + + for (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) + { + // 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 (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 ()); + + 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::LabeledEuclideanClusterExtraction::extract (std::vector > &labeled_clusters) +{ + if (!initCompute () || + (input_ != 0 && input_->points.empty ()) || + (indices_ != 0 && indices_->empty ())) + { + labeled_clusters.clear (); + return; + } + + // Initialize the spatial locator + if (!tree_) + { + if (input_->isOrganized ()) + tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + tree_.reset (new pcl::search::KdTree (false)); + } + + // Send the input dataset to the spatial locator + tree_->setInputCloud (input_); + extractLabeledEuclideanClusters (*input_, tree_, 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(unsigned int i = 0; i < labeled_clusters.size(); i++) + std::sort (labeled_clusters[i].rbegin (), labeled_clusters[i].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 boost::shared_ptr > &, float , std::vector > &, unsigned int, unsigned int, unsigned int); + +#endif // PCL_EXTRACT_CLUSTERS_IMPL_H_ diff --git a/segmentation/src/extract_clusters.cpp b/segmentation/src/extract_clusters.cpp index 9f2ce8d3c2a..6b61e3f8fc5 100644 --- a/segmentation/src/extract_clusters.cpp +++ b/segmentation/src/extract_clusters.cpp @@ -39,8 +39,12 @@ #include "pcl/point_types.h" #include "pcl/segmentation/extract_clusters.h" #include "pcl/segmentation/impl/extract_clusters.hpp" +#include "pcl/segmentation/extract_labeled_clusters.h" +#include "pcl/segmentation/impl/extract_labeled_clusters.hpp" // Instantiations of specific point types PCL_INSTANTIATE(EuclideanClusterExtraction, PCL_XYZ_POINT_TYPES); PCL_INSTANTIATE(extractEuclideanClusters, PCL_XYZ_POINT_TYPES); PCL_INSTANTIATE(extractEuclideanClusters_indices, PCL_XYZ_POINT_TYPES); +PCL_INSTANTIATE(LabeledEuclideanClusterExtraction, PCL_XYZL_POINT_TYPES); +PCL_INSTANTIATE(extractLabeledEuclideanClusters, PCL_XYZL_POINT_TYPES);