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
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);};
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 
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, PointT, float > VoxelAdjacencyList
VoxelAdjacencyList::vertex_descriptor VoxelID
OctreePointCloudAdjacency(const double resolution_arg)
Constructor.
OctreeBase< LeafContainerT, BranchContainerT > OctreeBaseT
Octree pointcloud voxel class used for adjacency calculation
LeafNodeIterator leaf_begin(unsigned int maxDepth_arg=0)
VoxelAdjacencyList::edge_descriptor EdgeID
const OctreeLeafNodeIterator< OctreeAdjacencyT > ConstLeafNodeIterator
OctreeLeafNodeIterator< OctreeAdjacencyT > LeafNodeIterator
virtual ~OctreePointCloudAdjacency()
Empty class destructor.
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.
boost::shared_ptr< OctreeAdjacencyT > Ptr
Octree pointcloud class
boost::shared_ptr< const OctreeAdjacencyT > ConstPtr
void addPointsFromInputCloud()
Adds points from cloud to the octree.
Octree key class
Definition: octree_key.h:53
double resolution_
Octree resolution.
virtual void addPointIdx(const int pointIdx_arg)
Add point at index from input pointcloud dataset to octree.
OctreePointCloud< PointT, LeafContainerT, BranchContainerT, OctreeBaseT > OctreePointCloudT
OctreeT::BranchNode BranchNode
Octree class.
Definition: octree_base.h:63
OctreePointCloudAdjacency< PointT, LeafContainerT, BranchContainerT > OctreeAdjacencyT
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generates octree key for specified point (uses transform if provided)
Abstract octree iterator class
Simple stopwatch.
Definition: time.h:62
const OctreeDepthFirstIterator< OctreeAdjacencyT > ConstIterator
Octree leaf node iterator class.
void computeVoxelAdjacencyGraph(VoxelAdjacencyList &voxel_adjacency_graph)
Computes an adjacency graph of voxel relations.
void computeNeighbors(OctreeKey &key_arg, LeafContainerT *leaf_container)
Fills in the neighbors fields for new voxels.
LeafContainerT * getLeafContainerAtPoint(const PointT &point_arg) const
Gets the leaf container for a given point.
A point structure representing Euclidean xyz coordinates.
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...
OctreeDepthFirstIterator< OctreeAdjacencyT > Iterator
A point structure representing Euclidean xyz coordinates, and the RGB color.
Iterator depth_begin(unsigned int maxDepth_arg=0)
PointCloudConstPtr input_
Pointer to input point cloud dataset.