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