Point Cloud Library (PCL)  1.7.0
octree_base.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
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  * $Id$
37  */
38 
39 #ifndef PCL_OCTREE_BASE_HPP
40 #define PCL_OCTREE_BASE_HPP
41 
42 #include <vector>
43 
44 #include <pcl/impl/instantiate.hpp>
45 #include <pcl/point_types.h>
46 #include <pcl/octree/octree.h>
47 
48 namespace pcl
49 {
50  namespace octree
51  {
52  //////////////////////////////////////////////////////////////////////////////////////////////
53  template<typename LeafContainerT, typename BranchContainerT>
55  leaf_count_ (0),
56  branch_count_ (1),
57  root_node_ (new BranchNode ()),
58  depth_mask_ (0),
59  octree_depth_ (0),
60  dynamic_depth_enabled_ (false),
61  max_key_ ()
62  {
63  }
64 
65  //////////////////////////////////////////////////////////////////////////////////////////////
66  template<typename LeafContainerT, typename BranchContainerT>
68  {
69  // deallocate tree structure
70  deleteTree ();
71  delete (root_node_);
72  }
73 
74  //////////////////////////////////////////////////////////////////////////////////////////////
75  template<typename LeafContainerT, typename BranchContainerT>
76  void
78  {
79  unsigned int tree_depth;
80 
81  assert(max_voxel_index_arg>0);
82 
83  // tree depth == bitlength of maxVoxels
84  tree_depth = std::max ( (std::min (static_cast<unsigned int> (OctreeKey::maxDepth), static_cast<unsigned int> (std::ceil (Log2 (max_voxel_index_arg))))), 0u);
85 
86  // define depthMask_ by setting a single bit to 1 at bit position == tree depth
87  depth_mask_ = (1 << (tree_depth - 1));
88  }
89 
90  //////////////////////////////////////////////////////////////////////////////////////////////
91  template<typename LeafContainerT, typename BranchContainerT>
92  void
94  {
95  assert(depth_arg>0);
96 
97  // set octree depth
98  octree_depth_ = depth_arg;
99 
100  // define depthMask_ by setting a single bit to 1 at bit position == tree depth
101  depth_mask_ = (1 << (depth_arg - 1));
102 
103  // define max. keys
104  max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
105  }
106 
107  //////////////////////////////////////////////////////////////////////////////////////////////
108  template<typename LeafContainerT, typename BranchContainerT>
109  LeafContainerT*
111  unsigned int idx_y_arg,
112  unsigned int idx_z_arg)
113  {
114  // generate key
115  OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
116 
117  // check if key exist in octree
118  return (findLeaf (key));
119  }
120 
121  //////////////////////////////////////////////////////////////////////////////////////////////
122  template<typename LeafContainerT, typename BranchContainerT>
123  LeafContainerT*
125  unsigned int idx_y_arg,
126  unsigned int idx_z_arg)
127  {
128  // generate key
129  OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
130 
131  // check if key exist in octree
132  return (createLeaf (key));
133  }
134 
135  //////////////////////////////////////////////////////////////////////////////////////////////
136  template<typename LeafContainerT, typename BranchContainerT>
137  bool
139  unsigned int idx_y_arg,
140  unsigned int idx_z_arg) const
141  {
142  // generate key
143  OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
144 
145  // check if key exist in octree
146  return (existLeaf (key));
147  }
148 
149  //////////////////////////////////////////////////////////////////////////////////////////////
150  template<typename LeafContainerT, typename BranchContainerT>
151  void
153  unsigned int idx_y_arg,
154  unsigned int idx_z_arg)
155  {
156  // generate key
157  OctreeKey key (idx_x_arg, idx_y_arg, idx_z_arg);
158 
159  // check if key exist in octree
160  deleteLeafRecursive (key, depth_mask_, root_node_);
161  }
162 
163  //////////////////////////////////////////////////////////////////////////////////////////////
164  template<typename LeafContainerT, typename BranchContainerT>
165  void
167  {
168 
169  if (root_node_)
170  {
171  // reset octree
172  deleteBranch (*root_node_);
173  leaf_count_ = 0;
174  branch_count_ = 1;
175  }
176 
177  }
178 
179  //////////////////////////////////////////////////////////////////////////////////////////////
180  template<typename LeafContainerT, typename BranchContainerT>
181  void
182  OctreeBase<LeafContainerT, BranchContainerT>::serializeTree (std::vector<char>& binary_tree_out_arg)
183  {
184 
185  OctreeKey new_key;
186 
187  // clear binary vector
188  binary_tree_out_arg.clear ();
189  binary_tree_out_arg.reserve (this->branch_count_);
190 
191  serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, 0);
192  }
193 
194  //////////////////////////////////////////////////////////////////////////////////////////////
195  template<typename LeafContainerT, typename BranchContainerT>
196  void
197  OctreeBase<LeafContainerT, BranchContainerT>::serializeTree (std::vector<char>& binary_tree_out_arg,
198  std::vector<LeafContainerT*>& leaf_container_vector_arg)
199  {
200 
201  OctreeKey new_key;
202 
203  // clear output vectors
204  binary_tree_out_arg.clear ();
205  leaf_container_vector_arg.clear ();
206 
207  binary_tree_out_arg.reserve (this->branch_count_);
208  leaf_container_vector_arg.reserve (this->leaf_count_);
209 
210  serializeTreeRecursive (root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
211  }
212 
213  //////////////////////////////////////////////////////////////////////////////////////////////
214  template<typename LeafContainerT, typename BranchContainerT>
215  void
216  OctreeBase<LeafContainerT, BranchContainerT>::serializeLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg)
217  {
218  OctreeKey new_key;
219 
220  // clear output vector
221  leaf_container_vector_arg.clear ();
222 
223  leaf_container_vector_arg.reserve (this->leaf_count_);
224 
225  serializeTreeRecursive (root_node_, new_key, 0, &leaf_container_vector_arg);
226  }
227 
228  //////////////////////////////////////////////////////////////////////////////////////////////
229  template<typename LeafContainerT, typename BranchContainerT>
230  void
231  OctreeBase<LeafContainerT, BranchContainerT>::deserializeTree (std::vector<char>& binary_tree_out_arg)
232  {
233  OctreeKey new_key;
234 
235  // free existing tree before tree rebuild
236  deleteTree ();
237 
238  //iterator for binary tree structure vector
239  std::vector<char>::const_iterator binary_tree_out_it = binary_tree_out_arg.begin ();
240  std::vector<char>::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end ();
241 
242  deserializeTreeRecursive (root_node_,
243  depth_mask_,
244  new_key,
245  binary_tree_out_it,
246  binary_tree_out_it_end,
247  0,
248  0);
249 
250  }
251 
252  //////////////////////////////////////////////////////////////////////////////////////////////
253  template<typename LeafContainerT, typename BranchContainerT>
254  void
256  std::vector<LeafContainerT*>& leaf_container_vector_arg)
257  {
258  OctreeKey new_key;
259 
260  // set data iterator to first element
261  typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it = leaf_container_vector_arg.begin ();
262 
263  // set data iterator to last element
264  typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end = leaf_container_vector_arg.end ();
265 
266  // free existing tree before tree rebuild
267  deleteTree ();
268 
269  //iterator for binary tree structure vector
270  std::vector<char>::const_iterator binary_tree_input_it = binary_tree_in_arg.begin ();
271  std::vector<char>::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end ();
272 
273  deserializeTreeRecursive (root_node_,
274  depth_mask_,
275  new_key,
276  binary_tree_input_it,
277  binary_tree_input_it_end,
278  &leaf_vector_it,
279  &leaf_vector_it_end);
280 
281  }
282 
283  //////////////////////////////////////////////////////////////////////////////////////////////
284  template<typename LeafContainerT, typename BranchContainerT>
285  unsigned int
287  unsigned int depth_mask_arg,
288  BranchNode* branch_arg,
289  LeafNode*& return_leaf_arg,
290  BranchNode*& parent_of_leaf_arg)
291  {
292  // index to branch child
293  unsigned char child_idx;
294 
295  // find branch child from key
296  child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
297 
298  OctreeNode* child_node = (*branch_arg)[child_idx];
299 
300  if (!child_node)
301  {
302  if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1))
303  {
304  // if required branch does not exist -> create it
305  BranchNode* childBranch = createBranchChild (*branch_arg, child_idx);
306 
307  branch_count_++;
308 
309  // recursively proceed with indexed child branch
310  return createLeafRecursive (key_arg, depth_mask_arg / 2, childBranch, return_leaf_arg, parent_of_leaf_arg);
311 
312  }
313  else
314  {
315  // if leaf node at child_idx does not exist
316  LeafNode* leaf_node = createLeafChild (*branch_arg, child_idx);
317  return_leaf_arg = leaf_node;
318  parent_of_leaf_arg = branch_arg;
319  this->leaf_count_++;
320  }
321  }
322  else
323  {
324  // Node exists already
325  switch (child_node->getNodeType ())
326  {
327  case BRANCH_NODE:
328  // recursively proceed with indexed child branch
329  return createLeafRecursive (key_arg, depth_mask_arg / 2, static_cast<BranchNode*> (child_node),
330  return_leaf_arg, parent_of_leaf_arg);
331  break;
332 
333  case LEAF_NODE:
334  return_leaf_arg = static_cast<LeafNode*> (child_node);;
335  parent_of_leaf_arg = branch_arg;
336  break;
337  }
338 
339  }
340 
341  return (depth_mask_arg >> 1);
342  }
343 
344  //////////////////////////////////////////////////////////////////////////////////////////////
345  template<typename LeafContainerT, typename BranchContainerT>
346  void
348  unsigned int depth_mask_arg,
349  BranchNode* branch_arg,
350  LeafContainerT*& result_arg) const
351  {
352  // index to branch child
353  unsigned char child_idx;
354 
355  // find branch child from key
356  child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
357 
358  OctreeNode* child_node = (*branch_arg)[child_idx];
359 
360  if (child_node)
361  {
362  switch (child_node->getNodeType ())
363  {
364  case BRANCH_NODE:
365  // we have not reached maximum tree depth
366  BranchNode* child_branch;
367  child_branch = static_cast<BranchNode*> (child_node);
368 
369  findLeafRecursive (key_arg, depth_mask_arg / 2, child_branch, result_arg);
370  break;
371 
372  case LEAF_NODE:
373  // return existing leaf node
374  LeafNode* child_leaf;
375  child_leaf = static_cast<LeafNode*> (child_node);
376 
377  result_arg = child_leaf->getContainerPtr ();
378  break;
379  }
380  }
381  }
382 
383  //////////////////////////////////////////////////////////////////////////////////////////////
384  template<typename LeafContainerT, typename BranchContainerT>
385  bool
387  unsigned int depth_mask_arg,
388  BranchNode* branch_arg)
389  {
390  // index to branch child
391  unsigned char child_idx;
392  // indicates if branch is empty and can be safely removed
393  bool b_no_children;
394 
395  // find branch child from key
396  child_idx = key_arg.getChildIdxWithDepthMask (depth_mask_arg);
397 
398  OctreeNode* child_node = (*branch_arg)[child_idx];
399 
400  if (child_node)
401  {
402  switch (child_node->getNodeType ())
403  {
404 
405  case BRANCH_NODE:
406  BranchNode* child_branch;
407  child_branch = static_cast<BranchNode*> (child_node);
408 
409  // recursively explore the indexed child branch
410  b_no_children = deleteLeafRecursive (key_arg, depth_mask_arg / 2, child_branch);
411 
412  if (!b_no_children)
413  {
414  // child branch does not own any sub-child nodes anymore -> delete child branch
415  deleteBranchChild (*branch_arg, child_idx);
416  branch_count_--;
417  }
418  break;
419 
420  case LEAF_NODE:
421  // return existing leaf node
422 
423  // our child is a leaf node -> delete it
424  deleteBranchChild (*branch_arg, child_idx);
425  this->leaf_count_--;
426  break;
427  }
428  }
429 
430  // check if current branch still owns children
431  b_no_children = false;
432  for (child_idx = 0; (!b_no_children) && (child_idx < 8); child_idx++)
433  {
434  b_no_children = branch_arg->hasChild (child_idx);
435  }
436  // return true if current branch can be deleted
437  return (b_no_children);
438  }
439 
440  //////////////////////////////////////////////////////////////////////////////////////////////
441  template<typename LeafContainerT, typename BranchContainerT>
442  void
444  OctreeKey& key_arg,
445  std::vector<char>* binary_tree_out_arg,
446  typename std::vector<LeafContainerT*>* leaf_container_vector_arg) const
447  {
448 
449  // child iterator
450  unsigned char child_idx;
451  char node_bit_pattern;
452 
453  // branch occupancy bit pattern
454  node_bit_pattern = getBranchBitPattern (*branch_arg);
455 
456  // write bit pattern to output vector
457  if (binary_tree_out_arg)
458  binary_tree_out_arg->push_back (node_bit_pattern);
459 
460  // iterate over all children
461  for (child_idx = 0; child_idx < 8; child_idx++)
462  {
463 
464  // if child exist
465  if (branch_arg->hasChild (child_idx))
466  {
467  // add current branch voxel to key
468  key_arg.pushBranch (child_idx);
469 
470  OctreeNode *childNode = branch_arg->getChildPtr (child_idx);
471 
472  switch (childNode->getNodeType ())
473  {
474  case BRANCH_NODE:
475  {
476  // recursively proceed with indexed child branch
477  serializeTreeRecursive (static_cast<const BranchNode*> (childNode), key_arg, binary_tree_out_arg,
478  leaf_container_vector_arg);
479  break;
480  }
481  case LEAF_NODE:
482  {
483  LeafNode* child_leaf = static_cast<LeafNode*> (childNode);
484 
485  if (leaf_container_vector_arg)
486  leaf_container_vector_arg->push_back (child_leaf->getContainerPtr ());
487 
488  // we reached a leaf node -> execute serialization callback
489  serializeTreeCallback (**child_leaf, key_arg);
490  break;
491  }
492  default:
493  break;
494  }
495 
496  // pop current branch voxel from key
497  key_arg.popBranch ();
498  }
499  }
500  }
501 
502  //////////////////////////////////////////////////////////////////////////////////////////////
503  template<typename LeafContainerT, typename BranchContainerT>
504  void
506  typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
507  typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
508  typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
509  typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_end_arg)
510  {
511  // child iterator
512  unsigned char child_idx;
513  char node_bits;
514 
515  if (binary_tree_input_it_arg != binary_tree_input_it_end_arg)
516  {
517  // read branch occupancy bit pattern from input vector
518  node_bits = (*binary_tree_input_it_arg);
519  binary_tree_input_it_arg++;
520 
521  // iterate over all children
522  for (child_idx = 0; child_idx < 8; child_idx++)
523  {
524  // if occupancy bit for child_idx is set..
525  if (node_bits & (1 << child_idx))
526  {
527  // add current branch voxel to key
528  key_arg.pushBranch (child_idx);
529 
530  if (depth_mask_arg > 1)
531  {
532  // we have not reached maximum tree depth
533  BranchNode * newBranch = createBranchChild (*branch_arg, child_idx);
534 
535  branch_count_++;
536 
537  // recursively proceed with new child branch
538  deserializeTreeRecursive (newBranch, depth_mask_arg / 2, key_arg,
539  binary_tree_input_it_arg, binary_tree_input_it_end_arg,
540  leaf_container_vector_it_arg, leaf_container_vector_it_end_arg);
541  }
542  else
543  {
544  // we reached leaf node level
545 
546  LeafNode* child_leaf = createLeafChild (*branch_arg, child_idx);
547 
548  if (leaf_container_vector_it_arg && (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg))
549  {
550  LeafContainerT& container = **child_leaf;
551  container = ***leaf_container_vector_it_arg;
552  ++*leaf_container_vector_it_arg;
553  }
554 
555  leaf_count_++;
556 
557  // execute deserialization callback
558  deserializeTreeCallback (**child_leaf, key_arg);
559  }
560 
561  // pop current branch voxel from key
562  key_arg.popBranch ();
563  }
564  }
565  }
566  }
567 
568  }
569 }
570 
571 #define PCL_INSTANTIATE_OctreeBase(T) template class PCL_EXPORTS pcl::octree::OctreeBase<T>;
572 
573 #endif
574