Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/octree/include/pcl/octree/octree_pointcloud_adjacency.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2012, Jeremie Papon
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 #ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_H_
00041 #define PCL_OCTREE_POINTCLOUD_ADJACENCY_H_
00042 
00043 #include <pcl/common/geometry.h>
00044 #include <pcl/octree/boost.h>
00045 #include <pcl/octree/octree_base.h>
00046 #include <pcl/octree/octree_iterator.h>
00047 #include <pcl/octree/octree_pointcloud.h>
00048 #include <pcl/octree/octree_pointcloud_adjacency_container.h>
00049 
00050 #include <set>
00051 #include <list>
00052 
00053 //DEBUG TODO REMOVE
00054 #include <pcl/common/time.h>
00055 
00056 
00057 
00058 namespace pcl
00059 { 
00060   
00061   namespace octree
00062   {
00063     //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00064     /** \brief @b Octree pointcloud voxel class used for adjacency calculation 
00065      *  \note This pointcloud octree class generate an octree from a point cloud (zero-copy). 
00066      *  \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
00067      *  \note This class maintains adjacency information for its voxels 
00068      *  \note The OctreePointCloudAdjacencyContainer can be used to store data in leaf nodes
00069      *  \note An optional transform function can be provided which changes how the voxel grid is computed - this can be used to, for example, make voxel bins larger as they increase in distance from the origin (camera)
00070      *  \note See SupervoxelClustering for an example of how to provide a transform function 
00071      *  \note If used in academic work, please cite:
00072      * 
00073      * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
00074      *   Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
00075      *   In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013 
00076      *  \ingroup octree
00077      *  \author Jeremie Papon (jpapon@gmail.com)
00078      */
00079     //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00080     template< typename PointT, 
00081     typename LeafContainerT = OctreePointCloudAdjacencyContainer <PointT>,    
00082     typename BranchContainerT = OctreeContainerEmpty >
00083     class OctreePointCloudAdjacency : public OctreePointCloud< PointT, LeafContainerT, BranchContainerT>
00084     
00085     {
00086       
00087     public:
00088       typedef OctreeBase<LeafContainerT, BranchContainerT> OctreeBaseT;
00089       
00090       typedef OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT > OctreeAdjacencyT;
00091       typedef boost::shared_ptr<OctreeAdjacencyT> Ptr;
00092       typedef boost::shared_ptr<const OctreeAdjacencyT> ConstPtr;
00093       
00094       typedef OctreePointCloud<PointT, LeafContainerT, BranchContainerT,OctreeBaseT > OctreePointCloudT;
00095       typedef typename OctreePointCloudT::LeafNode LeafNode;
00096       typedef typename OctreePointCloudT::BranchNode BranchNode;
00097       
00098       typedef pcl::PointCloud<PointT> CloudT;
00099       
00100       //So we can access input
00101       using OctreePointCloudT::input_;
00102       using OctreePointCloudT::resolution_;
00103       
00104       // iterators are friends
00105       friend class OctreeIteratorBase<OctreeAdjacencyT> ;
00106       friend class OctreeDepthFirstIterator<OctreeAdjacencyT> ;
00107       friend class OctreeBreadthFirstIterator<OctreeAdjacencyT> ;
00108       friend class OctreeLeafNodeIterator<OctreeAdjacencyT> ;
00109       
00110       // Octree default iterators
00111       typedef OctreeDepthFirstIterator<OctreeAdjacencyT> Iterator;
00112       typedef const OctreeDepthFirstIterator<OctreeAdjacencyT> ConstIterator;
00113       Iterator depth_begin(unsigned int maxDepth_arg = 0) {return Iterator(this, maxDepth_arg);};
00114       const Iterator depth_end() {return Iterator();};
00115       
00116       // Octree leaf node iterators
00117       typedef OctreeLeafNodeIterator<OctreeAdjacencyT> LeafNodeIterator;
00118       typedef const OctreeLeafNodeIterator< OctreeAdjacencyT> ConstLeafNodeIterator;
00119       LeafNodeIterator leaf_begin(unsigned int maxDepth_arg = 0) {return LeafNodeIterator(this, maxDepth_arg);};
00120       const LeafNodeIterator leaf_end() {return LeafNodeIterator();};
00121            
00122       typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, PointT, float> VoxelAdjacencyList;
00123       typedef typename VoxelAdjacencyList::vertex_descriptor VoxelID;
00124       typedef typename VoxelAdjacencyList::edge_descriptor EdgeID;
00125       
00126       //Leaf vector - pointers to all leaves
00127       typedef std::vector <LeafContainerT*> LeafVectorT;
00128       //Fast leaf iterators that don't require traversing tree
00129       typedef typename LeafVectorT::iterator iterator;
00130       typedef typename LeafVectorT::const_iterator const_iterator;
00131       inline iterator begin () { return (leaf_vector_.begin ()); }
00132       inline iterator end ()   { return (leaf_vector_.end ()); }
00133       //size of neighbors
00134       inline size_t size () const { return leaf_vector_.size (); }
00135       
00136       
00137       /** \brief Constructor.
00138        *  \param[in] resolution_arg  octree resolution at lowest octree level (voxel size)
00139       **/
00140       OctreePointCloudAdjacency (const double resolution_arg);
00141       
00142       
00143       /** \brief Empty class destructor. */
00144       virtual ~OctreePointCloudAdjacency ()
00145       {
00146       }
00147       
00148       /** \brief Adds points from cloud to the octree  
00149        *  \note This overrides the addPointsFromInputCloud from the OctreePointCloud class
00150        */
00151       void 
00152       addPointsFromInputCloud ();
00153       
00154       /** \brief Gets the leaf container for a given point 
00155        *  \param[in] point_arg Point to search for
00156        *  \returns Pointer to the leaf container - null if no leaf container found
00157        */
00158       LeafContainerT *
00159       getLeafContainerAtPoint (const PointT& point_arg) const;
00160 
00161       /** \brief Computes an adjacency graph of voxel relations 
00162        * \note WARNING: This slows down rapidly as cloud size increases due number of edges 
00163        * \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel touching relationships - vertices are pointT, edges represent touching, and edge lengths are the distance between the points
00164        */
00165       void
00166       computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph);
00167       
00168       /** \brief Sets a point transform (and inverse) used to transform the space of the input cloud
00169        *  This is useful for changing how adjacency is calculated - such as relaxing
00170        *  the adjacency criterion for points further from the camera
00171        *  \param[in] transform_func A boost:function pointer to the transform to be used. The transform must have one parameter (a point) which it modifies in place
00172        */
00173       void 
00174       setTransformFunction (boost::function<void (PointT &p)> transform_func)
00175       {
00176         transform_func_ = transform_func;
00177       }
00178         
00179       /** \brief Tests whether input point is occluded from specified camera point by other voxels
00180           \param[in] point_arg Point to test for
00181           \param[in] camera_pos Position of camera, defaults to origin
00182           \returns True if path to camera is blocked by a voxel, false otherwise
00183       */
00184       bool
00185       testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos = PointXYZ (0,0,0));
00186         
00187     protected:
00188       
00189       /** \brief Add point at index from input pointcloud dataset to octree
00190        * \param[in] pointIdx_arg the index representing the point in the dataset given by \a setInputCloud to be added
00191        * \note This virtual implementation allows the use of a transform function to compute keys
00192        */
00193        virtual void                         
00194        addPointIdx (const int pointIdx_arg);
00195        
00196       /** \brief Fills in the neighbors fields for new voxels 
00197        *  \param[in] key_arg Key of the voxel to check neighbors for
00198        *  \param[in] leaf_container Pointer to container of the leaf to check neighbors for
00199        */
00200       void
00201       computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container);
00202 
00203       /** \brief Generates octree key for specified point (uses transform if provided)
00204        *  \param[in] point_arg Point to generate key for
00205        *  \param[out] key_arg Resulting octree key
00206        */
00207       void
00208       genOctreeKeyforPoint (const PointT& point_arg,OctreeKey & key_arg) const;
00209       
00210           private:
00211 
00212       StopWatch timer_;
00213 
00214       //Local leaf pointer vector used to make iterating through leaves fast
00215       LeafVectorT leaf_vector_;
00216 
00217       boost::function<void (PointT &p)> transform_func_;
00218     };
00219     
00220   }  
00221   
00222 }
00223 
00224 
00225 
00226 
00227 
00228 
00229 
00230 
00231 
00232 //#ifdef PCL_NO_PRECOMPILE
00233 #include <pcl/octree/impl/octree_pointcloud_adjacency.hpp>
00234 //#endif
00235 
00236 #endif //PCL_OCTREE_POINTCLOUD_SUPER_VOXEL_H_
00237