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