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