Point Cloud Library (PCL)  1.9.1
octree2buf_base.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, 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_TREE_2BUF_BASE_H
40 #define PCL_OCTREE_TREE_2BUF_BASE_H
41 
42 #include <vector>
43 
44 #include <pcl/octree/octree_nodes.h>
45 #include <pcl/octree/octree_container.h>
46 #include <pcl/octree/octree_key.h>
47 #include <pcl/octree/octree_iterator.h>
48 
49 
50 namespace pcl
51 {
52  namespace octree
53  {
54 
55  template<typename ContainerT>
57  {
58 
59  public:
60  /** \brief Empty constructor. */
62  {
63  reset ();
64  }
65 
66  /** \brief Copy constructor. */
68  {
69  *this = source;
70  }
71 
72  /** \brief Copy operator. */
73  inline BufferedBranchNode&
74  operator = (const BufferedBranchNode &source_arg)
75  {
76 
77  unsigned char i, b;
78 
79  memset (child_node_array_, 0, sizeof(child_node_array_));
80 
81  for (b = 0; b < 2; ++b)
82  for (i = 0; i < 8; ++i)
83  if (source_arg.child_node_array_[b][i])
84  child_node_array_[b][i] = source_arg.child_node_array_[b][i]->deepCopy ();
85 
86  return (*this);
87 
88  }
89 
90  /** \brief Empty constructor. */
92  {
93  }
94 
95  /** \brief Method to perform a deep copy of the octree */
96  virtual BufferedBranchNode*
97  deepCopy () const
98  {
99  return new BufferedBranchNode (*this);
100  }
101 
102  /** \brief Get child pointer in current branch node
103  * \param buffer_arg: buffer selector
104  * \param index_arg: index of child in node
105  * \return pointer to child node
106  * */
107  inline OctreeNode*
108  getChildPtr (unsigned char buffer_arg, unsigned char index_arg) const
109  {
110  assert( (buffer_arg<2) && (index_arg<8));
111  return child_node_array_[buffer_arg][index_arg];
112  }
113 
114  /** \brief Set child pointer in current branch node
115  * \param buffer_arg: buffer selector
116  * \param index_arg: index of child in node
117  * \param newNode_arg: pointer to new child node
118  * */
119  inline void setChildPtr (unsigned char buffer_arg, unsigned char index_arg,
120  OctreeNode* newNode_arg)
121  {
122  assert( (buffer_arg<2) && (index_arg<8));
123  child_node_array_[buffer_arg][index_arg] = newNode_arg;
124  }
125 
126  /** \brief Check if branch is pointing to a particular child node
127  * \param buffer_arg: buffer selector
128  * \param index_arg: index of child in node
129  * \return "true" if pointer to child node exists; "false" otherwise
130  * */
131  inline bool hasChild (unsigned char buffer_arg, unsigned char index_arg) const
132  {
133  assert( (buffer_arg<2) && (index_arg<8));
134  return (child_node_array_[buffer_arg][index_arg] != 0);
135  }
136 
137  /** \brief Get the type of octree node. Returns LEAVE_NODE type */
138  virtual node_type_t getNodeType () const
139  {
140  return BRANCH_NODE;
141  }
142 
143  /** \brief Reset branch node container for every branch buffer. */
144  inline void reset ()
145  {
146  memset (&child_node_array_[0][0], 0, sizeof(OctreeNode*) * 8 * 2);
147  }
148 
149  /** \brief Get const pointer to container */
150  const ContainerT*
151  operator->() const
152  {
153  return &container_;
154  }
155 
156  /** \brief Get pointer to container */
157  ContainerT*
159  {
160  return &container_;
161  }
162 
163  /** \brief Get const reference to container */
164  const ContainerT&
165  operator* () const
166  {
167  return container_;
168  }
169 
170  /** \brief Get reference to container */
171  ContainerT&
173  {
174  return container_;
175  }
176 
177  /** \brief Get const reference to container */
178  const ContainerT&
179  getContainer () const
180  {
181  return container_;
182  }
183 
184  /** \brief Get reference to container */
185  ContainerT&
187  {
188  return container_;
189  }
190 
191  /** \brief Get const pointer to container */
192  const ContainerT*
194  {
195  return &container_;
196  }
197 
198  /** \brief Get pointer to container */
199  ContainerT*
201  {
202  return &container_;
203  }
204 
205  protected:
206  ContainerT container_;
207 
209  };
210 
211  /** \brief @b Octree double buffer class
212  *
213  * \note This octree implementation keeps two separate octree structures
214  * in memory.
215  *
216  * \note This allows for differentially compare the octree structures (change detection, differential encoding).
217  * \note The tree depth defines the maximum amount of octree voxels / leaf nodes (should be initially defined).
218  * \note All leaf nodes are addressed by integer indices.
219  * \note Note: The tree depth equates to the bit length of the voxel indices.
220  * \ingroup octree
221  * \author Julius Kammerl (julius@kammerl.de)
222  */
223  template<typename LeafContainerT = int,
224  typename BranchContainerT = OctreeContainerEmpty >
226  {
227 
228  public:
229 
231 
232  // iterators are friends
233  friend class OctreeIteratorBase<OctreeT> ;
234  friend class OctreeDepthFirstIterator<OctreeT> ;
235  friend class OctreeBreadthFirstIterator<OctreeT> ;
236  friend class OctreeLeafNodeDepthFirstIterator<OctreeT> ;
237  friend class OctreeLeafNodeBreadthFirstIterator<OctreeT> ;
238 
241 
242  typedef BranchContainerT BranchContainer;
243  typedef LeafContainerT LeafContainer;
244 
245  // Octree default iterators
248  Iterator begin(unsigned int max_depth_arg = 0) {return Iterator(this, max_depth_arg);};
249  const Iterator end() {return Iterator();};
250 
251  // Octree leaf node iterators
252  // The previous deprecated names
253  // LeafNodeIterator and ConstLeafNodeIterator are deprecated.
254  // Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead.
257 
258  PCL_DEPRECATED ("Please use leaf_depth_begin () instead.")
259  LeafNodeIterator leaf_begin (unsigned int max_depth_arg = 0)
260  {
261  return LeafNodeIterator (this, max_depth_arg);
262  };
263 
264  PCL_DEPRECATED ("Please use leaf_depth_end () instead.")
265  const LeafNodeIterator leaf_end ()
266  {
267  return LeafNodeIterator ();
268  };
269 
270  // The currently valide names
273  LeafNodeDepthFirstIterator leaf_depth_begin (unsigned int max_depth_arg = 0)
274  {
275  return LeafNodeDepthFirstIterator (this, max_depth_arg);
276  };
277 
278  const LeafNodeDepthFirstIterator leaf_depth_end ()
279  {
280  return LeafNodeDepthFirstIterator();
281  };
282 
283  // Octree depth-first iterators
286  DepthFirstIterator depth_begin(unsigned int maxDepth_arg = 0) {return DepthFirstIterator(this, maxDepth_arg);};
287  const DepthFirstIterator depth_end() {return DepthFirstIterator();};
288 
289  // Octree breadth-first iterators
292  BreadthFirstIterator breadth_begin(unsigned int max_depth_arg = 0) {return BreadthFirstIterator(this, max_depth_arg);};
293  const BreadthFirstIterator breadth_end() {return BreadthFirstIterator();};
294 
295  // Octree leaf node iterators
298 
299  LeafNodeBreadthIterator leaf_breadth_begin (unsigned int max_depth_arg = 0u)
300  {
301  return LeafNodeBreadthIterator (this, max_depth_arg? max_depth_arg : this->octree_depth_);
302  };
303 
304  const LeafNodeBreadthIterator leaf_breadth_end ()
305  {
306  return LeafNodeBreadthIterator (this, 0, NULL);
307  };
308 
309  /** \brief Empty constructor. */
310  Octree2BufBase ();
311 
312  /** \brief Empty deconstructor. */
313  virtual
314  ~Octree2BufBase ();
315 
316  /** \brief Copy constructor. */
317  Octree2BufBase (const Octree2BufBase& source) :
318  leaf_count_ (source.leaf_count_),
319  branch_count_ (source.branch_count_),
320  root_node_ (new (BranchNode) (*(source.root_node_))),
321  depth_mask_ (source.depth_mask_),
322  max_key_ (source.max_key_),
323  buffer_selector_ (source.buffer_selector_),
324  tree_dirty_flag_ (source.tree_dirty_flag_),
325  octree_depth_ (source.octree_depth_),
326  dynamic_depth_enabled_(source.dynamic_depth_enabled_)
327  {
328  }
329 
330  /** \brief Copy constructor. */
331  inline Octree2BufBase&
333  {
334  leaf_count_ = source.leaf_count_;
335  branch_count_ = source.branch_count_;
336  root_node_ = new (BranchNode) (* (source.root_node_));
337  depth_mask_ = source.depth_mask_;
338  max_key_ = source.max_key_;
339  buffer_selector_ = source.buffer_selector_;
340  tree_dirty_flag_ = source.tree_dirty_flag_;
341  octree_depth_ = source.octree_depth_;
342  dynamic_depth_enabled_ = source.dynamic_depth_enabled_;
343  return (*this);
344  }
345 
346  /** \brief Set the maximum amount of voxels per dimension.
347  * \param max_voxel_index_arg: maximum amount of voxels per dimension
348  * */
349  void
350  setMaxVoxelIndex (unsigned int max_voxel_index_arg);
351 
352  /** \brief Set the maximum depth of the octree.
353  * \param depth_arg: maximum depth of octree
354  * */
355  void
356  setTreeDepth (unsigned int depth_arg);
357 
358  /** \brief Get the maximum depth of the octree.
359  * \return depth_arg: maximum depth of octree
360  * */
361  inline unsigned int getTreeDepth () const
362  {
363  return this->octree_depth_;
364  }
365 
366  /** \brief Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
367  * \note If leaf node already exist, this method returns the existing node
368  * \param idx_x_arg: index of leaf node in the X axis.
369  * \param idx_y_arg: index of leaf node in the Y axis.
370  * \param idx_z_arg: index of leaf node in the Z axis.
371  * \return pointer to new leaf node container.
372  * */
373  LeafContainerT*
374  createLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
375 
376  /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
377  * \note If leaf node already exist, this method returns the existing node
378  * \param idx_x_arg: index of leaf node in the X axis.
379  * \param idx_y_arg: index of leaf node in the Y axis.
380  * \param idx_z_arg: index of leaf node in the Z axis.
381  * \return pointer to leaf node container if found, null pointer otherwise.
382  * */
383  LeafContainerT*
384  findLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
385 
386  /** \brief Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
387  * \param idx_x_arg: index of leaf node in the X axis.
388  * \param idx_y_arg: index of leaf node in the Y axis.
389  * \param idx_z_arg: index of leaf node in the Z axis.
390  * \return "true" if leaf node search is successful, otherwise it returns "false".
391  * */
392  bool
393  existLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const;
394 
395  /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
396  * \param idx_x_arg: index of leaf node in the X axis.
397  * \param idx_y_arg: index of leaf node in the Y axis.
398  * \param idx_z_arg: index of leaf node in the Z axis.
399  * */
400  void
401  removeLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);
402 
403  /** \brief Return the amount of existing leafs in the octree.
404  * \return amount of registered leaf nodes.
405  * */
406  inline std::size_t getLeafCount () const
407  {
408  return (leaf_count_);
409  }
410 
411  /** \brief Return the amount of existing branches in the octree.
412  * \return amount of branch nodes.
413  * */
414  inline std::size_t getBranchCount () const
415  {
416  return (branch_count_);
417  }
418 
419  /** \brief Delete the octree structure and its leaf nodes.
420  * */
421  void
422  deleteTree ();
423 
424  /** \brief Delete octree structure of previous buffer. */
425  inline void deletePreviousBuffer ()
426  {
427  treeCleanUpRecursive (root_node_);
428  }
429 
430  /** \brief Delete the octree structure in the current buffer. */
431  inline void deleteCurrentBuffer ()
432  {
433  buffer_selector_ = !buffer_selector_;
434  treeCleanUpRecursive (root_node_);
435  leaf_count_ = 0;
436  }
437 
438  /** \brief Switch buffers and reset current octree structure. */
439  void
440  switchBuffers ();
441 
442  /** \brief Serialize octree into a binary output vector describing its branch node structure.
443  * \param binary_tree_out_arg: reference to output vector for writing binary tree structure.
444  * \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree
445  * */
446  void
447  serializeTree (std::vector<char>& binary_tree_out_arg,
448  bool do_XOR_encoding_arg = false);
449 
450  /** \brief Serialize octree into a binary output vector describing its branch node structure and and push all DataT elements stored in the octree to a vector.
451  * \param binary_tree_out_arg: reference to output vector for writing binary tree structure.
452  * \param leaf_container_vector_arg: pointer to all LeafContainerT objects in the octree
453  * \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree
454  * */
455  void
456  serializeTree (std::vector<char>& binary_tree_out_arg,
457  std::vector<LeafContainerT*>& leaf_container_vector_arg,
458  bool do_XOR_encoding_arg = false);
459 
460  /** \brief Outputs a vector of all DataT elements that are stored within the octree leaf nodes.
461  * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree
462  * */
463  void
464  serializeLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg);
465 
466  /** \brief Outputs a vector of all DataT elements from leaf nodes, that do not exist in the previous octree buffer.
467  * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree
468  * */
469  void
470  serializeNewLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg);
471 
472  /** \brief Deserialize a binary octree description vector and create a corresponding octree structure. Leaf nodes are initialized with getDataTByKey(..).
473  * \param binary_tree_in_arg: reference to input vector for reading binary tree structure.
474  * \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree
475  * */
476  void
477  deserializeTree (std::vector<char>& binary_tree_in_arg,
478  bool do_XOR_decoding_arg = false);
479 
480  /** \brief Deserialize a binary octree description and create a corresponding octree structure. Leaf nodes are initialized with DataT elements from the dataVector.
481  * \param binary_tree_in_arg: reference to inpvectoream for reading binary tree structure.
482  * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree
483  * \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree
484  * */
485  void
486  deserializeTree (std::vector<char>& binary_tree_in_arg,
487  std::vector<LeafContainerT*>& leaf_container_vector_arg,
488  bool do_XOR_decoding_arg = false);
489 
490  protected:
491 
492  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
493  // Protected octree methods based on octree keys
494  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
495 
496  /** \brief Retrieve root node */
497  OctreeNode*
498  getRootNode () const
499  {
500  return (this->root_node_);
501  }
502 
503  /** \brief Find leaf node
504  * \param key_arg: octree key addressing a leaf node.
505  * \return pointer to leaf container. If leaf node is not found, this pointer returns 0.
506  * */
507  inline LeafContainerT*
508  findLeaf (const OctreeKey& key_arg) const
509  {
510  LeafContainerT* result = 0;
511  findLeafRecursive (key_arg, depth_mask_, root_node_, result);
512  return result;
513  }
514 
515  /** \brief Create a leaf node.
516  * \note If the leaf node at the given octree node does not exist, it will be created and added to the tree.
517  * \param key_arg: octree key addressing a leaf node.
518  * \return pointer to an existing or created leaf container.
519  * */
520  inline LeafContainerT*
521  createLeaf (const OctreeKey& key_arg)
522  {
523  LeafNode* leaf_node;
524  BranchNode* leaf_node_parent;
525 
526  createLeafRecursive (key_arg, depth_mask_ ,root_node_, leaf_node, leaf_node_parent, false);
527 
528  LeafContainerT* ret = leaf_node->getContainerPtr();
529 
530  return ret;
531  }
532 
533  /** \brief Check if leaf doesn't exist in the octree
534  * \param key_arg: octree key addressing a leaf node.
535  * \return "true" if leaf node is found; "false" otherwise
536  * */
537  inline bool existLeaf (const OctreeKey& key_arg) const
538  {
539  return (findLeaf(key_arg) != 0);
540  }
541 
542  /** \brief Remove leaf node from octree
543  * \param key_arg: octree key addressing a leaf node.
544  * */
545  inline void removeLeaf (const OctreeKey& key_arg)
546  {
547  if (key_arg <= max_key_)
548  {
549  deleteLeafRecursive (key_arg, depth_mask_, root_node_);
550 
551  // we changed the octree structure -> dirty
552  tree_dirty_flag_ = true;
553  }
554  }
555 
556  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
557  // Branch node accessor inline functions
558  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
559 
560  /** \brief Check if branch is pointing to a particular child node
561  * \param branch_arg: reference to octree branch class
562  * \param child_idx_arg: index to child node
563  * \return "true" if pointer to child node exists; "false" otherwise
564  * */
565  inline bool
566  branchHasChild (const BranchNode& branch_arg, unsigned char child_idx_arg) const
567  {
568  // test occupancyByte for child existence
569  return (branch_arg.getChildPtr(buffer_selector_, child_idx_arg) != 0);
570  }
571 
572  /** \brief Retrieve a child node pointer for child node at child_idx.
573  * \param branch_arg: reference to octree branch class
574  * \param child_idx_arg: index to child node
575  * \return pointer to octree child node class
576  */
577  inline OctreeNode*
578  getBranchChildPtr (const BranchNode& branch_arg,
579  unsigned char child_idx_arg) const
580  {
581  return branch_arg.getChildPtr(buffer_selector_, child_idx_arg);
582  }
583 
584  /** \brief Assign new child node to branch
585  * \param branch_arg: reference to octree branch class
586  * \param child_idx_arg: index to child node
587  * \param new_child_arg: pointer to new child node
588  * */
589  inline void
590  setBranchChildPtr (BranchNode& branch_arg, unsigned char child_idx_arg, OctreeNode* new_child_arg)
591  {
592  branch_arg.setChildPtr (buffer_selector_, child_idx_arg, new_child_arg);
593  }
594 
595  /** \brief Generate bit pattern reflecting the existence of child node pointers for current buffer
596  * \param branch_arg: reference to octree branch class
597  * \return a single byte with 8 bits of child node information
598  * */
599  inline char getBranchBitPattern (const BranchNode& branch_arg) const
600  {
601  unsigned char i;
602  char node_bits;
603 
604  // create bit pattern
605  node_bits = 0;
606  for (i = 0; i < 8; i++)
607  {
608  const OctreeNode* child = branch_arg.getChildPtr(buffer_selector_, i);
609  node_bits |= static_cast<char> ( (!!child) << i);
610  }
611 
612  return (node_bits);
613  }
614 
615  /** \brief Generate bit pattern reflecting the existence of child node pointers in specific buffer
616  * \param branch_arg: reference to octree branch class
617  * \param bufferSelector_arg: buffer selector
618  * \return a single byte with 8 bits of child node information
619  * */
620  inline char getBranchBitPattern (const BranchNode& branch_arg,
621  unsigned char bufferSelector_arg) const
622  {
623  unsigned char i;
624  char node_bits;
625 
626  // create bit pattern
627  node_bits = 0;
628  for (i = 0; i < 8; i++)
629  {
630  const OctreeNode* child = branch_arg.getChildPtr(bufferSelector_arg, i);
631  node_bits |= static_cast<char> ( (!!child) << i);
632  }
633 
634  return (node_bits);
635  }
636 
637  /** \brief Generate XOR bit pattern reflecting differences between the two octree buffers
638  * \param branch_arg: reference to octree branch class
639  * \return a single byte with 8 bits of child node XOR difference information
640  * */
642  const BranchNode& branch_arg) const
643  {
644  unsigned char i;
645  char node_bits[2];
646 
647  // create bit pattern for both buffers
648  node_bits[0] = node_bits[1] = 0;
649 
650  for (i = 0; i < 8; i++)
651  {
652  const OctreeNode* childA = branch_arg.getChildPtr(0, i);
653  const OctreeNode* childB = branch_arg.getChildPtr(1, i);
654 
655  node_bits[0] |= static_cast<char> ( (!!childA) << i);
656  node_bits[1] |= static_cast<char> ( (!!childB) << i);
657  }
658 
659  return node_bits[0] ^ node_bits[1];
660  }
661 
662  /** \brief Test if branch changed between previous and current buffer
663  * \param branch_arg: reference to octree branch class
664  * \return "true", if child node information differs between current and previous octree buffer
665  * */
666  inline bool hasBranchChanges (const BranchNode& branch_arg) const
667  {
668  return (getBranchXORBitPattern (branch_arg) > 0);
669  }
670 
671  /** \brief Delete child node and all its subchilds from octree in specific buffer
672  * \param branch_arg: reference to octree branch class
673  * \param buffer_selector_arg: buffer selector
674  * \param child_idx_arg: index to child node
675  * */
676  inline void deleteBranchChild (BranchNode& branch_arg,
677  unsigned char buffer_selector_arg,
678  unsigned char child_idx_arg)
679  {
680  if (branch_arg.hasChild(buffer_selector_arg, child_idx_arg))
681  {
682  OctreeNode* branchChild = branch_arg.getChildPtr(buffer_selector_arg, child_idx_arg);
683 
684  switch (branchChild->getNodeType ())
685  {
686  case BRANCH_NODE:
687  {
688  // free child branch recursively
689  deleteBranch (*static_cast<BranchNode*> (branchChild));
690 
691  // delete unused branch
692  delete (branchChild);
693  break;
694  }
695 
696  case LEAF_NODE:
697  {
698  // push unused leaf to branch pool
699  delete (branchChild);
700  break;
701  }
702  default:
703  break;
704  }
705 
706  // set branch child pointer to 0
707  branch_arg.setChildPtr(buffer_selector_arg, child_idx_arg, 0);
708  }
709  }
710 
711  /** \brief Delete child node and all its subchilds from octree in current buffer
712  * \param branch_arg: reference to octree branch class
713  * \param child_idx_arg: index to child node
714  * */
715  inline void deleteBranchChild (BranchNode& branch_arg, unsigned char child_idx_arg)
716  {
717  deleteBranchChild(branch_arg, buffer_selector_, child_idx_arg);
718  }
719 
720  /** \brief Delete branch and all its subchilds from octree (both buffers)
721  * \param branch_arg: reference to octree branch class
722  * */
723  inline void deleteBranch (BranchNode& branch_arg)
724  {
725  char i;
726 
727  // delete all branch node children
728  for (i = 0; i < 8; i++)
729  {
730 
731  if (branch_arg.getChildPtr(0, i) == branch_arg.getChildPtr(1, i))
732  {
733  // reference was copied - there is only one child instance to be deleted
734  deleteBranchChild (branch_arg, 0, i);
735 
736  // remove pointers from both buffers
737  branch_arg.setChildPtr(0, i, 0);
738  branch_arg.setChildPtr(1, i, 0);
739  }
740  else
741  {
742  deleteBranchChild (branch_arg, 0, i);
743  deleteBranchChild (branch_arg, 1, i);
744  }
745  }
746  }
747 
748  /** \brief Fetch and add a new branch child to a branch class in current buffer
749  * \param branch_arg: reference to octree branch class
750  * \param child_idx_arg: index to child node
751  * \return pointer of new branch child to this reference
752  * */
754  unsigned char child_idx_arg)
755  {
756  BranchNode* new_branch_child = new BranchNode();
757 
758  branch_arg.setChildPtr (buffer_selector_, child_idx_arg,
759  static_cast<OctreeNode*> (new_branch_child));
760 
761  return new_branch_child;
762  }
763 
764  /** \brief Fetch and add a new leaf child to a branch class
765  * \param branch_arg: reference to octree branch class
766  * \param child_idx_arg: index to child node
767  * \return pointer of new leaf child to this reference
768  * */
769  inline LeafNode*
770  createLeafChild (BranchNode& branch_arg, unsigned char child_idx_arg)
771  {
772  LeafNode* new_leaf_child = new LeafNode();
773 
774  branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_leaf_child);
775 
776  return new_leaf_child;
777  }
778 
779  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
780  // Recursive octree methods
781  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
782 
783  /** \brief Create a leaf node at octree key. If leaf node does already exist, it is returned.
784  * \param key_arg: reference to an octree key
785  * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator
786  * \param branch_arg: current branch node
787  * \param return_leaf_arg: return pointer to leaf container
788  * \param parent_of_leaf_arg: return pointer to parent of leaf node
789  * \param branch_reset_arg: Reset pointer array of current branch
790  * \return depth mask at which leaf node was created/found
791  **/
792  unsigned int
793  createLeafRecursive (const OctreeKey& key_arg,
794  unsigned int depth_mask_arg,
795  BranchNode* branch_arg,
796  LeafNode*& return_leaf_arg,
797  BranchNode*& parent_of_leaf_arg,
798  bool branch_reset_arg = false);
799 
800 
801  /** \brief Recursively search for a given leaf node and return a pointer.
802  * \note If leaf node does not exist, a 0 pointer is returned.
803  * \param key_arg: reference to an octree key
804  * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator
805  * \param branch_arg: current branch node
806  * \param result_arg: pointer to leaf container class
807  **/
808  void
809  findLeafRecursive (const OctreeKey& key_arg,
810  unsigned int depth_mask_arg,
811  BranchNode* branch_arg,
812  LeafContainerT*& result_arg) const;
813 
814 
815  /** \brief Recursively search and delete leaf node
816  * \param key_arg: reference to an octree key
817  * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator
818  * \param branch_arg: current branch node
819  * \return "true" if branch does not contain any childs; "false" otherwise. This indicates if current branch can be deleted.
820  **/
821  bool
822  deleteLeafRecursive (const OctreeKey& key_arg,
823  unsigned int depth_mask_arg,
824  BranchNode* branch_arg);
825 
826  /** \brief Recursively explore the octree and output binary octree description together with a vector of leaf node DataT content.
827  * \param branch_arg: current branch node
828  * \param key_arg: reference to an octree key
829  * \param binary_tree_out_arg: binary output vector
830  * \param leaf_container_vector_arg: vector to return pointers to all leaf container in the tree.
831  * \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree
832  * \param new_leafs_filter_arg: execute callback only for leaf nodes that did not exist in preceding buffer
833  **/
834  void
835  serializeTreeRecursive (BranchNode* branch_arg,
836  OctreeKey& key_arg,
837  std::vector<char>* binary_tree_out_arg,
838  typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
839  bool do_XOR_encoding_arg = false,
840  bool new_leafs_filter_arg = false);
841 
842  /** \brief Rebuild an octree based on binary XOR octree description and DataT objects for leaf node initialization.
843  * \param branch_arg: current branch node
844  * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator
845  * \param key_arg: reference to an octree key
846  * \param binary_tree_in_it_arg iterator of binary input data
847  * \param binary_tree_in_it_end_arg
848  * \param leaf_container_vector_it_arg: iterator pointing to leaf container pointers to be added to a leaf node
849  * \param leaf_container_vector_it_end_arg: iterator pointing to leaf container pointers pointing to last object in input container.
850  * \param branch_reset_arg: Reset pointer array of current branch
851  * \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree
852  **/
853  void
854  deserializeTreeRecursive (BranchNode* branch_arg,
855  unsigned int depth_mask_arg,
856  OctreeKey& key_arg,
857  typename std::vector<char>::const_iterator& binary_tree_in_it_arg,
858  typename std::vector<char>::const_iterator& binary_tree_in_it_end_arg,
859  typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
860  typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_end_arg,
861  bool branch_reset_arg = false,
862  bool do_XOR_decoding_arg = false);
863 
864 
865  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
866  // Serialization callbacks
867  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
868 
869  /** \brief Callback executed for every leaf node data during serialization
870  **/
871  virtual void serializeTreeCallback (LeafContainerT &, const OctreeKey &)
872  {
873 
874  }
875 
876  /** \brief Callback executed for every leaf node data during deserialization
877  **/
878  virtual void deserializeTreeCallback (LeafContainerT&, const OctreeKey&)
879  {
880 
881  }
882 
883  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
884  // Helpers
885  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
886 
887  /** \brief Recursively explore the octree and remove unused branch and leaf nodes
888  * \param branch_arg: current branch node
889  **/
890  void
891  treeCleanUpRecursive (BranchNode* branch_arg);
892 
893  /** \brief Helper function to calculate the binary logarithm
894  * \param n_arg: some value
895  * \return binary logarithm (log2) of argument n_arg
896  */
897  inline double Log2 (double n_arg)
898  {
899  return log (n_arg) / log (2.0);
900  }
901 
902  /** \brief Test if octree is able to dynamically change its depth. This is required for adaptive bounding box adjustment.
903  * \return "false" - not resizeable due to XOR serialization
904  **/
905  inline bool octreeCanResize ()
906  {
907  return (false);
908  }
909 
910  /** \brief Prints binary representation of a byte - used for debugging
911  * \param data_arg - byte to be printed to stdout
912  **/
913  inline void printBinary (char data_arg)
914  {
915  unsigned char mask = 1; // Bit mask
916 
917  // Extract the bits
918  for (int i = 0; i < 8; i++)
919  {
920  // Mask each bit in the byte and print it
921  std::cout << ((data_arg & (mask << i)) ? "1" : "0");
922  }
923  std::cout << std::endl;
924  }
925 
926  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
927  // Globals
928  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
929 
930  /** \brief Amount of leaf nodes **/
931  std::size_t leaf_count_;
932 
933  /** \brief Amount of branch nodes **/
934  std::size_t branch_count_;
935 
936  /** \brief Pointer to root branch node of octree **/
938 
939  /** \brief Depth mask based on octree depth **/
940  unsigned int depth_mask_;
941 
942  /** \brief key range */
944 
945  /** \brief Currently active octree buffer **/
946  unsigned char buffer_selector_;
947 
948  // flags indicating if unused branches and leafs might exist in previous buffer
950 
951  /** \brief Octree depth */
952  unsigned int octree_depth_;
953 
954  /** \brief Enable dynamic_depth
955  * \note Note that this parameter is ignored in octree2buf! */
957 
958  };
959  }
960 }
961 
962 #ifdef PCL_NO_PRECOMPILE
963 #include <pcl/octree/impl/octree2buf_base.hpp>
964 #endif
965 
966 #endif
967 
LeafContainerT * createLeaf(const OctreeKey &key_arg)
Create a leaf node.
double Log2(double n_arg)
Helper function to calculate the binary logarithm.
OctreeNode * getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const
Get child pointer in current branch node.
const OctreeLeafNodeDepthFirstIterator< OctreeT > ConstLeafNodeDepthFirstIterator
virtual node_type_t getNodeType() const
Get the type of octree node.
bool octreeCanResize()
Test if octree is able to dynamically change its depth.
ContainerT * getContainerPtr()
Get pointer to container.
virtual ~BufferedBranchNode()
Empty constructor.
unsigned int octree_depth_
Octree depth.
Octree2BufBase(const Octree2BufBase &source)
Copy constructor.
const OctreeBreadthFirstIterator< OctreeT > ConstBreadthFirstIterator
char getBranchBitPattern(const BranchNode &branch_arg) const
Generate bit pattern reflecting the existence of child node pointers for current buffer.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
virtual BufferedBranchNode * deepCopy() const
Method to perform a deep copy of the octree.
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
const DepthFirstIterator depth_end()
const ContainerT & operator*() const
Get const reference to container.
void setBranchChildPtr(BranchNode &branch_arg, unsigned char child_idx_arg, OctreeNode *new_child_arg)
Assign new child node to branch.
bool branchHasChild(const BranchNode &branch_arg, unsigned char child_idx_arg) const
Check if branch is pointing to a particular child node.
LeafNode * createLeafChild(BranchNode &branch_arg, unsigned char child_idx_arg)
Fetch and add a new leaf child to a branch class.
OctreeNode * getRootNode() const
Retrieve root node.
const OctreeLeafNodeDepthFirstIterator< OctreeT > ConstLeafNodeIterator
const LeafNodeBreadthIterator leaf_breadth_end()
OctreeKey max_key_
key range
const OctreeDepthFirstIterator< OctreeT > ConstDepthFirstIterator
void deleteBranch(BranchNode &branch_arg)
Delete branch and all its subchilds from octree (both buffers)
void deleteBranchChild(BranchNode &branch_arg, unsigned char child_idx_arg)
Delete child node and all its subchilds from octree in current buffer.
OctreeLeafNodeBreadthFirstIterator< OctreeT > LeafNodeBreadthIterator
const OctreeDepthFirstIterator< OctreeT > ConstIterator
OctreeDepthFirstIterator< OctreeT > DepthFirstIterator
BranchContainerT BranchContainer
BreadthFirstIterator breadth_begin(unsigned int max_depth_arg=0)
void deleteBranchChild(BranchNode &branch_arg, unsigned char buffer_selector_arg, unsigned char child_idx_arg)
Delete child node and all its subchilds from octree in specific buffer.
bool dynamic_depth_enabled_
Enable dynamic_depth.
std::size_t getLeafCount() const
Return the amount of existing leafs in the octree.
virtual void deserializeTreeCallback(LeafContainerT &, const OctreeKey &)
Callback executed for every leaf node data during deserialization.
BufferedBranchNode()
Empty constructor.
const ContainerT & getContainer() const
Get const reference to container.
OctreeLeafNode< LeafContainerT > LeafNode
void deleteCurrentBuffer()
Delete the octree structure in the current buffer.
Octree leaf node iterator class.
LeafNodeBreadthIterator leaf_breadth_begin(unsigned int max_depth_arg=0u)
std::size_t branch_count_
Amount of branch nodes.
void deletePreviousBuffer()
Delete octree structure of previous buffer.
OctreeDepthFirstIterator< OctreeT > Iterator
OctreeNode * getBranchChildPtr(const BranchNode &branch_arg, unsigned char child_idx_arg) const
Retrieve a child node pointer for child node at child_idx.
char getBranchXORBitPattern(const BranchNode &branch_arg) const
Generate XOR bit pattern reflecting differences between the two octree buffers.
virtual OctreeNode * deepCopy() const =0
Pure virtual method to perform a deep copy of the octree.
const LeafNodeDepthFirstIterator leaf_depth_end()
Octree key class
Definition: octree_key.h:51
Abstract octree leaf class
Definition: octree_nodes.h:97
const ContainerT * operator->() const
Get const pointer to container.
LeafNodeDepthFirstIterator leaf_depth_begin(unsigned int max_depth_arg=0)
unsigned char buffer_selector_
Currently active octree buffer.
BufferedBranchNode< BranchContainerT > BranchNode
Iterator begin(unsigned int max_depth_arg=0)
void printBinary(char data_arg)
Prints binary representation of a byte - used for debugging.
const ContainerT * getContainerPtr() const
Get const pointer to container.
Definition: octree_nodes.h:178
virtual void serializeTreeCallback(LeafContainerT &, const OctreeKey &)
Callback executed for every leaf node data during serialization.
OctreeLeafNodeDepthFirstIterator< OctreeT > LeafNodeDepthFirstIterator
Abstract octree iterator class
ContainerT & getContainer()
Get reference to container.
OctreeLeafNodeDepthFirstIterator< OctreeT > LeafNodeIterator
Octree double buffer class
void removeLeaf(const OctreeKey &key_arg)
Remove leaf node from octree.
BranchNode * createBranchChild(BranchNode &branch_arg, unsigned char child_idx_arg)
Fetch and add a new branch child to a branch class in current buffer.
std::size_t getBranchCount() const
Return the amount of existing branches in the octree.
BranchNode * root_node_
Pointer to root branch node of octree.
unsigned int depth_mask_
Depth mask based on octree depth.
char getBranchBitPattern(const BranchNode &branch_arg, unsigned char bufferSelector_arg) const
Generate bit pattern reflecting the existence of child node pointers in specific buffer.
void setChildPtr(unsigned char buffer_arg, unsigned char index_arg, OctreeNode *newNode_arg)
Set child pointer in current branch node.
bool hasBranchChanges(const BranchNode &branch_arg) const
Test if branch changed between previous and current buffer.
DepthFirstIterator depth_begin(unsigned int maxDepth_arg=0)
LeafContainerT * findLeaf(const OctreeKey &key_arg) const
Find leaf node.
Octree container class that does not store any information.
bool existLeaf(const OctreeKey &key_arg) const
Check if leaf doesn&#39;t exist in the octree.
const ContainerT * getContainerPtr() const
Get const pointer to container.
OctreeNode * child_node_array_[2][8]
Abstract octree node class
Definition: octree_nodes.h:68
Octree2BufBase< LeafContainerT, BranchContainerT > OctreeT
std::size_t leaf_count_
Amount of leaf nodes.
unsigned int getTreeDepth() const
Get the maximum depth of the octree.
BufferedBranchNode(const BufferedBranchNode &source)
Copy constructor.
void reset()
Reset branch node container for every branch buffer.
BufferedBranchNode & operator=(const BufferedBranchNode &source_arg)
Copy operator.
bool hasChild(unsigned char buffer_arg, unsigned char index_arg) const
Check if branch is pointing to a particular child node.
const OctreeLeafNodeBreadthFirstIterator< OctreeT > ConstLeafNodeBreadthIterator