Point Cloud Library (PCL)
1.7.0
|
00001 00002 /* 00003 * Software License Agreement (BSD License) 00004 * 00005 * Point Cloud Library (PCL) - www.pointclouds.org 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 Willow Garage, Inc. 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 * Author : jpapon@gmail.com 00037 * Email : jpapon@gmail.com 00038 * 00039 */ 00040 00041 #ifndef PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_H_ 00042 #define PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_H_ 00043 00044 #include <pcl/features/normal_3d.h> 00045 #include <pcl/pcl_base.h> 00046 #include <pcl/point_cloud.h> 00047 #include <pcl/point_types.h> 00048 #include <pcl/octree/octree.h> 00049 #include <pcl/octree/octree_pointcloud_adjacency.h> 00050 #include <pcl/search/search.h> 00051 #include <pcl/segmentation/boost.h> 00052 00053 00054 00055 //DEBUG TODO REMOVE 00056 #include <pcl/common/time.h> 00057 00058 00059 namespace pcl 00060 { 00061 /** \brief Supervoxel container class - stores a cluster extracted using supervoxel clustering 00062 */ 00063 template <typename PointT> 00064 class Supervoxel 00065 { 00066 public: 00067 Supervoxel () : 00068 voxels_ (new pcl::PointCloud<PointT> ()), 00069 normals_ (new pcl::PointCloud<Normal> ()) 00070 { } 00071 00072 typedef boost::shared_ptr<Supervoxel<PointT> > Ptr; 00073 typedef boost::shared_ptr<const Supervoxel<PointT> > ConstPtr; 00074 00075 /** \brief Gets the centroid of the supervoxel 00076 * \param[out] centroid_arg centroid of the supervoxel 00077 */ 00078 void 00079 getCentroidPoint (PointXYZRGBA ¢roid_arg) 00080 { 00081 centroid_arg = centroid_; 00082 } 00083 00084 /** \brief Gets the point normal for the supervoxel 00085 * \param[out] normal_arg Point normal of the supervoxel 00086 * \note This isn't an average, it is a normal computed using all of the voxels in the supervoxel as support 00087 */ 00088 void 00089 getCentroidPointNormal (PointNormal &normal_arg) 00090 { 00091 normal_arg.x = centroid_.x; 00092 normal_arg.y = centroid_.y; 00093 normal_arg.z = centroid_.z; 00094 normal_arg.normal_x = normal_.normal_x; 00095 normal_arg.normal_y = normal_.normal_y; 00096 normal_arg.normal_z = normal_.normal_z; 00097 normal_arg.curvature = normal_.curvature; 00098 } 00099 00100 /** \brief The normal calculated for the voxels contained in the supervoxel */ 00101 pcl::Normal normal_; 00102 /** \brief The centroid of the supervoxel - average voxel */ 00103 pcl::PointXYZRGBA centroid_; 00104 /** \brief A Pointcloud of the voxels in the supervoxel */ 00105 typename pcl::PointCloud<PointT>::Ptr voxels_; 00106 /** \brief A Pointcloud of the normals for the points in the supervoxel */ 00107 typename pcl::PointCloud<Normal>::Ptr normals_; 00108 }; 00109 00110 /** \brief Implements a supervoxel algorithm based on voxel structure, normals, and rgb values 00111 * \note Supervoxels are oversegmented volumetric patches (usually surfaces) 00112 * \note Usually, color isn't needed (and can be detrimental)- spatial structure is mainly used 00113 * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter 00114 * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds 00115 * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013 00116 * \author Jeremie Papon (jpapon@gmail.com) 00117 */ 00118 template <typename PointT> 00119 class PCL_EXPORTS SupervoxelClustering : public pcl::PCLBase<PointT> 00120 { 00121 //Forward declaration of friended helper class 00122 class SupervoxelHelper; 00123 friend class SupervoxelHelper; 00124 #define MAX_LABEL 16384 00125 public: 00126 /** \brief VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContainer 00127 * \note It stores xyz, rgb, normal, distance, an index, and an owner. 00128 */ 00129 class VoxelData 00130 { 00131 public: 00132 VoxelData (): 00133 xyz_ (0.0f, 0.0f, 0.0f), 00134 rgb_ (0.0f, 0.0f, 0.0f), 00135 normal_ (0.0f, 0.0f, 0.0f, 0.0f), 00136 curvature_ (0.0f), 00137 owner_ (0) 00138 {} 00139 00140 /** \brief Gets the data of in the form of a point 00141 * \param[out] point_arg Will contain the point value of the voxeldata 00142 */ 00143 void 00144 getPoint (PointT &point_arg) const; 00145 00146 /** \brief Gets the data of in the form of a normal 00147 * \param[out] normal_arg Will contain the normal value of the voxeldata 00148 */ 00149 void 00150 getNormal (Normal &normal_arg) const; 00151 00152 Eigen::Vector3f xyz_; 00153 Eigen::Vector3f rgb_; 00154 Eigen::Vector4f normal_; 00155 float curvature_; 00156 float distance_; 00157 int idx_; 00158 SupervoxelHelper* owner_; 00159 }; 00160 00161 typedef pcl::octree::OctreePointCloudAdjacencyContainer<PointT, VoxelData> LeafContainerT; 00162 typedef std::vector <LeafContainerT*> LeafVectorT; 00163 00164 typedef typename pcl::PointCloud<PointT> PointCloudT; 00165 typedef typename pcl::PointCloud<Normal> NormalCloudT; 00166 typedef typename pcl::octree::OctreePointCloudAdjacency<PointT, LeafContainerT> OctreeAdjacencyT; 00167 typedef typename pcl::octree::OctreePointCloudSearch <PointT> OctreeSearchT; 00168 typedef typename pcl::search::KdTree<PointT> KdTreeT; 00169 typedef boost::shared_ptr<std::vector<int> > IndicesPtr; 00170 00171 using PCLBase <PointT>::initCompute; 00172 using PCLBase <PointT>::deinitCompute; 00173 using PCLBase <PointT>::input_; 00174 00175 typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, uint32_t, float> VoxelAdjacencyList; 00176 typedef VoxelAdjacencyList::vertex_descriptor VoxelID; 00177 typedef VoxelAdjacencyList::edge_descriptor EdgeID; 00178 00179 00180 public: 00181 00182 /** \brief Constructor that sets default values for member variables. 00183 * \param[in] voxel_resolution The resolution (in meters) of voxels used 00184 * \param[in] seed_resolution The average size (in meters) of resulting supervoxels 00185 * \param[in] use_single_camera_transform Set to true if point density in cloud falls off with distance from origin (such as with a cloud coming from one stationary camera), set false if input cloud is from multiple captures from multiple locations. 00186 */ 00187 SupervoxelClustering (float voxel_resolution, float seed_resolution, bool use_single_camera_transform = true); 00188 00189 /** \brief This destructor destroys the cloud, normals and search method used for 00190 * finding neighbors. In other words it frees memory. 00191 */ 00192 virtual 00193 ~SupervoxelClustering (); 00194 00195 /** \brief Set the resolution of the octree voxels */ 00196 void 00197 setVoxelResolution (float resolution); 00198 00199 /** \brief Get the resolution of the octree voxels */ 00200 float 00201 getVoxelResolution () const; 00202 00203 /** \brief Set the resolution of the octree seed voxels */ 00204 void 00205 setSeedResolution (float seed_resolution); 00206 00207 /** \brief Get the resolution of the octree seed voxels */ 00208 float 00209 getSeedResolution () const; 00210 00211 /** \brief Set the importance of color for supervoxels */ 00212 void 00213 setColorImportance (float val); 00214 00215 /** \brief Set the importance of spatial distance for supervoxels */ 00216 void 00217 setSpatialImportance (float val); 00218 00219 /** \brief Set the importance of scalar normal product for supervoxels */ 00220 void 00221 setNormalImportance (float val); 00222 00223 /** \brief This method launches the segmentation algorithm and returns the supervoxels that were 00224 * obtained during the segmentation. 00225 * \param[out] supervoxel_clusters A map of labels to pointers to supervoxel structures 00226 */ 00227 virtual void 00228 extract (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters); 00229 00230 /** \brief This method sets the cloud to be supervoxelized 00231 * \param[in] cloud The cloud to be supervoxelize 00232 */ 00233 virtual void 00234 setInputCloud (typename pcl::PointCloud<PointT>::ConstPtr cloud); 00235 00236 /** \brief This method refines the calculated supervoxels - may only be called after extract 00237 * \param[in] num_itr The number of iterations of refinement to be done (2 or 3 is usually sufficient) 00238 * \param[out] supervoxel_clusters The resulting refined supervoxels 00239 */ 00240 virtual void 00241 refineSupervoxels (int num_itr, std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters); 00242 00243 //////////////////////////////////////////////////////////// 00244 /** \brief Returns an RGB colorized cloud showing superpixels 00245 * Otherwise it returns an empty pointer. 00246 * Points that belong to the same supervoxel have the same color. 00247 * But this function doesn't guarantee that different segments will have different 00248 * color(it's random). Points that are unlabeled will be black 00249 */ 00250 typename pcl::PointCloud<PointXYZRGBA>::Ptr 00251 getColoredCloud () const; 00252 00253 /** \brief Returns a deep copy of the voxel centroid cloud */ 00254 typename pcl::PointCloud<PointT>::Ptr 00255 getVoxelCentroidCloud () const; 00256 00257 /** \brief Returns labeled cloud 00258 * Points that belong to the same supervoxel have the same label. 00259 * Labels for segments start from 1, unlabled points have label 0 00260 */ 00261 typename pcl::PointCloud<PointXYZL>::Ptr 00262 getLabeledCloud () const; 00263 00264 /** \brief Returns an RGB colorized voxelized cloud showing superpixels 00265 * Otherwise it returns an empty pointer. 00266 * Points that belong to the same supervoxel have the same color. 00267 * But this function doesn't guarantee that different segments will have different 00268 * color(it's random). Points that are unlabeled will be black 00269 */ 00270 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr 00271 getColoredVoxelCloud () const; 00272 00273 /** \brief Returns labeled voxelized cloud 00274 * Points that belong to the same supervoxel have the same label. 00275 * Labels for segments start from 1, unlabled points have label 0 00276 */ 00277 pcl::PointCloud<pcl::PointXYZL>::Ptr 00278 getLabeledVoxelCloud () const; 00279 00280 /** \brief Gets the adjacency list (Boost Graph library) which gives connections between supervoxels 00281 * \param[out] adjacency_list_arg BGL graph where supervoxel labels are vertices, edges are touching relationships 00282 */ 00283 void 00284 getSupervoxelAdjacencyList (VoxelAdjacencyList &adjacency_list_arg) const; 00285 00286 /** \brief Get a multimap which gives supervoxel adjacency 00287 * \param[out] label_adjacency Multi-Map which maps a supervoxel label to all adjacent supervoxel labels 00288 */ 00289 void 00290 getSupervoxelAdjacency (std::multimap<uint32_t, uint32_t> &label_adjacency) const; 00291 00292 /** \brief Static helper function which returns a pointcloud of normals for the input supervoxels 00293 * \param[in] supervoxel_clusters Supervoxel cluster map coming from this class 00294 * \returns Cloud of PointNormals of the supervoxels 00295 * 00296 */ 00297 static pcl::PointCloud<pcl::PointNormal>::Ptr 00298 makeSupervoxelNormalCloud (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters); 00299 private: 00300 00301 /** \brief This method simply checks if it is possible to execute the segmentation algorithm with 00302 * the current settings. If it is possible then it returns true. 00303 */ 00304 virtual bool 00305 prepareForSegmentation (); 00306 00307 /** \brief This selects points to use as initial supervoxel centroids 00308 * \param[out] seed_points The selected points 00309 */ 00310 void 00311 selectInitialSupervoxelSeeds (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points); 00312 00313 /** \brief This method creates the internal supervoxel helpers based on the provided seed points 00314 * \param[in] seed_points The selected points 00315 */ 00316 void 00317 createSupervoxelHelpers (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points); 00318 00319 /** \brief This performs the superpixel evolution */ 00320 void 00321 expandSupervoxels (int depth); 00322 00323 /** \brief This sets the data of the voxels in the tree */ 00324 void 00325 computeVoxelData (); 00326 00327 /** \brief Reseeds the supervoxels by finding the voxel closest to current centroid */ 00328 void 00329 reseedSupervoxels (); 00330 00331 /** \brief Constructs the map of supervoxel clusters from the internal supervoxel helpers */ 00332 void 00333 makeSupervoxels (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters); 00334 00335 /** \brief Stores the resolution used in the octree */ 00336 float resolution_; 00337 00338 /** \brief Stores the resolution used to seed the superpixels */ 00339 float seed_resolution_; 00340 00341 /** \brief Distance function used for comparing voxelDatas */ 00342 float 00343 voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const; 00344 00345 /** \brief Transform function used to normalize voxel density versus distance from camera */ 00346 void 00347 transformFunction (PointT &p); 00348 00349 /** \brief Contains a KDtree for the voxelized cloud */ 00350 typename pcl::search::KdTree<PointT>::Ptr voxel_kdtree_; 00351 00352 /** \brief Octree Adjacency structure with leaves at voxel resolution */ 00353 typename OctreeAdjacencyT::Ptr adjacency_octree_; 00354 00355 /** \brief Contains the Voxelized centroid Cloud */ 00356 typename PointCloudT::Ptr voxel_centroid_cloud_; 00357 00358 /** \brief Importance of color in clustering */ 00359 float color_importance_; 00360 /** \brief Importance of distance from seed center in clustering */ 00361 float spatial_importance_; 00362 /** \brief Importance of similarity in normals for clustering */ 00363 float normal_importance_; 00364 00365 /** \brief Stores the colors used for the superpixel labels*/ 00366 std::vector<uint32_t> label_colors_; 00367 00368 /** \brief Internal storage class for supervoxels 00369 * \note Stores pointers to leaves of clustering internal octree, 00370 * \note so should not be used outside of clustering class 00371 */ 00372 class SupervoxelHelper 00373 { 00374 public: 00375 SupervoxelHelper (uint32_t label, SupervoxelClustering* parent_arg): 00376 label_ (label), 00377 parent_ (parent_arg) 00378 { } 00379 00380 void 00381 addLeaf (LeafContainerT* leaf_arg); 00382 00383 void 00384 removeLeaf (LeafContainerT* leaf_arg); 00385 00386 void 00387 removeAllLeaves (); 00388 00389 void 00390 expand (); 00391 00392 void 00393 refineNormals (); 00394 00395 void 00396 updateCentroid (); 00397 00398 void 00399 getVoxels (typename pcl::PointCloud<PointT>::Ptr &voxels) const; 00400 00401 void 00402 getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const; 00403 00404 typedef float (SupervoxelClustering::*DistFuncPtr)(const VoxelData &v1, const VoxelData &v2); 00405 00406 uint32_t 00407 getLabel () const 00408 { return label_; } 00409 00410 Eigen::Vector4f 00411 getNormal () const 00412 { return centroid_.normal_; } 00413 00414 Eigen::Vector3f 00415 getRGB () const 00416 { return centroid_.rgb_; } 00417 00418 Eigen::Vector3f 00419 getXYZ () const 00420 { return centroid_.xyz_;} 00421 00422 void 00423 getXYZ (float &x, float &y, float &z) const 00424 { x=centroid_.xyz_[0]; y=centroid_.xyz_[1]; z=centroid_.xyz_[2]; } 00425 00426 void 00427 getRGB (uint32_t &rgba) const 00428 { 00429 rgba = static_cast<uint32_t>(centroid_.rgb_[0]) << 16 | 00430 static_cast<uint32_t>(centroid_.rgb_[1]) << 8 | 00431 static_cast<uint32_t>(centroid_.rgb_[2]); 00432 } 00433 00434 void 00435 getNormal (pcl::Normal &normal_arg) const 00436 { 00437 normal_arg.normal_x = centroid_.normal_[0]; 00438 normal_arg.normal_y = centroid_.normal_[1]; 00439 normal_arg.normal_z = centroid_.normal_[2]; 00440 normal_arg.curvature = centroid_.curvature_; 00441 } 00442 00443 void 00444 getNeighborLabels (std::set<uint32_t> &neighbor_labels) const; 00445 00446 VoxelData 00447 getCentroid () const 00448 { return centroid_; } 00449 00450 00451 size_t 00452 size () const { return leaves_.size (); } 00453 private: 00454 //Stores leaves 00455 std::set<LeafContainerT*> leaves_; 00456 uint32_t label_; 00457 VoxelData centroid_; 00458 SupervoxelClustering* parent_; 00459 00460 00461 }; 00462 00463 typedef boost::ptr_list<SupervoxelHelper> HelperListT; 00464 HelperListT supervoxel_helpers_; 00465 00466 //TODO DEBUG REMOVE 00467 StopWatch timer_; 00468 public: 00469 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00470 00471 00472 00473 }; 00474 00475 } 00476 00477 #ifdef PCL_NO_PRECOMPILE 00478 #include <pcl/segmentation/impl/supervoxel_clustering.hpp> 00479 #endif 00480 00481 #endif