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