extract_clusters.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  *
00038  */
00039 
00040 #ifndef PCL_EXTRACT_CLUSTERS_H_
00041 #define PCL_EXTRACT_CLUSTERS_H_
00042 
00043 #include <pcl/pcl_base.h>
00044 
00045 #include <pcl/search/pcl_search.h>
00046 
00047 namespace pcl
00048 {
00050 
00060   template <typename PointT> void 
00061   extractEuclideanClusters (
00062       const PointCloud<PointT> &cloud, const boost::shared_ptr<search::Search<PointT> > &tree, 
00063       float tolerance, std::vector<PointIndices> &clusters, 
00064       unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
00065 
00067 
00078   template <typename PointT> void 
00079   extractEuclideanClusters (
00080       const PointCloud<PointT> &cloud, const std::vector<int> &indices, 
00081       const boost::shared_ptr<search::Search<PointT> > &tree, float tolerance, std::vector<PointIndices> &clusters, 
00082       unsigned int min_pts_per_cluster = 1, unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ());
00083 
00085 
00098   template <typename PointT, typename Normal> void 
00099   extractEuclideanClusters (
00100       const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals, 
00101       float tolerance, const boost::shared_ptr<KdTree<PointT> > &tree, 
00102       std::vector<PointIndices> &clusters, double eps_angle, 
00103       unsigned int min_pts_per_cluster = 1, 
00104       unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
00105   {
00106     if (tree->getInputCloud ()->points.size () != cloud.points.size ())
00107     {
00108       PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
00109       return;
00110     }
00111     if (cloud.points.size () != normals.points.size ())
00112     {
00113       PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\n", cloud.points.size (), normals.points.size ());
00114       return;
00115     }
00116 
00117     // Create a bool vector of processed point indices, and initialize it to false
00118     std::vector<bool> processed (cloud.points.size (), false);
00119 
00120     std::vector<int> nn_indices;
00121     std::vector<float> nn_distances;
00122     // Process all points in the indices vector
00123     for (size_t i = 0; i < cloud.points.size (); ++i)
00124     {
00125       if (processed[i])
00126         continue;
00127 
00128       std::vector<unsigned int> seed_queue;
00129       int sq_idx = 0;
00130       seed_queue.push_back (static_cast<int> (i));
00131 
00132       processed[i] = true;
00133 
00134       while (sq_idx < static_cast<int> (seed_queue.size ()))
00135       {
00136         // Search for sq_idx
00137         if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
00138         {
00139           sq_idx++;
00140           continue;
00141         }
00142 
00143         for (size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
00144         {
00145           if (processed[nn_indices[j]])                         // Has this point been processed before ?
00146             continue;
00147 
00148           //processed[nn_indices[j]] = true;
00149           // [-1;1]
00150           double dot_p = normals.points[i].normal[0] * normals.points[nn_indices[j]].normal[0] +
00151                          normals.points[i].normal[1] * normals.points[nn_indices[j]].normal[1] +
00152                          normals.points[i].normal[2] * normals.points[nn_indices[j]].normal[2];
00153           if ( fabs (acos (dot_p)) < eps_angle )
00154           {
00155             processed[nn_indices[j]] = true;
00156             seed_queue.push_back (nn_indices[j]);
00157           }
00158         }
00159 
00160         sq_idx++;
00161       }
00162 
00163       // If this queue is satisfactory, add to the clusters
00164       if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
00165       {
00166         pcl::PointIndices r;
00167         r.indices.resize (seed_queue.size ());
00168         for (size_t j = 0; j < seed_queue.size (); ++j)
00169           r.indices[j] = seed_queue[j];
00170 
00171         // These two lines should not be needed: (can anyone confirm?) -FF
00172         std::sort (r.indices.begin (), r.indices.end ());
00173         r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
00174 
00175         r.header = cloud.header;
00176         clusters.push_back (r);   // We could avoid a copy by working directly in the vector
00177       }
00178     }
00179   }
00180 
00181 
00183 
00197   template <typename PointT, typename Normal> 
00198   void extractEuclideanClusters (
00199       const PointCloud<PointT> &cloud, const PointCloud<Normal> &normals, 
00200       const std::vector<int> &indices, const boost::shared_ptr<KdTree<PointT> > &tree, 
00201       float tolerance, std::vector<PointIndices> &clusters, double eps_angle, 
00202       unsigned int min_pts_per_cluster = 1, 
00203       unsigned int max_pts_per_cluster = (std::numeric_limits<int>::max) ())
00204   {
00205     // \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
00206     //and indices[i]
00207     if (tree->getInputCloud ()->points.size () != cloud.points.size ())
00208     {
00209       PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
00210       return;
00211     }
00212     if (tree->getIndices ()->size () != indices.size ())
00213     {
00214       PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different set of indices (%zu) than the input set (%zu)!\n", tree->getIndices ()->size (), indices.size ());
00215       return;
00216     }
00217     if (cloud.points.size () != normals.points.size ())
00218     {
00219       PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\n", cloud.points.size (), normals.points.size ());
00220       return;
00221     }
00222     // Create a bool vector of processed point indices, and initialize it to false
00223     std::vector<bool> processed (cloud.points.size (), false);
00224 
00225     std::vector<int> nn_indices;
00226     std::vector<float> nn_distances;
00227     // Process all points in the indices vector
00228     for (size_t i = 0; i < indices.size (); ++i)
00229     {
00230       if (processed[indices[i]])
00231         continue;
00232 
00233       std::vector<int> seed_queue;
00234       int sq_idx = 0;
00235       seed_queue.push_back (indices[i]);
00236 
00237       processed[indices[i]] = true;
00238 
00239       while (sq_idx < static_cast<int> (seed_queue.size ()))
00240       {
00241         // Search for sq_idx
00242         if (!tree->radiusSearch (cloud.points[seed_queue[sq_idx]], tolerance, nn_indices, nn_distances))
00243         {
00244           sq_idx++;
00245           continue;
00246         }
00247 
00248         for (size_t j = 1; j < nn_indices.size (); ++j)             // nn_indices[0] should be sq_idx
00249         {
00250           if (processed[nn_indices[j]])                             // Has this point been processed before ?
00251             continue;
00252 
00253           //processed[nn_indices[j]] = true;
00254           // [-1;1]
00255           double dot_p =
00256             normals.points[indices[i]].normal[0] * normals.points[indices[nn_indices[j]]].normal[0] +
00257             normals.points[indices[i]].normal[1] * normals.points[indices[nn_indices[j]]].normal[1] +
00258             normals.points[indices[i]].normal[2] * normals.points[indices[nn_indices[j]]].normal[2];
00259           if ( fabs (acos (dot_p)) < eps_angle )
00260           {
00261             processed[nn_indices[j]] = true;
00262             seed_queue.push_back (nn_indices[j]);
00263           }
00264         }
00265 
00266         sq_idx++;
00267       }
00268 
00269       // If this queue is satisfactory, add to the clusters
00270       if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
00271       {
00272         pcl::PointIndices r;
00273         r.indices.resize (seed_queue.size ());
00274         for (size_t j = 0; j < seed_queue.size (); ++j)
00275           r.indices[j] = seed_queue[j];
00276 
00277         // These two lines should not be needed: (can anyone confirm?) -FF
00278         std::sort (r.indices.begin (), r.indices.end ());
00279         r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
00280 
00281         r.header = cloud.header;
00282         clusters.push_back (r);
00283       }
00284     }
00285   }
00286 
00290 
00294   template <typename PointT>
00295   class EuclideanClusterExtraction: public PCLBase<PointT>
00296   {
00297     typedef PCLBase<PointT> BasePCLBase;
00298 
00299     public:
00300       typedef pcl::PointCloud<PointT> PointCloud;
00301       typedef typename PointCloud::Ptr PointCloudPtr;
00302       typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00303 
00304       typedef typename pcl::search::Search<PointT> KdTree;
00305       typedef typename pcl::search::Search<PointT>::Ptr KdTreePtr;
00306 
00307       typedef PointIndices::Ptr PointIndicesPtr;
00308       typedef PointIndices::ConstPtr PointIndicesConstPtr;
00309 
00311 
00312       EuclideanClusterExtraction () : tree_ (), 
00313                                       cluster_tolerance_ (0),
00314                                       min_pts_per_cluster_ (1), 
00315                                       max_pts_per_cluster_ (std::numeric_limits<int>::max ())
00316       {};
00317 
00321       inline void 
00322       setSearchMethod (const KdTreePtr &tree) 
00323       { 
00324         tree_ = tree; 
00325       }
00326 
00330       inline KdTreePtr 
00331       getSearchMethod () const 
00332       { 
00333         return (tree_); 
00334       }
00335 
00339       inline void 
00340       setClusterTolerance (double tolerance) 
00341       { 
00342         cluster_tolerance_ = tolerance; 
00343       }
00344 
00346       inline double 
00347       getClusterTolerance () const 
00348       { 
00349         return (cluster_tolerance_); 
00350       }
00351 
00355       inline void 
00356       setMinClusterSize (int min_cluster_size) 
00357       { 
00358         min_pts_per_cluster_ = min_cluster_size; 
00359       }
00360 
00362       inline int 
00363       getMinClusterSize () const 
00364       { 
00365         return (min_pts_per_cluster_); 
00366       }
00367 
00371       inline void 
00372       setMaxClusterSize (int max_cluster_size) 
00373       { 
00374         max_pts_per_cluster_ = max_cluster_size; 
00375       }
00376 
00378       inline int 
00379       getMaxClusterSize () const 
00380       { 
00381         return (max_pts_per_cluster_); 
00382       }
00383 
00387       void 
00388       extract (std::vector<PointIndices> &clusters);
00389 
00390     protected:
00391       // Members derived from the base class
00392       using BasePCLBase::input_;
00393       using BasePCLBase::indices_;
00394       using BasePCLBase::initCompute;
00395       using BasePCLBase::deinitCompute;
00396 
00398       KdTreePtr tree_;
00399 
00401       double cluster_tolerance_;
00402 
00404       int min_pts_per_cluster_;
00405 
00407       int max_pts_per_cluster_;
00408 
00410       virtual std::string getClassName () const { return ("EuclideanClusterExtraction"); }
00411 
00412   };
00413 
00417   inline bool 
00418   comparePointClusters (const pcl::PointIndices &a, const pcl::PointIndices &b)
00419   {
00420     return (a.indices.size () < b.indices.size ());
00421   }
00422 }
00423 
00424 #ifdef PCL_NO_PRECOMPILE
00425 #include <pcl/segmentation/impl/extract_clusters.hpp>
00426 #endif
00427 
00428 #endif  //#ifndef PCL_EXTRACT_CLUSTERS_H_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:23:42