Point Cloud Library (PCL)  1.9.1-dev
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 #pragma once
41 
42 #include <pcl/octree/boost.h>
43 #include <pcl/octree/octree_pointcloud.h>
44 #include <pcl/octree/octree_pointcloud_adjacency_container.h>
45 
46 #include <set>
47 #include <list>
48 
49 namespace pcl
50 {
51 
52  namespace octree
53  {
54  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
55  /** \brief @b Octree pointcloud voxel class which maintains adjacency information for its voxels.
56  *
57  * This pointcloud octree class generates an octree from a point cloud (zero-copy). The octree pointcloud is
58  * initialized with its voxel resolution. Its bounding box is automatically adjusted or can be predefined.
59  *
60  * The OctreePointCloudAdjacencyContainer class can be used to store data in leaf nodes.
61  *
62  * An optional transform function can be provided which changes how the voxel grid is computed - this can be used to,
63  * for example, make voxel bins larger as they increase in distance from the origin (camera).
64  * \note See SupervoxelClustering for an example of how to provide a transform function.
65  *
66  * If used in academic work, please cite:
67  *
68  * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
69  * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
70  * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013
71  *
72  * \ingroup octree
73  * \author Jeremie Papon (jpapon@gmail.com) */
74  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75  template <typename PointT,
76  typename LeafContainerT = OctreePointCloudAdjacencyContainer<PointT>,
77  typename BranchContainerT = OctreeContainerEmpty>
78  class OctreePointCloudAdjacency : public OctreePointCloud<PointT, LeafContainerT, BranchContainerT>
79  {
80 
81  public:
82 
84 
86  using Ptr = boost::shared_ptr<OctreeAdjacencyT>;
87  using ConstPtr = boost::shared_ptr<const OctreeAdjacencyT>;
88 
92 
94  using PointCloudPtr = boost::shared_ptr<PointCloud>;
95  using PointCloudConstPtr = boost::shared_ptr<const PointCloud>;
96 
97  // BGL graph
98  using VoxelAdjacencyList = boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, PointT, float>;
99  using VoxelID = typename VoxelAdjacencyList::vertex_descriptor;
100  using EdgeID = typename VoxelAdjacencyList::edge_descriptor;
101 
102  // Leaf vector - pointers to all leaves
103  using LeafVectorT = std::vector<LeafContainerT *>;
104 
105  // Fast leaf iterators that don't require traversing tree
106  using iterator = typename LeafVectorT::iterator;
107  using const_iterator = typename LeafVectorT::const_iterator;
108 
109  inline iterator begin () { return (leaf_vector_.begin ()); }
110  inline iterator end () { return (leaf_vector_.end ()); }
111  inline LeafContainerT* at (std::size_t idx) { return leaf_vector_.at (idx); }
112 
113  // Size of neighbors
114  inline std::size_t size () const { return leaf_vector_.size (); }
115 
116  /** \brief Constructor.
117  *
118  * \param[in] resolution_arg Octree resolution at lowest octree level (voxel size) */
119  OctreePointCloudAdjacency (const double resolution_arg);
120 
121  /** \brief Empty class destructor. */
123  {
124  }
125 
126  /** \brief Adds points from cloud to the octree.
127  *
128  * \note This overrides addPointsFromInputCloud() from the OctreePointCloud class. */
129  void
131 
132  /** \brief Gets the leaf container for a given point.
133  *
134  * \param[in] point_arg Point to search for
135  *
136  * \returns Pointer to the leaf container - null if no leaf container found. */
137  LeafContainerT*
138  getLeafContainerAtPoint (const PointT& point_arg) const;
139 
140  /** \brief Computes an adjacency graph of voxel relations.
141  *
142  * \warning This slows down rapidly as cloud size increases due to the number of edges.
143  *
144  * \param[out] voxel_adjacency_graph Boost Graph Library Adjacency graph of the voxel touching relationships.
145  * Vertices are PointT, edges represent touching, and edge lengths are the distance between the points. */
146  void
147  computeVoxelAdjacencyGraph (VoxelAdjacencyList &voxel_adjacency_graph);
148 
149  /** \brief Sets a point transform (and inverse) used to transform the space of the input cloud.
150  *
151  * This is useful for changing how adjacency is calculated - such as relaxing the adjacency criterion for
152  * points further from the camera.
153  *
154  * \param[in] transform_func A boost:function pointer to the transform to be used. The transform must have one
155  * parameter (a point) which it modifies in place. */
156  void
157  setTransformFunction (std::function<void (PointT &p)> transform_func)
158  {
159  transform_func_ = transform_func;
160  }
161 
162  /** \brief Tests whether input point is occluded from specified camera point by other voxels.
163  *
164  * \param[in] point_arg Point to test for
165  * \param[in] camera_pos Position of camera, defaults to origin
166  *
167  * \returns True if path to camera is blocked by a voxel, false otherwise. */
168  bool
169  testForOcclusion (const PointT& point_arg, const PointXYZ &camera_pos = PointXYZ (0, 0, 0));
170 
171  protected:
172 
173  /** \brief Add point at index from input pointcloud dataset to octree.
174  *
175  * \param[in] point_idx_arg The index representing the point in the dataset given by setInputCloud() to be added
176  *
177  * \note This virtual implementation allows the use of a transform function to compute keys. */
178  void
179  addPointIdx (const int point_idx_arg) override;
180 
181  /** \brief Fills in the neighbors fields for new voxels.
182  *
183  * \param[in] key_arg Key of the voxel to check neighbors for
184  * \param[in] leaf_container Pointer to container of the leaf to check neighbors for */
185  void
186  computeNeighbors (OctreeKey &key_arg, LeafContainerT* leaf_container);
187 
188  /** \brief Generates octree key for specified point (uses transform if provided).
189  *
190  * \param[in] point_arg Point to generate key for
191  * \param[out] key_arg Resulting octree key */
192  void
193  genOctreeKeyforPoint (const PointT& point_arg, OctreeKey& key_arg) const;
194 
195  private:
196 
197  /** \brief Add point at given index from input point cloud to octree.
198  *
199  * Index will be also added to indices vector. This functionality is not enabled for adjacency octree. */
201 
202  /** \brief Add point simultaneously to octree and input point cloud.
203  *
204  * This functionality is not enabled for adjacency octree. */
206 
215 
216  /// Local leaf pointer vector used to make iterating through leaves fast.
217  LeafVectorT leaf_vector_;
218 
219  std::function<void (PointT &p)> transform_func_;
220 
221  };
222 
223  }
224 
225 }
226 
227 // Note: Do not precompile this octree type because it is typically used with custom leaf containers.
228 #include <pcl/octree/impl/octree_pointcloud_adjacency.hpp>
typename VoxelAdjacencyList::edge_descriptor EdgeID
void genOctreeKeyforPoint(const PointT &point_arg, OctreeKey &key_arg) const
Generates octree key for specified point (uses transform if provided).
Octree pointcloud class
Octree class.
Definition: octree_base.h:61
typename OctreeT::BranchNode BranchNode
OctreePointCloudAdjacency(const double resolution_arg)
Constructor.
typename VoxelAdjacencyList::vertex_descriptor VoxelID
void addPointToCloud(const PointT &point_arg, PointCloudPtr cloud_arg)
Add point simultaneously to octree and input point cloud.
void setTransformFunction(std::function< void(PointT &p)> transform_func)
Sets a point transform (and inverse) used to transform the space of the input cloud.
Octree pointcloud voxel class which maintains adjacency information for its voxels.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void addPointIdx(const int point_idx_arg) override
Add point at index from input pointcloud dataset to octree.
boost::shared_ptr< PointCloud > PointCloudPtr
PointCloudConstPtr input_
Pointer to input point cloud dataset.
void computeNeighbors(OctreeKey &key_arg, LeafContainerT *leaf_container)
Fills in the neighbors fields for new voxels.
A point structure representing Euclidean xyz coordinates.
typename LeafVectorT::const_iterator const_iterator
boost::shared_ptr< const OctreeAdjacencyT > ConstPtr
LeafContainerT * getLeafContainerAtPoint(const PointT &point_arg) const
Gets the leaf container for a given point.
boost::shared_ptr< OctreeAdjacencyT > Ptr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Octree key class
Definition: octree_key.h:50
void addPointFromCloud(const int point_idx_arg, IndicesPtr indices_arg)
Add point at given index from input point cloud to octree.
Abstract octree leaf class
Definition: octree_nodes.h:96
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< const PointCloud > PointCloudConstPtr
void computeVoxelAdjacencyGraph(VoxelAdjacencyList &voxel_adjacency_graph)
Computes an adjacency graph of voxel relations.
typename OctreeT::LeafNode LeafNode
A point structure representing Euclidean xyz coordinates, and the RGB color.
Abstract octree branch class
Definition: octree_nodes.h:203
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, PointT, float > VoxelAdjacencyList
double resolution_
Octree resolution.
void addPointsFromInputCloud()
Adds points from cloud to the octree.