Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/segmentation/include/pcl/segmentation/supervoxel_clustering.h
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 &centroid_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