Point Cloud Library (PCL)
1.7.0
|
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 { 00049 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00050 /** \brief Decompose a region of space into clusters based on the Euclidean distance between points 00051 * \param cloud the point cloud message 00052 * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching 00053 * \note the tree has to be created as a spatial locator on \a cloud 00054 * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space 00055 * \param clusters the resultant clusters containing point indices (as a vector of PointIndices) 00056 * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) 00057 * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) 00058 * \ingroup segmentation 00059 */ 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 00066 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00067 /** \brief Decompose a region of space into clusters based on the Euclidean distance between points 00068 * \param cloud the point cloud message 00069 * \param indices a list of point indices to use from \a cloud 00070 * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching 00071 * \note the tree has to be created as a spatial locator on \a cloud and \a indices 00072 * \param tolerance the spatial cluster tolerance as a measure in L2 Euclidean space 00073 * \param clusters the resultant clusters containing point indices (as a vector of PointIndices) 00074 * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) 00075 * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) 00076 * \ingroup segmentation 00077 */ 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 00084 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00085 /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal 00086 * angular deviation 00087 * \param cloud the point cloud message 00088 * \param normals the point cloud message containing normal information 00089 * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching 00090 * \note the tree has to be created as a spatial locator on \a cloud 00091 * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space 00092 * \param clusters the resultant clusters containing point indices (as a vector of PointIndices) 00093 * \param eps_angle the maximum allowed difference between normals in radians for cluster/region growing 00094 * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) 00095 * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) 00096 * \ingroup segmentation 00097 */ 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 00182 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00183 /** \brief Decompose a region of space into clusters based on the euclidean distance between points, and the normal 00184 * angular deviation 00185 * \param cloud the point cloud message 00186 * \param normals the point cloud message containing normal information 00187 * \param indices a list of point indices to use from \a cloud 00188 * \param tree the spatial locator (e.g., kd-tree) used for nearest neighbors searching 00189 * \note the tree has to be created as a spatial locator on \a cloud 00190 * \param tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space 00191 * \param clusters the resultant clusters containing point indices (as PointIndices) 00192 * \param eps_angle the maximum allowed difference between normals in degrees for cluster/region growing 00193 * \param min_pts_per_cluster minimum number of points that a cluster may contain (default: 1) 00194 * \param max_pts_per_cluster maximum number of points that a cluster may contain (default: max int) 00195 * \ingroup segmentation 00196 */ 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 00287 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00288 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00289 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00290 /** \brief @b EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sense. 00291 * \author Radu Bogdan Rusu 00292 * \ingroup segmentation 00293 */ 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 00310 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00311 /** \brief Empty constructor. */ 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 00318 /** \brief Provide a pointer to the search object. 00319 * \param[in] tree a pointer to the spatial search object. 00320 */ 00321 inline void 00322 setSearchMethod (const KdTreePtr &tree) 00323 { 00324 tree_ = tree; 00325 } 00326 00327 /** \brief Get a pointer to the search method used. 00328 * @todo fix this for a generic search tree 00329 */ 00330 inline KdTreePtr 00331 getSearchMethod () const 00332 { 00333 return (tree_); 00334 } 00335 00336 /** \brief Set the spatial cluster tolerance as a measure in the L2 Euclidean space 00337 * \param[in] tolerance the spatial cluster tolerance as a measure in the L2 Euclidean space 00338 */ 00339 inline void 00340 setClusterTolerance (double tolerance) 00341 { 00342 cluster_tolerance_ = tolerance; 00343 } 00344 00345 /** \brief Get the spatial cluster tolerance as a measure in the L2 Euclidean space. */ 00346 inline double 00347 getClusterTolerance () const 00348 { 00349 return (cluster_tolerance_); 00350 } 00351 00352 /** \brief Set the minimum number of points that a cluster needs to contain in order to be considered valid. 00353 * \param[in] min_cluster_size the minimum cluster size 00354 */ 00355 inline void 00356 setMinClusterSize (int min_cluster_size) 00357 { 00358 min_pts_per_cluster_ = min_cluster_size; 00359 } 00360 00361 /** \brief Get the minimum number of points that a cluster needs to contain in order to be considered valid. */ 00362 inline int 00363 getMinClusterSize () const 00364 { 00365 return (min_pts_per_cluster_); 00366 } 00367 00368 /** \brief Set the maximum number of points that a cluster needs to contain in order to be considered valid. 00369 * \param[in] max_cluster_size the maximum cluster size 00370 */ 00371 inline void 00372 setMaxClusterSize (int max_cluster_size) 00373 { 00374 max_pts_per_cluster_ = max_cluster_size; 00375 } 00376 00377 /** \brief Get the maximum number of points that a cluster needs to contain in order to be considered valid. */ 00378 inline int 00379 getMaxClusterSize () const 00380 { 00381 return (max_pts_per_cluster_); 00382 } 00383 00384 /** \brief Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()> 00385 * \param[out] clusters the resultant point clusters 00386 */ 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 00397 /** \brief A pointer to the spatial search object. */ 00398 KdTreePtr tree_; 00399 00400 /** \brief The spatial cluster tolerance as a measure in the L2 Euclidean space. */ 00401 double cluster_tolerance_; 00402 00403 /** \brief The minimum number of points that a cluster needs to contain in order to be considered valid (default = 1). */ 00404 int min_pts_per_cluster_; 00405 00406 /** \brief The maximum number of points that a cluster needs to contain in order to be considered valid (default = MAXINT). */ 00407 int max_pts_per_cluster_; 00408 00409 /** \brief Class getName method. */ 00410 virtual std::string getClassName () const { return ("EuclideanClusterExtraction"); } 00411 00412 }; 00413 00414 /** \brief Sort clusters method (for std::sort). 00415 * \ingroup segmentation 00416 */ 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_