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