octree_pointcloud_adjacency.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2012, Jeremie Papon
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *  Author : jpapon@gmail.com
 *  Email  : jpapon@gmail.com
 */

#ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_H_
#define PCL_OCTREE_POINTCLOUD_ADJACENCY_H_

#include <pcl/common/geometry.h>
#include <pcl/octree/boost.h>
#include <pcl/octree/octree_base.h>
#include <pcl/octree/octree_iterator.h>
#include <pcl/octree/octree_pointcloud.h>
#include <pcl/octree/octree_pointcloud_adjacency_container.h>

#include <set>
#include <list>

//DEBUG TODO REMOVE
#include <pcl/common/time.h>



namespace pcl
{ 
  
  namespace octree
  {


    template< typename PointT, 
    typename LeafContainerT = OctreePointCloudAdjacencyContainer <PointT>,    
    typename BranchContainerT = OctreeContainerEmpty >
    class OctreePointCloudAdjacency : public OctreePointCloud< PointT, LeafContainerT, BranchContainerT>
    
    {
      
    public:
      typedef OctreeBase<LeafContainerT, BranchContainerT> OctreeBaseT;
      
      typedef OctreePointCloudAdjacency<PointT, LeafContainerT, BranchContainerT > OctreeAdjacencyT;
      typedef boost::shared_ptr<OctreeAdjacencyT> Ptr;
      typedef boost::shared_ptr<const OctreeAdjacencyT> ConstPtr;
      
      typedef OctreePointCloud<PointT, LeafContainerT, BranchContainerT,OctreeBaseT > OctreePointCloudT;
      typedef typename OctreePointCloudT::LeafNode LeafNode;
      typedef typename OctreePointCloudT::BranchNode BranchNode;
      
      typedef pcl::PointCloud<PointT> CloudT;
      
      //So we can access input
      using OctreePointCloudT::input_;
      using OctreePointCloudT::resolution_;
      
      // iterators are friends
      friend class OctreeIteratorBase<OctreeAdjacencyT> ;
      friend class OctreeDepthFirstIterator<OctreeAdjacencyT> ;
      friend class OctreeBreadthFirstIterator<OctreeAdjacencyT> ;
      friend class OctreeLeafNodeIterator<OctreeAdjacencyT> ;
      
      // Octree default iterators
      typedef OctreeDepthFirstIterator<OctreeAdjacencyT> Iterator;
      typedef const OctreeDepthFirstIterator<OctreeAdjacencyT> ConstIterator;
      Iterator depth_begin(unsigned int maxDepth_arg = 0) {return Iterator(this, maxDepth_arg);};
      const Iterator depth_end() {return Iterator();};
      
      // Octree leaf node iterators
      typedef OctreeLeafNodeIterator<OctreeAdjacencyT> LeafNodeIterator;
      typedef const OctreeLeafNodeIterator< OctreeAdjacencyT> ConstLeafNodeIterator;
      LeafNodeIterator leaf_begin(unsigned int maxDepth_arg = 0) {return LeafNodeIterator(this, maxDepth_arg);};
      const LeafNodeIterator leaf_end() {return LeafNodeIterator();};
           
      typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, PointT, float> VoxelAdjacencyList;
      typedef typename VoxelAdjacencyList::vertex_descriptor VoxelID;
      typedef typename VoxelAdjacencyList::edge_descriptor EdgeID;
      
      //Leaf vector - pointers to all leaves
      typedef std::vector <LeafContainerT*> LeafVectorT;
      //Fast leaf iterators that don't require traversing tree
      typedef typename LeafVectorT::iterator iterator;
      typedef typename LeafVectorT::const_iterator const_iterator;
      inline iterator begin () { return (leaf_vector_.begin ()); }
      inline iterator end ()   { return (leaf_vector_.end ()); }
      //size of neighbors
      inline size_t size () const { return leaf_vector_.size (); }
      
      
      OctreePointCloudAdjacency (const double resolution_arg);
      
      
      virtual ~OctreePointCloudAdjacency ()
      {
      }
      
      void 
      addPointsFromInputCloud ();
      
      LeafContainerT *
      getLeafContainerAtPoint (const PointT& point_arg) const;

      void
      computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph);
      
      void 
      setTransformFunction (boost::function<void (PointT &p)> transform_func)
      {
        transform_func_ = transform_func;
      }
        
      bool
      testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos = PointXYZ (0,0,0));
        
    protected:
      
       virtual void                         
       addPointIdx (const int pointIdx_arg);
       
      void
      computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container);

      void
      genOctreeKeyforPoint (const PointT& point_arg,OctreeKey & key_arg) const;
      
          private:

      StopWatch timer_;

      //Local leaf pointer vector used to make iterating through leaves fast
      LeafVectorT leaf_vector_;

      boost::function<void (PointT &p)> transform_func_;
    };
    
  }  
  
}









//#ifdef PCL_NO_PRECOMPILE
#include <pcl/octree/impl/octree_pointcloud_adjacency.hpp>
//#endif

#endif //PCL_OCTREE_POINTCLOUD_SUPER_VOXEL_H_