Point Cloud Library (PCL)  1.8.0
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/console/print.h>
44 #include <pcl/common/geometry.h>
45 #include <pcl/octree/boost.h>
46 #include <pcl/octree/octree_impl.h>
47 #include <pcl/octree/octree_pointcloud_adjacency_container.h>
48 
49 #include <set>
50 #include <list>
51 
52 namespace pcl
53 {
54 
55  namespace octree
56  {
57  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
58  /** \brief @b Octree pointcloud voxel class which maintains adjacency information for its voxels.
59  *
60  * This pointcloud octree class generates an octree from a point cloud (zero-copy). The octree pointcloud is
61  * initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
62  *
63  * The OctreePointCloudAdjacencyContainer class can be used to store data in leaf nodes.
64  *
65  * An optional transform function can be provided which changes how the voxel grid is computed - this can be used to,
66  * for example, make voxel bins larger as they increase in distance from the origin (camera).
67  * \note See SupervoxelClustering for an example of how to provide a transform function.
68  *
69  * If used in academic work, please cite:
70  *
71  * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
72  * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
73  * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013
74  *
75  * \ingroup octree
76  * \author Jeremie Papon (jpapon@gmail.com) */
77  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
78  template <typename PointT,
79  typename LeafContainerT = OctreePointCloudAdjacencyContainer<PointT>,
80  typename BranchContainerT = OctreeContainerEmpty>
81  class OctreePointCloudAdjacency : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT>
82  {
83 
84  public:
85 
87 
89  typedef boost::shared_ptr<OctreeAdjacencyT> Ptr;
90  typedef boost::shared_ptr<const OctreeAdjacencyT> ConstPtr;
91 
95 
97  typedef boost::shared_ptr<PointCloud> PointCloudPtr;
98  typedef boost::shared_ptr<const PointCloud> PointCloudConstPtr;
99 
100  // Iterators are friends
101  friend class OctreeIteratorBase<OctreeAdjacencyT>;
102  friend class OctreeDepthFirstIterator<OctreeAdjacencyT>;
103  friend class OctreeBreadthFirstIterator<OctreeAdjacencyT>;
104  friend class OctreeLeafNodeIterator<OctreeAdjacencyT>;
105 
106  // Octree default iterators
109 
110  Iterator depth_begin (unsigned int max_depth_arg = 0) { return Iterator (this, max_depth_arg); }
111  const Iterator depth_end () { return Iterator (); }
112 
113  // Octree leaf node iterators
116 
117  LeafNodeIterator leaf_begin (unsigned int max_depth_arg = 0) { return LeafNodeIterator (this, max_depth_arg); }
118  const LeafNodeIterator leaf_end () { return LeafNodeIterator (); }
119 
120  // BGL graph
121  typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, PointT, float> VoxelAdjacencyList;
122  typedef typename VoxelAdjacencyList::vertex_descriptor VoxelID;
123  typedef typename VoxelAdjacencyList::edge_descriptor EdgeID;
124 
125  // Leaf vector - pointers to all leaves
126  typedef std::vector<LeafContainerT*> LeafVectorT;
127 
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 
132  inline iterator begin () { return (leaf_vector_.begin ()); }
133  inline iterator end () { return (leaf_vector_.end ()); }
134  inline LeafContainerT* at (size_t idx) { return leaf_vector_.at (idx); }
135 
136  // Size of neighbors
137  inline size_t size () const { return leaf_vector_.size (); }
138 
139  /** \brief Constructor.
140  *
141  * \param[in] resolution_arg Octree resolution at lowest octree level (voxel size) */
142  OctreePointCloudAdjacency (const double resolution_arg);
143 
144  /** \brief Empty class destructor. */
146  {
147  }
148 
149  /** \brief Adds points from cloud to the octree.
150  *
151  * \note This overrides addPointsFromInputCloud() from the OctreePointCloud class. */
152  void
154 
155  /** \brief Gets the leaf container for a given point.
156  *
157  * \param[in] point_arg Point to search for
158  *
159  * \returns Pointer to the leaf container - null if no leaf container found. */
160  LeafContainerT*
161  getLeafContainerAtPoint (const PointT& point_arg) const;
162 
163  /** \brief Computes an adjacency graph of voxel relations.
164  *
165  * \warning This slows down rapidly as cloud size increases due to the number of edges.
166  *
167  * \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel touching relationships.
168  * Vertices are PointT, edges represent touching, and edge lengths are the distance between the points. */
169  void
170  computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph);
171 
172  /** \brief Sets a point transform (and inverse) used to transform the space of the input cloud.
173  *
174  * This is useful for changing how adjacency is calculated - such as relaxing the adjacency criterion for
175  * points further from the camera.
176  *
177  * \param[in] transform_func A boost:function pointer to the transform to be used. The transform must have one
178  * parameter (a point) which it modifies in place. */
179  void
180  setTransformFunction (boost::function<void (PointT &p)> transform_func)
181  {
182  transform_func_ = transform_func;
183  }
184 
185  /** \brief Tests whether input point is occluded from specified camera point by other voxels.
186  *
187  * \param[in] point_arg Point to test for
188  * \param[in] camera_pos Position of camera, defaults to origin
189  *
190  * \returns True if path to camera is blocked by a voxel, false otherwise. */
191  bool
192  testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos = PointXYZ (0, 0, 0));
193 
194  protected:
195 
196  /** \brief Add point at index from input pointcloud dataset to octree.
197  *
198  * \param[in] point_idx_arg The index representing the point in the dataset given by setInputCloud() to be added
199  *
200  * \note This virtual implementation allows the use of a transform function to compute keys. */
201  virtual void
202  addPointIdx (const int point_idx_arg);
203 
204  /** \brief Fills in the neighbors fields for new voxels.
205  *
206  * \param[in] key_arg Key of the voxel to check neighbors for
207  * \param[in] leaf_container Pointer to container of the leaf to check neighbors for */
208  void
209  computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container);
210 
211  /** \brief Generates octree key for specified point (uses transform if provided).
212  *
213  * \param[in] point_arg Point to generate key for
214  * \param[out] key_arg Resulting octree key */
215  void
216  genOctreeKeyforPoint (const PointT& point_arg, OctreeKey& key_arg) const;
217 
218  private:
219 
220  /** \brief Add point at given index from input point cloud to octree.
221  *
222  * Index will be also added to indices vector. This functionality is not enabled for adjacency octree. */
224 
225  /** \brief Add point simultaneously to octree and input point cloud.
226  *
227  * This functionality is not enabled for adjacency octree. */
229 
238 
239  /// Local leaf pointer vector used to make iterating through leaves fast.
240  LeafVectorT leaf_vector_;
241 
242  boost::function<void (PointT &p)> transform_func_;
243 
244  };
245 
246  }
247 
248 }
249 
250 //#ifdef PCL_NO_PRECOMPILE
251 #include <pcl/octree/impl/octree_pointcloud_adjacency.hpp>
252 //#endif
253 
254 #endif // PCL_OCTREE_POINTCLOUD_ADJACENCY_H_
255 
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generates octree key for specified point (uses transform if provided).
Octree pointcloud class
virtual ~OctreePointCloudAdjacency()
Empty class destructor.
Octree class.
Definition: octree_base.h:63
OctreePointCloudAdjacency(const double resolution_arg)
Constructor.
void addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
Octree pointcloud voxel class which maintains adjacency information for its voxels.
Defines some geometrical functions and utility functions.
VoxelAdjacencyList::vertex_descriptor VoxelID
void setTransformFunction(boost::function< void(PointT &p)> transform_func)
Sets a point transform (and inverse) used to transform the space of the input cloud.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
OctreeLeafNodeIterator< OctreeAdjacencyT > LeafNodeIterator
Octree leaf node iterator class.
Iterator depth_begin(unsigned int max_depth_arg=0)
VoxelAdjacencyList::edge_descriptor EdgeID
PointCloudConstPtr input_
Pointer to input point cloud dataset.
const OctreeLeafNodeIterator< OctreeAdjacencyT > ConstLeafNodeIterator
virtual void addPointIdx(const int point_idx_arg)
Add point at index from input pointcloud dataset to octree.
void computeNeighbors(OctreeKey &key_arg, LeafContainerT *leaf_container)
Fills in the neighbors fields for new voxels.
A point structure representing Euclidean xyz coordinates.
LeafNodeIterator leaf_begin(unsigned int max_depth_arg=0)
OctreePointCloudAdjacency< PointT, LeafContainerT, BranchContainerT > OctreeAdjacencyT
LeafContainerT * getLeafContainerAtPoint(const PointT &point_arg) const
Gets the leaf container for a given point.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
OctreeBase< LeafContainerT, BranchContainerT > OctreeBaseT
Octree key class
Definition: octree_key.h:53
boost::shared_ptr< PointCloud > PointCloudPtr
void addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
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
boost::shared_ptr< const OctreeAdjacencyT > ConstPtr
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, PointT, float > VoxelAdjacencyList
Abstract octree iterator class
OctreeT::BranchNode BranchNode
void computeVoxelAdjacencyGraph(VoxelAdjacencyList &voxel_adjacency_graph)
Computes an adjacency graph of voxel relations.
A point structure representing Euclidean xyz coordinates, and the RGB color.
OctreePointCloud< PointT, LeafContainerT, BranchContainerT, OctreeBaseT > OctreePointCloudT
OctreeDepthFirstIterator< OctreeAdjacencyT > Iterator
double resolution_
Octree resolution.
void addPointsFromInputCloud()
Adds points from cloud to the octree.
const OctreeDepthFirstIterator< OctreeAdjacencyT > ConstIterator