Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
47namespace pcl {
48
49namespace 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//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75template <typename PointT,
76 typename LeafContainerT = OctreePointCloudAdjacencyContainer<PointT>,
77 typename BranchContainerT = OctreeContainerEmpty>
79: public OctreePointCloud<PointT, LeafContainerT, BranchContainerT> {
80
81public:
83
86 using Ptr = shared_ptr<OctreeAdjacencyT>;
87 using ConstPtr = shared_ptr<const OctreeAdjacencyT>;
88
93
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
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
189protected:
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
215private:
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.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
Octree key class
Definition octree_key.h:54
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.
OctreePointCloudAdjacency< PointT, LeafContainerT, BranchContainerT > OctreeAdjacencyT
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
OctreeBase< LeafContainerT, BranchContainerT > OctreeBaseT
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.
typename OctreePointCloudT::BranchNode BranchNode
typename OctreePointCloudT::LeafNode LeafNode
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.