Point Cloud Library (PCL)  1.11.1
octree2buf_base.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  */
38 
39 #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.
OctreeNode * getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const
Get child pointer in current branch node.
void serializeLeafs(std::vector< LeafContainerT *> &leaf_container_vector_arg)
Outputs a vector of all DataT elements that are stored within the octree leaf nodes.
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.
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.
node_type_t getNodeType() const override
Get the type of octree node.
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.
char getBranchBitPattern(const BranchNode &branch_arg) const
Generate bit pattern reflecting the existence of child node pointers for current buffer.
OctreeLeafNode< LeafContainerT > LeafNode
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)
OctreeLeafNodeBreadthFirstIterator< OctreeT > LeafNodeBreadthIterator
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...
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.
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< BranchContainerT > BranchNode
OctreeBreadthFirstIterator< OctreeT > BreadthFirstIterator
void deleteTree()
Delete the octree structure and its leaf nodes.
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.
OctreeLeafNodeDepthFirstIterator< OctreeT > LeafNodeIterator
void switchBuffers()
Switch buffers and reset current octree structure.
BreadthFirstIterator breadth_begin(unsigned int max_depth_arg=0)
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
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.
std::size_t getLeafCount() const
Return the amount of existing leafs in the octree.
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).
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 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...
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.
OctreeDepthFirstIterator< OctreeT > DepthFirstIterator
const BreadthFirstIterator breadth_end()
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).
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()
OctreeLeafNodeDepthFirstIterator< OctreeT > LeafNodeDepthFirstIterator
BufferedBranchNode * deepCopy() const override
Method to perform a deep copy of the octree.
Octree key class
Definition: octree_key.h:52
Abstract octree leaf class
Definition: octree_nodes.h:83
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:156
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.
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major...
Definition: pcl_macros.h:147
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]
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.
Defines all the PCL and non-PCL macros used.
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).
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 deleteLeafRecursive(const OctreeKey &key_arg, unsigned int depth_mask_arg, BranchNode *branch_arg)
Recursively search and delete leaf node.
bool hasChild(unsigned char buffer_arg, unsigned char index_arg) const
Check if branch is pointing to a particular child node.