forked from PointCloudLibrary/pcl
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
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
- Loading branch information
Showing
5 changed files
with
330 additions
and
2 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
169 changes: 169 additions & 0 deletions
169
segmentation/include/pcl/segmentation/extract_labeled_clusters.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <pcl/pcl_base.h> | ||
| #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 <typename PointT> void extractLabeledEuclideanClusters (const PointCloud<PointT> &cloud, const boost::shared_ptr<search::Search<PointT> > &tree, float tolerance, std::vector<std::vector<PointIndices> > &labeled_clusters, unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) (), unsigned int max_label = (std::numeric_limits<int>::max)); | ||
|
|
||
| ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// | ||
| /** \brief @b LabeledEuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense, with label info. | ||
| * \author Koen Buys | ||
| * \ingroup segmentation | ||
| */ | ||
| template <typename PointT> | ||
| class LabeledEuclideanClusterExtraction: public PCLBase<PointT> | ||
| { | ||
| typedef PCLBase<PointT> BasePCLBase; | ||
|
|
||
| public: | ||
| typedef pcl::PointCloud<PointT> PointCloud; | ||
| typedef typename PointCloud::Ptr PointCloudPtr; | ||
| typedef typename PointCloud::ConstPtr PointCloudConstPtr; | ||
|
|
||
| typedef typename pcl::search::Search<PointT> KdTree; | ||
| typedef typename pcl::search::Search<PointT>::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<int>::max ()), | ||
| max_label_ (std::numeric_limits<int>::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 <setInputCloud (), setIndices ()> | ||
| * \param clusters the resultant point clusters | ||
| */ | ||
| void extract (std::vector<std::vector<PointIndices> > &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_ |
153 changes: 153 additions & 0 deletions
153
segmentation/include/pcl/segmentation/impl/extract_labeled_clusters.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <typename PointT> void | ||
| pcl::extractLabeledEuclideanClusters (const PointCloud<PointT> &cloud, | ||
| const boost::shared_ptr<search::Search<PointT> > &tree, | ||
| float tolerance, | ||
| std::vector<std::vector<PointIndices> > &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<bool> processed (cloud.points.size (), false); | ||
|
|
||
| std::vector<int> nn_indices; | ||
| std::vector<float> 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<int> 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 <typename PointT> void | ||
| pcl::LabeledEuclideanClusterExtraction<PointT>::extract (std::vector<std::vector<PointIndices> > &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<PointT> ()); | ||
| else | ||
| tree_.reset (new pcl::search::KdTree<PointT> (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<T>; | ||
| #define PCL_INSTANTIATE_extractLabeledEuclideanClusters(T) template void PCL_EXPORTS pcl::extractLabeledEuclideanClusters<T>(const pcl::PointCloud<T> &, const boost::shared_ptr<pcl::search::Search<T> > &, float , std::vector<std::vector<pcl::PointIndices> > &, unsigned int, unsigned int, unsigned int); | ||
|
|
||
| #endif // PCL_EXTRACT_CLUSTERS_IMPL_H_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters