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