Point Cloud Library (PCL)  1.3.1
extract_clusters.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: extract_clusters.hpp 3035 2011-11-01 04:29:18Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
00039 #define PCL_SEGMENTATION_IMPL_EXTRACT_CLUSTERS_H_
00040 
00041 #include "pcl/segmentation/extract_clusters.h"
00042 
00044 template <typename PointT> void
00045 pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud, 
00046                                const boost::shared_ptr<search::Search<PointT> > &tree,
00047                                float tolerance, std::vector<PointIndices> &clusters,
00048                                unsigned int min_pts_per_cluster, 
00049                                unsigned int max_pts_per_cluster)
00050 {
00051   if (tree->getInputCloud ()->points.size () != cloud.points.size ())
00052   {
00053     PCL_ERROR ("[pcl::extractEuclideanClusters] 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 ());
00054     return;
00055   }
00056   // Create a bool vector of processed point indices, and initialize it to false
00057   std::vector<bool> processed (cloud.points.size (), false);
00058 
00059   std::vector<int> nn_indices;
00060   std::vector<float> nn_distances;
00061   // Process all points in the indices vector
00062   for (size_t i = 0; i < cloud.points.size (); ++i)
00063   {
00064     if (processed[i])
00065       continue;
00066 
00067     std::vector<int> seed_queue;
00068     int sq_idx = 0;
00069     seed_queue.push_back (i);
00070 
00071     processed[i] = true;
00072 
00073     while (sq_idx < (int)seed_queue.size ())
00074     {
00075       // Search for sq_idx
00076       if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
00077       {
00078         sq_idx++;
00079         continue;
00080       }
00081 
00082       for (size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
00083       {
00084         if (processed[nn_indices[j]])                             // Has this point been processed before ?
00085           continue;
00086 
00087         // Perform a simple Euclidean clustering
00088         seed_queue.push_back (nn_indices[j]);
00089         processed[nn_indices[j]] = true;
00090       }
00091 
00092       sq_idx++;
00093     }
00094 
00095     // If this queue is satisfactory, add to the clusters
00096     if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
00097     {
00098       pcl::PointIndices r;
00099       r.indices.resize (seed_queue.size ());
00100       for (size_t j = 0; j < seed_queue.size (); ++j)
00101         r.indices[j] = seed_queue[j];
00102 
00103       std::sort (r.indices.begin (), r.indices.end ());
00104       r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
00105 
00106       r.header = cloud.header;
00107       clusters.push_back (r);   // We could avoid a copy by working directly in the vector
00108     }
00109   }
00110 }
00111 
00113 template <typename PointT> void
00114 pcl::extractEuclideanClusters (const PointCloud<PointT> &cloud, 
00115                                const std::vector<int> &indices,
00116                                const boost::shared_ptr<search::Search<PointT> > &tree,
00117                                float tolerance, std::vector<PointIndices> &clusters,
00118                                unsigned int min_pts_per_cluster, 
00119                                unsigned int max_pts_per_cluster)
00120 {
00121   // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
00122   //and indices[i]
00123   if (tree->getInputCloud ()->points.size () != cloud.points.size ())
00124   {
00125     PCL_ERROR ("[pcl::extractEuclideanClusters] 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 ());
00126     return;
00127   }
00128   if (tree->getIndices ()->size () != indices.size ())
00129   {
00130     PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%lu) than the input set (%lu)!\n", (unsigned long)tree->getIndices ()->size (), (unsigned long)indices.size ());
00131     return;
00132   }
00133 
00134   // Create a bool vector of processed point indices, and initialize it to false
00135   std::vector<bool> processed (indices.size (), false);
00136 
00137   std::vector<int> nn_indices;
00138   std::vector<float> nn_distances;
00139   // Process all points in the indices vector
00140   for (size_t i = 0; i < indices.size (); ++i)
00141   {
00142     if (processed[i])
00143       continue;
00144 
00145     std::vector<int> seed_queue;
00146     int sq_idx = 0;
00147     seed_queue.push_back (i);
00148 
00149     processed[i] = true;
00150 
00151     while (sq_idx < (int)seed_queue.size ())
00152     {
00153       // Search for sq_idx
00154       if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
00155       {
00156         sq_idx++;
00157         continue;
00158       }
00159 
00160       for (size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
00161       {
00162         if (processed[nn_indices[j]])                             // Has this point been processed before ?
00163           continue;
00164 
00165         // Perform a simple Euclidean clustering
00166         seed_queue.push_back (nn_indices[j]);
00167         processed[nn_indices[j]] = true;
00168       }
00169 
00170       sq_idx++;
00171     }
00172 
00173     // If this queue is satisfactory, add to the clusters
00174     if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
00175     {
00176       pcl::PointIndices r;
00177       r.indices.resize (seed_queue.size ());
00178       for (size_t j = 0; j < seed_queue.size (); ++j)
00179         // This is the only place where indices come into play
00180         r.indices[j] = indices[seed_queue[j]];
00181 
00182       //r.indices.assign(seed_queue.begin(), seed_queue.end());
00183       std::sort (r.indices.begin (), r.indices.end ());
00184       r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
00185 
00186       r.header = cloud.header;
00187       clusters.push_back (r);   // We could avoid a copy by working directly in the vector
00188     }
00189   }
00190 }
00191 
00195 
00196 template <typename PointT> void 
00197 pcl::EuclideanClusterExtraction<PointT>::extract (std::vector<PointIndices> &clusters)
00198 {
00199   if (!initCompute () || 
00200       (input_ != 0   && input_->points.empty ()) ||
00201       (indices_ != 0 && indices_->empty ()))
00202   {
00203     clusters.clear ();
00204     return;
00205   }
00206 
00207   // Initialize the spatial locator
00208   if (!tree_)
00209   {
00210     if (input_->isOrganized ())
00211       tree_.reset (new pcl::search::OrganizedNeighbor<PointT> ());
00212     else
00213       tree_.reset (new pcl::search::KdTree<PointT> (false));
00214   }
00215 
00216   // Send the input dataset to the spatial locator
00217   tree_->setInputCloud (input_, indices_);
00218   extractEuclideanClusters (*input_, *indices_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_);
00219 
00220   //tree_->setInputCloud (input_);
00221   //extractEuclideanClusters (*input_, tree_, cluster_tolerance_, clusters, min_pts_per_cluster_, max_pts_per_cluster_);
00222 
00223   // Sort the clusters based on their size (largest one first)
00224   std::sort (clusters.rbegin (), clusters.rend (), comparePointClusters);
00225 
00226   deinitCompute ();
00227 }
00228 
00229 #define PCL_INSTANTIATE_EuclideanClusterExtraction(T) template class PCL_EXPORTS pcl::EuclideanClusterExtraction<T>;
00230 #define PCL_INSTANTIATE_extractEuclideanClusters(T) template void PCL_EXPORTS pcl::extractEuclideanClusters<T>(const pcl::PointCloud<T> &, const boost::shared_ptr<pcl::search::Search<T> > &, float , std::vector<pcl::PointIndices> &, unsigned int, unsigned int);
00231 #define PCL_INSTANTIATE_extractEuclideanClusters_indices(T) template void PCL_EXPORTS pcl::extractEuclideanClusters<T>(const pcl::PointCloud<T> &, const std::vector<int> &, const boost::shared_ptr<pcl::search::Search<T> > &, float , std::vector<pcl::PointIndices> &, unsigned int, unsigned int);
00232 
00233 #endif        // PCL_EXTRACT_CLUSTERS_IMPL_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines