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