Point Cloud Library (PCL)  1.7.1
octree_pointcloud_adjacency.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012, Jeremie Papon
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author : jpapon@gmail.com
37  * Email : jpapon@gmail.com
38  */
39 
40 #ifndef PCL_OCTREE_POINTCLOUD_ADJACENCY_H_
41 #define PCL_OCTREE_POINTCLOUD_ADJACENCY_H_
42 
43 #include <pcl/common/geometry.h>
44 #include <pcl/octree/boost.h>
45 #include <pcl/octree/octree_base.h>
46 #include <pcl/octree/octree_iterator.h>
47 #include <pcl/octree/octree_pointcloud.h>
48 #include <pcl/octree/octree_pointcloud_adjacency_container.h>
49 
50 #include <set>
51 #include <list>
52 
53 //DEBUG TODO REMOVE
54 #include <pcl/common/time.h>
55 
56 
57 
58 namespace pcl
59 {
60 
61  namespace octree
62  {
63  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
64  /** \brief @b Octree pointcloud voxel class used for adjacency calculation
65  * \note This pointcloud octree class generate an octree from a point cloud (zero-copy).
66  * \note The octree pointcloud is initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
67  * \note This class maintains adjacency information for its voxels
68  * \note The OctreePointCloudAdjacencyContainer can be used to store data in leaf nodes
69  * \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)
70  * \note See SupervoxelClustering for an example of how to provide a transform function
71  * \note If used in academic work, please cite:
72  *
73  * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
74  * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
75  * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013
76  * \ingroup octree
77  * \author Jeremie Papon (jpapon@gmail.com)
78  */
79  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
80  template< typename PointT,
81  typename LeafContainerT = OctreePointCloudAdjacencyContainer <PointT>,
82  typename BranchContainerT = OctreeContainerEmpty >
83  class OctreePointCloudAdjacency : public OctreePointCloud< PointT, LeafContainerT, BranchContainerT>
84 
85  {
86 
87  public:
89 
91  typedef boost::shared_ptr<OctreeAdjacencyT> Ptr;
92  typedef boost::shared_ptr<const OctreeAdjacencyT> ConstPtr;
93 
97 
99 
100  //So we can access input
103 
104  // iterators are friends
105  friend class OctreeIteratorBase<OctreeAdjacencyT> ;
106  friend class OctreeDepthFirstIterator<OctreeAdjacencyT> ;
107  friend class OctreeBreadthFirstIterator<OctreeAdjacencyT> ;
108  friend class OctreeLeafNodeIterator<OctreeAdjacencyT> ;
109 
110  // Octree default iterators
113  Iterator depth_begin(unsigned int maxDepth_arg = 0) {return Iterator(this, maxDepth_arg);};
114  const Iterator depth_end() {return Iterator();};
115 
116  // Octree leaf node iterators
119  LeafNodeIterator leaf_begin(unsigned int maxDepth_arg = 0) {return LeafNodeIterator(this, maxDepth_arg);};
120  const LeafNodeIterator leaf_end() {return LeafNodeIterator();};
121 
122  typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, PointT, float> VoxelAdjacencyList;
123  typedef typename VoxelAdjacencyList::vertex_descriptor VoxelID;
124  typedef typename VoxelAdjacencyList::edge_descriptor EdgeID;
125 
126  //Leaf vector - pointers to all leaves
127  typedef std::vector <LeafContainerT*> LeafVectorT;
128  //Fast leaf iterators that don't require traversing tree
129  typedef typename LeafVectorT::iterator iterator;
130  typedef typename LeafVectorT::const_iterator const_iterator;
131  inline iterator begin () { return (leaf_vector_.begin ()); }
132  inline iterator end () { return (leaf_vector_.end ()); }
133  //size of neighbors
134  inline size_t size () const { return leaf_vector_.size (); }
135 
136 
137  /** \brief Constructor.
138  * \param[in] resolution_arg octree resolution at lowest octree level (voxel size)
139  **/
140  OctreePointCloudAdjacency (const double resolution_arg);
141 
142 
143  /** \brief Empty class destructor. */
145  {
146  }
147 
148  /** \brief Adds points from cloud to the octree
149  * \note This overrides the addPointsFromInputCloud from the OctreePointCloud class
150  */
151  void
153 
154  /** \brief Gets the leaf container for a given point
155  * \param[in] point_arg Point to search for
156  * \returns Pointer to the leaf container - null if no leaf container found
157  */
158  LeafContainerT *
159  getLeafContainerAtPoint (const PointT& point_arg) const;
160 
161  /** \brief Computes an adjacency graph of voxel relations
162  * \note WARNING: This slows down rapidly as cloud size increases due number of edges
163  * \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
164  */
165  void
166  computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph);
167 
168  /** \brief Sets a point transform (and inverse) used to transform the space of the input cloud
169  * This is useful for changing how adjacency is calculated - such as relaxing
170  * the adjacency criterion for points further from the camera
171  * \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
172  */
173  void
174  setTransformFunction (boost::function<void (PointT &p)> transform_func)
175  {
176  transform_func_ = transform_func;
177  }
178 
179  /** \brief Tests whether input point is occluded from specified camera point by other voxels
180  \param[in] point_arg Point to test for
181  \param[in] camera_pos Position of camera, defaults to origin
182  \returns True if path to camera is blocked by a voxel, false otherwise
183  */
184  bool
185  testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos = PointXYZ (0,0,0));
186 
187  protected:
188 
189  /** \brief Add point at index from input pointcloud dataset to octree
190  * \param[in] pointIdx_arg the index representing the point in the dataset given by \a setInputCloud to be added
191  * \note This virtual implementation allows the use of a transform function to compute keys
192  */
193  virtual void
194  addPointIdx (const int pointIdx_arg);
195 
196  /** \brief Fills in the neighbors fields for new voxels
197  * \param[in] key_arg Key of the voxel to check neighbors for
198  * \param[in] leaf_container Pointer to container of the leaf to check neighbors for
199  */
200  void
201  computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container);
202 
203  /** \brief Generates octree key for specified point (uses transform if provided)
204  * \param[in] point_arg Point to generate key for
205  * \param[out] key_arg Resulting octree key
206  */
207  void
208  genOctreeKeyforPoint (const PointT& point_arg,OctreeKey & key_arg) const;
209 
210  private:
211 
212  StopWatch timer_;
213 
214  //Local leaf pointer vector used to make iterating through leaves fast
215  LeafVectorT leaf_vector_;
216 
217  boost::function<void (PointT &p)> transform_func_;
218  };
219 
220  }
221 
222 }
223 
224 
225 
226 
227 
228 
229 
230 
231 
232 //#ifdef PCL_NO_PRECOMPILE
233 #include <pcl/octree/impl/octree_pointcloud_adjacency.hpp>
234 //#endif
235 
236 #endif //PCL_OCTREE_POINTCLOUD_SUPER_VOXEL_H_
237 
virtual ~OctreePointCloudAdjacency()
Empty class destructor.
OctreeLeafNodeIterator< OctreeAdjacencyT > LeafNodeIterator
VoxelAdjacencyList::edge_descriptor EdgeID
OctreePointCloud< PointT, LeafContainerT, BranchContainerT, OctreeBaseT > OctreePointCloudT
VoxelAdjacencyList::vertex_descriptor VoxelID
Simple stopwatch.
Definition: time.h:62
Octree class.
Definition: octree_base.h:63
bool testForOcclusion(const PointT &point_arg, const PointXYZ &camera_pos=PointXYZ(0, 0, 0))
Tests whether input point is occluded from specified camera point by other voxels.
const OctreeLeafNodeIterator< OctreeAdjacencyT > ConstLeafNodeIterator
LeafContainerT * getLeafContainerAtPoint(const PointT &point_arg) const
Gets the leaf container for a given point.
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generates octree key for specified point (uses transform if provided)
virtual void addPointIdx(const int pointIdx_arg)
Add point at index from input pointcloud dataset to octree.
LeafNodeIterator leaf_begin(unsigned int maxDepth_arg=0)
Octree pointcloud voxel class used for adjacency calculation
double resolution_
Octree resolution.
OctreeDepthFirstIterator< OctreeAdjacencyT > Iterator
OctreeT::BranchNode BranchNode
void computeNeighbors(OctreeKey &key_arg, LeafContainerT *leaf_container)
Fills in the neighbors fields for new voxels.
OctreeBase< LeafContainerT, BranchContainerT > OctreeBaseT
Octree leaf node iterator class.
void setTransformFunction(boost::function< void(PointT &p)> transform_func)
Sets a point transform (and inverse) used to transform the space of the input cloud This is useful fo...
Octree key class
Definition: octree_key.h:53
PointCloudConstPtr input_
Pointer to input point cloud dataset.
void computeVoxelAdjacencyGraph(VoxelAdjacencyList &voxel_adjacency_graph)
Computes an adjacency graph of voxel relations.
Octree pointcloud class
boost::shared_ptr< OctreeAdjacencyT > Ptr
const OctreeDepthFirstIterator< OctreeAdjacencyT > ConstIterator
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, PointT, float > VoxelAdjacencyList
A point structure representing Euclidean xyz coordinates, and the RGB color.
OctreePointCloudAdjacency(const double resolution_arg)
Constructor.
boost::shared_ptr< const OctreeAdjacencyT > ConstPtr
OctreePointCloudAdjacency< PointT, LeafContainerT, BranchContainerT > OctreeAdjacencyT
A point structure representing Euclidean xyz coordinates.
void addPointsFromInputCloud()
Adds points from cloud to the octree.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Abstract octree iterator class
Iterator depth_begin(unsigned int maxDepth_arg=0)