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