Point Cloud Library (PCL)  1.7.0
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/octree/octree_pointcloud_adjacency.h>
42 
43 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
44 template<typename PointT, typename LeafContainerT, typename BranchContainerT>
46 : OctreePointCloud<PointT, LeafContainerT, BranchContainerT
47 , OctreeBase<LeafContainerT, BranchContainerT> > (resolution_arg)
48 {
49 
50 }
51 
52 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
53 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
55 {
56  //double t1,t2;
57 
58  //t1 = timer_.getTime ();
60 
61 
62  //t2 = timer_.getTime ();
63  //std::cout << "Add Points:"<<t2-t1<<" ms Num leaves ="<<this->getLeafCount ()<<"\n";
64 
65  std::list <std::pair<OctreeKey,LeafContainerT*> > delete_list;
66  //double t_temp, t_neigh, t_compute, t_getLeaf;
67  //t_neigh = t_compute = t_getLeaf = 0;
68  LeafContainerT *leaf_container;
69  typename OctreeAdjacencyT::LeafNodeIterator leaf_itr;
70  leaf_vector_.reserve (this->getLeafCount ());
71  for ( leaf_itr = this->leaf_begin () ; leaf_itr != this->leaf_end (); ++leaf_itr)
72  {
73  //t_temp = timer_.getTime ();
74  OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey ();
75  leaf_container = &(leaf_itr.getLeafContainer ());
76  //t_getLeaf += timer_.getTime () - t_temp;
77 
78  //t_temp = timer_.getTime ();
79  //Run the leaf's compute function
80  leaf_container->computeData ();
81  //t_compute += timer_.getTime () - t_temp;
82 
83  //t_temp = timer_.getTime ();
84  // std::cout << "Computing neighbors\n";
85  computeNeighbors (leaf_key, leaf_container);
86  //t_neigh += timer_.getTime () - t_temp;
87 
88  leaf_vector_.push_back (leaf_container);
89 
90  }
91  //Go through and delete voxels scheduled
92  for (typename std::list<std::pair<OctreeKey,LeafContainerT*> >::iterator delete_itr = delete_list.begin (); delete_itr != delete_list.end (); ++delete_itr)
93  {
94  leaf_container = delete_itr->second;
95  //Remove pointer to it from all neighbors
96  typename std::set<LeafContainerT*>::iterator neighbor_itr = leaf_container->begin ();
97  typename std::set<LeafContainerT*>::iterator neighbor_end = leaf_container->end ();
98  for ( ; neighbor_itr != neighbor_end; ++neighbor_itr)
99  {
100  //Don't delete self neighbor
101  if (*neighbor_itr != leaf_container)
102  (*neighbor_itr)->removeNeighbor (leaf_container);
103  }
104  this->removeLeaf (delete_itr->first);
105  }
106 
107  //Make sure our leaf vector is correctly sized
108  assert (leaf_vector_.size () == this->getLeafCount ());
109 
110  // std::cout << "Time spent getting leaves ="<<t_getLeaf<<"\n";
111  // std::cout << "Time spent computing data in leaves="<<t_compute<<"\n";
112  // std::cout << "Time spent computing neighbors="<<t_neigh<<"\n";
113 
114 }
115 
116 //////////////////////////////////////////////////////////////////////////////////////////////
117 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
119 {
120  if (transform_func_)
121  {
122  PointT temp (point_arg);
123  transform_func_ (temp);
124  // calculate integer key for transformed point coordinates
125  key_arg.x = static_cast<unsigned int> ((temp.x - this->min_x_) / this->resolution_);
126  key_arg.y = static_cast<unsigned int> ((temp.y - this->min_y_) / this->resolution_);
127  key_arg.z = static_cast<unsigned int> ((temp.z - this->min_z_) / this->resolution_);
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  if (transform_func_)
152  {
153  PointT temp (point);
154  transform_func_ (temp);
155  this->adoptBoundingBoxToPoint (temp);
156  }
157  else
158  this->adoptBoundingBoxToPoint (point);
159 
160  // generate key
161  this->genOctreeKeyforPoint (point, key);
162  // add point to octree at key
163  LeafContainerT* container = this->createLeaf(key);
164  container->addPoint (point);
165 }
166 
167 
168 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
169 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
171 {
172 
173  OctreeKey neighbor_key;
174 
175  for (int dx = -1; dx <= 1; ++dx)
176  {
177  for (int dy = -1; dy <= 1; ++dy)
178  {
179  for (int dz = -1; dz <= 1; ++dz)
180  {
181  neighbor_key.x = key_arg.x + dx;
182  neighbor_key.y = key_arg.y + dy;
183  neighbor_key.z = key_arg.z + dz;
184  LeafContainerT *neighbor = this->findLeaf (neighbor_key);
185  if (neighbor)
186  {
187  leaf_container->addNeighbor (neighbor);
188  }
189  }
190  }
191  }
192 }
193 
194 
195 
196 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
197 template<typename PointT, typename LeafContainerT, typename BranchContainerT> LeafContainerT*
199  const PointT& point_arg) const
200 {
201  OctreeKey key;
202  LeafContainerT* leaf = 0;
203  // generate key
204  this->genOctreeKeyforPoint (point_arg, key);
205 
206  leaf = this->findLeaf (key);
207 
208  return leaf;
209 }
210 
211 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
212 template<typename PointT, typename LeafContainerT, typename BranchContainerT> void
214 {
215  //TODO Change this to use leaf centers, not centroids!
216 
217  voxel_adjacency_graph.clear ();
218  //Add a vertex for each voxel, store ids in map
219  std::map <LeafContainerT*, VoxelID> leaf_vertex_id_map;
220  for (typename OctreeAdjacencyT::LeafNodeIterator leaf_itr = this->leaf_begin () ; leaf_itr != this->leaf_end (); ++leaf_itr)
221  {
222  OctreeKey leaf_key = leaf_itr.getCurrentOctreeKey ();
223  PointT centroid_point;
224  this->genLeafNodeCenterFromOctreeKey (leaf_key, centroid_point);
225  VoxelID node_id = add_vertex (voxel_adjacency_graph);
226 
227  voxel_adjacency_graph[node_id] = centroid_point;
228  LeafContainerT* leaf_container = &(leaf_itr.getLeafContainer ());
229  leaf_vertex_id_map[leaf_container] = node_id;
230  }
231 
232  //Iterate through and add edges to adjacency graph
233  for ( typename std::vector<LeafContainerT*>::iterator leaf_itr = leaf_vector_.begin (); leaf_itr != leaf_vector_.end (); ++leaf_itr)
234  {
235  typename LeafContainerT::iterator neighbor_itr = (*leaf_itr)->begin ();
236  typename LeafContainerT::iterator neighbor_end = (*leaf_itr)->end ();
237  LeafContainerT* neighbor_container;
238  VoxelID u = (leaf_vertex_id_map.find (*leaf_itr))->second;
239  PointT p_u = voxel_adjacency_graph[u];
240  for ( ; neighbor_itr != neighbor_end; ++neighbor_itr)
241  {
242  neighbor_container = *neighbor_itr;
243  EdgeID edge;
244  bool edge_added;
245  VoxelID v = (leaf_vertex_id_map.find (neighbor_container))->second;
246  boost::tie (edge, edge_added) = add_edge (u,v,voxel_adjacency_graph);
247 
248  PointT p_v = voxel_adjacency_graph[v];
249  float dist = (p_v.getVector3fMap () - p_u.getVector3fMap ()).norm ();
250  voxel_adjacency_graph[edge] = dist;
251 
252  }
253 
254  }
255 
256 }
257 
258 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
259 template<typename PointT, typename LeafContainerT, typename BranchContainerT> bool
261 {
262  OctreeKey key;
263  this->genOctreeKeyforPoint (point_arg, key);
264  // This code follows the method in Octree::PointCloud
265  Eigen::Vector3f sensor(camera_pos.x,
266  camera_pos.y,
267  camera_pos.z);
268 
269  Eigen::Vector3f leaf_centroid(static_cast<float> ((static_cast<double> (key.x) + 0.5f) * this->resolution_ + this->min_x_),
270  static_cast<float> ((static_cast<double> (key.y) + 0.5f) * this->resolution_ + this->min_y_),
271  static_cast<float> ((static_cast<double> (key.z) + 0.5f) * this->resolution_ + this->min_z_));
272  Eigen::Vector3f direction = sensor - leaf_centroid;
273 
274  float norm = direction.norm ();
275  direction.normalize ();
276  float precision = 1.0f;
277  const float step_size = static_cast<const float> (resolution_) * precision;
278  const int nsteps = std::max (1, static_cast<int> (norm / step_size));
279 
280  OctreeKey prev_key = key;
281  // Walk along the line segment with small steps.
282  Eigen::Vector3f p = leaf_centroid;
283  PointT octree_p;
284  for (int i = 0; i < nsteps; ++i)
285  {
286  //Start at the leaf voxel, and move back towards sensor.
287  p += (direction * step_size);
288 
289  octree_p.x = p.x ();
290  octree_p.y = p.y ();
291  octree_p.z = p.z ();
292  // std::cout << octree_p<< "\n";
293  OctreeKey key;
294  this->genOctreeKeyforPoint (octree_p, key);
295 
296  // Not a new key, still the same voxel (starts at self).
297  if ((key == prev_key))
298  continue;
299 
300  prev_key = key;
301 
302  LeafContainerT *leaf = this->findLeaf (key);
303  //If the voxel is occupied, there is a possible occlusion
304  if (leaf)
305  {
306  return true;
307  }
308  }
309 
310  //If we didn't run into a voxel on the way to this camera, it can't be occluded.
311  return false;
312 
313 }
314 
315 #define PCL_INSTANTIATE_OctreePointCloudAdjacency(T) template class PCL_EXPORTS pcl::octree::OctreePointCloudAdjacency<T>;
316 
317 #endif
318