Point Cloud Library (PCL)  1.12.1-dev
octree2buf_base.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  */
38 
39 #pragma once
40 
41 #include <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
93  * \param index_arg: index of child in node
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
105  * \param index_arg: index of child in node
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
119  * \param index_arg: index of child in node
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
228  friend class OctreeIteratorBase<OctreeT>;
229  friend class OctreeDepthFirstIterator<OctreeT>;
230  friend class OctreeBreadthFirstIterator<OctreeT>;
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 valide 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_;
976 
977  /** \brief Amount of branch nodes **/
978  std::size_t branch_count_;
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_;
991 
992  /** \brief flags indicating if unused branches and leafs might exist in previous
993  * buffer **/
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
OctreeMatrix< OctreeNode *, 2, 8 > child_node_array_
BufferedBranchNode(const BufferedBranchNode &source)
Copy constructor.
ContainerT & getContainer()
Get reference to container.
const ContainerT * operator->() const
Get const pointer to container.
BufferedBranchNode * deepCopy() const override
Method to perform a deep copy of the octree.
const ContainerT & getContainer() const
Get const reference to container.
ContainerT * getContainerPtr()
Get pointer to container.
const ContainerT * getContainerPtr() const
Get const pointer to container.
std::array< std::array< T, COL >, ROW > OctreeMatrix
node_type_t getNodeType() const override
Get the type of octree node.
BufferedBranchNode & operator=(const BufferedBranchNode &source_arg)
Copy operator.
~BufferedBranchNode() override=default
Empty constructor.
ContainerT * operator->()
Get pointer to container.
const ContainerT & operator*() const
Get const reference to container.
void reset()
Reset branch node container for every branch buffer.
ContainerT & operator*()
Get reference to container.
void setChildPtr(unsigned char buffer_arg, unsigned char index_arg, OctreeNode *newNode_arg)
Set child pointer in current branch node.
BufferedBranchNode()
Empty constructor.
OctreeNode * getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const
Get child pointer in current branch node.
bool hasChild(unsigned char buffer_arg, unsigned char index_arg) const
Check if branch is pointing to a particular child node.
Octree double buffer class
LeafNode * createLeafChild(BranchNode &branch_arg, unsigned char child_idx_arg)
Fetch and add a new leaf child to a branch class.
OctreeLeafNodeDepthFirstIterator< OctreeT > LeafNodeDepthFirstIterator
void deleteBranch(BranchNode &branch_arg)
Delete branch and all its subchilds from octree (both buffers)
bool hasBranchChanges(const BranchNode &branch_arg) const
Test if branch changed between previous and current buffer.
OctreeNode * getRootNode() const
Retrieve root node.
LeafContainerT * findLeaf(const OctreeKey &key_arg) const
Find leaf 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.
OctreeDepthFirstIterator< OctreeT > DepthFirstIterator
char getBranchBitPattern(const BranchNode &branch_arg) const
Generate bit pattern reflecting the existence of child node pointers for current buffer.
Octree2BufBase(const Octree2BufBase &source)
Copy constructor.
OctreeKey max_key_
key range
LeafNodeBreadthIterator leaf_breadth_begin(uindex_t max_depth_arg=0u)
Iterator begin(uindex_t max_depth_arg=0)
void switchBuffers()
Switch buffers and reset current octree structure.
OctreeLeafNode< LeafContainerT > LeafNode
const LeafNodeBreadthIterator leaf_breadth_end()
std::size_t getLeafCount() const
Return the amount of existing leafs in the 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.
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.
std::size_t leaf_count_
Amount of leaf nodes
uindex_t octree_depth_
Octree depth.
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.
void treeCleanUpRecursive(BranchNode *branch_arg)
Recursively explore the octree and remove unused branch and leaf nodes.
char getBranchBitPattern(const BranchNode &branch_arg, unsigned char bufferSelector_arg) const
Generate bit pattern reflecting the existence of child node pointers in specific buffer.
bool octreeCanResize()
Test if octree is able to dynamically change its depth.
void deleteTree()
Delete the octree structure and its leaf nodes.
void removeLeaf(const OctreeKey &key_arg)
Remove leaf node from octree.
unsigned char buffer_selector_
Currently active octree buffer
void deleteCurrentBuffer()
Delete the octree structure in the current buffer.
void deleteBranchChild(BranchNode &branch_arg, unsigned char child_idx_arg)
Delete child node and all its subchilds from octree in current buffer.
char getBranchXORBitPattern(const BranchNode &branch_arg) const
Generate XOR bit pattern reflecting differences between the two octree buffers.
virtual void deserializeTreeCallback(LeafContainerT &, const OctreeKey &)
Callback executed for every leaf node data during deserialization.
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...
bool branchHasChild(const BranchNode &branch_arg, unsigned char child_idx_arg) const
Check if branch is pointing to a particular child node.
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).
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.
void deletePreviousBuffer()
Delete octree structure of previous buffer.
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).
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.
OctreeLeafNodeBreadthFirstIterator< OctreeT > LeafNodeBreadthIterator
BranchNode * root_node_
Pointer to root branch node of octree
void setTreeDepth(uindex_t depth_arg)
Set the maximum depth of the octree.
BreadthFirstIterator breadth_begin(uindex_t max_depth_arg=0)
BranchContainerT BranchContainer
void printBinary(char data_arg)
Prints binary representation of a byte - used for debugging.
bool tree_dirty_flag_
flags indicating if unused branches and leafs might exist in previous buffer
void setBranchChildPtr(BranchNode &branch_arg, unsigned char child_idx_arg, OctreeNode *new_child_arg)
Assign new child node to branch.
std::size_t branch_count_
Amount of branch nodes
void setMaxVoxelIndex(uindex_t max_voxel_index_arg)
Set the maximum amount of voxels per dimension.
bool dynamic_depth_enabled_
Enable dynamic_depth.
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).
const DepthFirstIterator depth_end()
bool deleteLeafRecursive(const OctreeKey &key_arg, uindex_t depth_mask_arg, BranchNode *branch_arg)
Recursively search and delete leaf node.
const BreadthFirstIterator breadth_end()
OctreeNode * getBranchChildPtr(const BranchNode &branch_arg, unsigned char child_idx_arg) const
Retrieve a child node pointer for child node at child_idx.
DepthFirstIterator depth_begin(uindex_t maxDepth_arg=0)
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).
LeafContainerT * createLeaf(const OctreeKey &key_arg)
Create a leaf node.
bool existLeaf(const OctreeKey &key_arg) const
Check if leaf doesn't exist in the octree.
uindex_t depth_mask_
Depth mask based on octree depth
uindex_t getTreeDepth() const
Get the maximum depth of the octree.
std::size_t getBranchCount() const
Return the amount of existing branches in the octree.
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...
OctreeBreadthFirstIterator< OctreeT > BreadthFirstIterator
virtual void serializeTreeCallback(LeafContainerT &, const OctreeKey &)
Callback executed for every leaf node data during serialization.
BufferedBranchNode< BranchContainerT > BranchNode
LeafNodeDepthFirstIterator leaf_depth_begin(uindex_t max_depth_arg=0)
Octree2BufBase()
Empty constructor.
Octree2BufBase & operator=(const Octree2BufBase &source)
Copy constructor.
const LeafNodeDepthFirstIterator leaf_depth_end()
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.
virtual ~Octree2BufBase()
Empty deconstructor.
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...
OctreeDepthFirstIterator< OctreeT > Iterator
Abstract octree iterator class
Octree key class
Definition: octree_key.h:54
Octree leaf node iterator class.
Abstract octree leaf class
Definition: octree_nodes.h:81
const ContainerT * getContainerPtr() const
Get const pointer to container.
Definition: octree_nodes.h:154
Abstract octree node class
Definition: octree_nodes.h:59
virtual node_type_t getNodeType() const =0
Pure virtual method for retrieving the type of octree node (branch or leaf)
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition: types.h:120
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
Defines all the PCL and non-PCL macros used.