Point Cloud Library (PCL)  1.11.1-dev
octree_pointcloud_adjacency.hpp
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 the copyright holder(s) 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  */
37 
38 #pragma once
39 
40 #include <pcl/common/geometry.h>
41 #include <pcl/common/point_tests.h> // for pcl::isFinite
42 #include <pcl/console/print.h>
43 
44 /*
45  * OctreePointCloudAdjacency is not precompiled, since it's used in other
46  * parts of PCL with custom LeafContainers. So if PCL_NO_PRECOMPILE is NOT
47  * used, octree_pointcloud_adjacency.h includes this file but octree_pointcloud.h
48  * would not include the implementation because it's precompiled. So we need to
49  * include it here "manually".
50  */
51 #include <pcl/octree/impl/octree_pointcloud.hpp>
52 
53 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
54 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
56  OctreePointCloudAdjacency(const double resolution_arg)
58  LeafContainerT,
59  BranchContainerT,
60  OctreeBase<LeafContainerT, BranchContainerT>>(resolution_arg)
61 {}
62 
63 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
64 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
65 void
68 {
69  // double t1,t2;
70  float minX = std::numeric_limits<float>::max(),
71  minY = std::numeric_limits<float>::max(),
72  minZ = std::numeric_limits<float>::max();
73  float maxX = -std::numeric_limits<float>::max(),
74  maxY = -std::numeric_limits<float>::max(),
75  maxZ = -std::numeric_limits<float>::max();
76 
77  for (std::size_t i = 0; i < input_->size(); ++i) {
78  PointT temp((*input_)[i]);
79  if (transform_func_) // Search for point with
80  transform_func_(temp);
81  if (!pcl::isFinite(
82  temp)) // Check to make sure transform didn't make point not finite
83  continue;
84  if (temp.x < minX)
85  minX = temp.x;
86  if (temp.y < minY)
87  minY = temp.y;
88  if (temp.z < minZ)
89  minZ = temp.z;
90  if (temp.x > maxX)
91  maxX = temp.x;
92  if (temp.y > maxY)
93  maxY = temp.y;
94  if (temp.z > maxZ)
95  maxZ = temp.z;
96  }
97  this->defineBoundingBox(minX, minY, minZ, maxX, maxY, maxZ);
98 
100 
101  leaf_vector_.reserve(this->getLeafCount());
102  for (auto leaf_itr = this->leaf_depth_begin(); leaf_itr != this->leaf_depth_end();
103  ++leaf_itr) {
104  OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey();
105  LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer());
106 
107  // Run the leaf's compute function
108  leaf_container->computeData();
109 
110  computeNeighbors(leaf_key, leaf_container);
111 
112  leaf_vector_.push_back(leaf_container);
113  }
114  // Make sure our leaf vector is correctly sized
115  assert(leaf_vector_.size() == this->getLeafCount());
116 }
117 
118 //////////////////////////////////////////////////////////////////////////////////////////////
119 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
120 void
122  genOctreeKeyforPoint(const PointT& point_arg, OctreeKey& key_arg) const
123 {
124  if (transform_func_) {
125  PointT temp(point_arg);
126  transform_func_(temp);
127  // calculate integer key for transformed point coordinates
128  if (pcl::isFinite(temp)) // Make sure transformed point is finite - if it is not, it
129  // gets default key
130  {
131  key_arg.x =
132  static_cast<unsigned int>((temp.x - this->min_x_) / this->resolution_);
133  key_arg.y =
134  static_cast<unsigned int>((temp.y - this->min_y_) / this->resolution_);
135  key_arg.z =
136  static_cast<unsigned int>((temp.z - this->min_z_) / this->resolution_);
137  }
138  else {
139  key_arg = OctreeKey();
140  }
141  }
142  else {
143  // calculate integer key for point coordinates
144  key_arg.x =
145  static_cast<unsigned int>((point_arg.x - this->min_x_) / this->resolution_);
146  key_arg.y =
147  static_cast<unsigned int>((point_arg.y - this->min_y_) / this->resolution_);
148  key_arg.z =
149  static_cast<unsigned int>((point_arg.z - this->min_z_) / this->resolution_);
150  }
151 }
152 
153 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
154 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
155 void
157  addPointIdx(const int pointIdx_arg)
158 {
159  OctreeKey key;
160 
161  assert(pointIdx_arg < static_cast<int>(this->input_->size()));
162 
163  const PointT& point = (*this->input_)[pointIdx_arg];
164  if (!pcl::isFinite(point))
165  return;
166 
167  // generate key
168  this->genOctreeKeyforPoint(point, key);
169  // add point to octree at key
170  LeafContainerT* container = this->createLeaf(key);
171  container->addPoint(point);
172 }
173 
174 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
175 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
176 void
178  computeNeighbors(OctreeKey& key_arg, LeafContainerT* leaf_container)
179 {
180  // Make sure requested key is valid
181  if (key_arg.x > this->max_key_.x || key_arg.y > this->max_key_.y ||
182  key_arg.z > this->max_key_.z) {
183  PCL_ERROR("OctreePointCloudAdjacency::computeNeighbors Requested neighbors for "
184  "invalid octree key\n");
185  return;
186  }
187 
188  OctreeKey neighbor_key;
189  int dx_min = (key_arg.x > 0) ? -1 : 0;
190  int dy_min = (key_arg.y > 0) ? -1 : 0;
191  int dz_min = (key_arg.z > 0) ? -1 : 0;
192  int dx_max = (key_arg.x == this->max_key_.x) ? 0 : 1;
193  int dy_max = (key_arg.y == this->max_key_.y) ? 0 : 1;
194  int dz_max = (key_arg.z == this->max_key_.z) ? 0 : 1;
195 
196  for (int dx = dx_min; dx <= dx_max; ++dx) {
197  for (int dy = dy_min; dy <= dy_max; ++dy) {
198  for (int dz = dz_min; dz <= dz_max; ++dz) {
199  neighbor_key.x = static_cast<std::uint32_t>(key_arg.x + dx);
200  neighbor_key.y = static_cast<std::uint32_t>(key_arg.y + dy);
201  neighbor_key.z = static_cast<std::uint32_t>(key_arg.z + dz);
202  LeafContainerT* neighbor = this->findLeaf(neighbor_key);
203  if (neighbor) {
204  leaf_container->addNeighbor(neighbor);
205  }
206  }
207  }
208  }
209 }
210 
211 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
212 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
213 LeafContainerT*
215  getLeafContainerAtPoint(const PointT& point_arg) const
216 {
217  OctreeKey key;
218  LeafContainerT* leaf = nullptr;
219  // generate key
220  this->genOctreeKeyforPoint(point_arg, key);
221 
222  leaf = this->findLeaf(key);
223 
224  return leaf;
225 }
226 
227 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
228 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
229 void
232 {
233  // TODO Change this to use leaf centers, not centroids!
234 
235  voxel_adjacency_graph.clear();
236  // Add a vertex for each voxel, store ids in map
237  std::map<LeafContainerT*, VoxelID> leaf_vertex_id_map;
238  for (typename OctreeAdjacencyT::LeafNodeDepthFirstIterator leaf_itr =
239  this->leaf_depth_begin();
240  leaf_itr != this->leaf_depth_end();
241  ++leaf_itr) {
242  OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey();
243  PointT centroid_point;
244  this->genLeafNodeCenterFromOctreeKey(leaf_key, centroid_point);
245  VoxelID node_id = add_vertex(voxel_adjacency_graph);
246 
247  voxel_adjacency_graph[node_id] = centroid_point;
248  LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer());
249  leaf_vertex_id_map[leaf_container] = node_id;
250  }
251 
252  // Iterate through and add edges to adjacency graph
253  for (typename std::vector<LeafContainerT*>::iterator leaf_itr = leaf_vector_.begin();
254  leaf_itr != leaf_vector_.end();
255  ++leaf_itr) {
256  VoxelID u = (leaf_vertex_id_map.find(*leaf_itr))->second;
257  PointT p_u = voxel_adjacency_graph[u];
258  for (auto neighbor_itr = (*leaf_itr)->cbegin(), neighbor_end = (*leaf_itr)->cend();
259  neighbor_itr != neighbor_end;
260  ++neighbor_itr) {
261  LeafContainerT* neighbor_container = *neighbor_itr;
262  EdgeID edge;
263  bool edge_added;
264  VoxelID v = (leaf_vertex_id_map.find(neighbor_container))->second;
265  boost::tie(edge, edge_added) = add_edge(u, v, voxel_adjacency_graph);
266 
267  PointT p_v = voxel_adjacency_graph[v];
268  float dist = (p_v.getVector3fMap() - p_u.getVector3fMap()).norm();
269  voxel_adjacency_graph[edge] = dist;
270  }
271  }
272 }
273 
274 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
275 template <typename PointT, typename LeafContainerT, typename BranchContainerT>
276 bool
278  testForOcclusion(const PointT& point_arg, const PointXYZ& camera_pos)
279 {
280  OctreeKey key;
281  this->genOctreeKeyforPoint(point_arg, key);
282  // This code follows the method in Octree::PointCloud
283  Eigen::Vector3f sensor(camera_pos.x, camera_pos.y, camera_pos.z);
284 
285  Eigen::Vector3f leaf_centroid(
286  static_cast<float>((static_cast<double>(key.x) + 0.5f) * this->resolution_ +
287  this->min_x_),
288  static_cast<float>((static_cast<double>(key.y) + 0.5f) * this->resolution_ +
289  this->min_y_),
290  static_cast<float>((static_cast<double>(key.z) + 0.5f) * this->resolution_ +
291  this->min_z_));
292  Eigen::Vector3f direction = sensor - leaf_centroid;
293 
294  float norm = direction.norm();
295  direction.normalize();
296  float precision = 1.0f;
297  const float step_size = static_cast<const float>(resolution_) * precision;
298  const int nsteps = std::max(1, static_cast<int>(norm / step_size));
299 
300  OctreeKey prev_key = key;
301  // Walk along the line segment with small steps.
302  Eigen::Vector3f p = leaf_centroid;
303  PointT octree_p;
304  for (int i = 0; i < nsteps; ++i) {
305  // Start at the leaf voxel, and move back towards sensor.
306  p += (direction * step_size);
307 
308  octree_p.x = p.x();
309  octree_p.y = p.y();
310  octree_p.z = p.z();
311  // std::cout << octree_p<< "\n";
312  OctreeKey key;
313  this->genOctreeKeyforPoint(octree_p, key);
314 
315  // Not a new key, still the same voxel (starts at self).
316  if ((key == prev_key))
317  continue;
318 
319  prev_key = key;
320 
321  LeafContainerT* leaf = this->findLeaf(key);
322  // If the voxel is occupied, there is a possible occlusion
323  if (leaf) {
324  return true;
325  }
326  }
327 
328  // If we didn't run into a voxel on the way to this camera, it can't be occluded.
329  return false;
330 }
331 
332 #define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) \
333  template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency<T>;
pcl::uint32_t
std::uint32_t uint32_t
Definition: types.h:58
pcl::isFinite
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
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:178
pcl::octree::OctreePointCloudAdjacency::VoxelID
typename VoxelAdjacencyList::vertex_descriptor VoxelID
Definition: octree_pointcloud_adjacency.h:100
pcl::octree::OctreePointCloud
Octree pointcloud class
Definition: octree_pointcloud.h:72
geometry.h
pcl::octree::OctreePointCloud::addPointsFromInputCloud
void addPointsFromInputCloud()
Add points from input point cloud to octree.
Definition: octree_pointcloud.hpp:78
pcl::octree::OctreeKey
Octree key class
Definition: octree_key.h:52
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::octree::OctreePointCloudAdjacency::EdgeID
typename VoxelAdjacencyList::edge_descriptor EdgeID
Definition: octree_pointcloud_adjacency.h:101
pcl::octree::OctreePointCloudAdjacency::addPointsFromInputCloud
void addPointsFromInputCloud()
Adds points from cloud to the octree.
Definition: octree_pointcloud_adjacency.hpp:67
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:122
pcl::octree::OctreeBase
Octree class.
Definition: octree_base.h:62
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition: point_types.hpp:300
pcl::octree::OctreeKey::z
std::uint32_t z
Definition: octree_key.h:151
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:157
pcl::octree::OctreeLeafNodeDepthFirstIterator
Octree leaf node iterator class.
Definition: octree_iterator.h:652
pcl::octree::OctreePointCloudAdjacency::OctreePointCloudAdjacency
OctreePointCloudAdjacency(const double resolution_arg)
Constructor.
Definition: octree_pointcloud_adjacency.hpp:56
pcl::octree::OctreePointCloudAdjacency::getLeafContainerAtPoint
LeafContainerT * getLeafContainerAtPoint(const PointT &point_arg) const
Gets the leaf container for a given point.
Definition: octree_pointcloud_adjacency.hpp:215
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:278
pcl::octree::OctreePointCloudAdjacency::computeVoxelAdjacencyGraph
void computeVoxelAdjacencyGraph(VoxelAdjacencyList &voxel_adjacency_graph)
Computes an adjacency graph of voxel relations.
Definition: octree_pointcloud_adjacency.hpp:231
pcl::octree::OctreePointCloudAdjacency::VoxelAdjacencyList
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, PointT, float > VoxelAdjacencyList
Definition: octree_pointcloud_adjacency.h:99
pcl::octree::OctreeKey::x
std::uint32_t x
Definition: octree_key.h:149
pcl::octree::OctreeKey::y
std::uint32_t y
Definition: octree_key.h:150