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 <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. */
80  ~BufferedBranchNode() override = default;
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 = index_t,
216  typename BranchContainerT = OctreeContainerEmpty>
218 
219 public:
221 
222  // iterators are friends
223  friend class OctreeIteratorBase<OctreeT>;
224  friend class OctreeDepthFirstIterator<OctreeT>;
225  friend class OctreeBreadthFirstIterator<OctreeT>;
228 
231 
232  using BranchContainer = BranchContainerT;
233  using LeafContainer = LeafContainerT;
234 
235  // Octree default iterators
238  Iterator
239  begin(uindex_t 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  // The currently valide names
261  leaf_depth_begin(uindex_t max_depth_arg = 0)
262  {
263  return LeafNodeDepthFirstIterator(this, max_depth_arg);
264  };
265 
268  {
270  };
271 
272  // Octree depth-first iterators
276  depth_begin(uindex_t maxDepth_arg = 0)
277  {
278  return DepthFirstIterator(this, maxDepth_arg);
279  };
280  const DepthFirstIterator
282  {
283  return DepthFirstIterator();
284  };
285 
286  // Octree breadth-first iterators
290  breadth_begin(uindex_t max_depth_arg = 0)
291  {
292  return BreadthFirstIterator(this, max_depth_arg);
293  };
296  {
297  return BreadthFirstIterator();
298  };
299 
300  // Octree leaf node iterators
304 
306  leaf_breadth_begin(uindex_t max_depth_arg = 0u)
307  {
308  return LeafNodeBreadthIterator(this,
309  max_depth_arg ? max_depth_arg : this->octree_depth_);
310  };
311 
314  {
315  return LeafNodeBreadthIterator(this, 0, nullptr);
316  };
317 
318  /** \brief Empty constructor. */
319  Octree2BufBase();
320 
321  /** \brief Empty deconstructor. */
322  virtual ~Octree2BufBase();
323 
324  /** \brief Copy constructor. */
326  : leaf_count_(source.leaf_count_)
327  , branch_count_(source.branch_count_)
328  , root_node_(new (BranchNode)(*(source.root_node_)))
329  , depth_mask_(source.depth_mask_)
330  , max_key_(source.max_key_)
333  , octree_depth_(source.octree_depth_)
335  {}
336 
337  /** \brief Copy constructor. */
338  inline Octree2BufBase&
339  operator=(const Octree2BufBase& source)
340  {
341  leaf_count_ = source.leaf_count_;
342  branch_count_ = source.branch_count_;
343  root_node_ = new (BranchNode)(*(source.root_node_));
344  depth_mask_ = source.depth_mask_;
345  max_key_ = source.max_key_;
348  octree_depth_ = source.octree_depth_;
350  return (*this);
351  }
352 
353  /** \brief Set the maximum amount of voxels per dimension.
354  * \param max_voxel_index_arg: maximum amount of voxels per dimension
355  */
356  void
357  setMaxVoxelIndex(uindex_t max_voxel_index_arg);
358 
359  /** \brief Set the maximum depth of the octree.
360  * \param depth_arg: maximum depth of octree
361  */
362  void
363  setTreeDepth(uindex_t depth_arg);
364 
365  /** \brief Get the maximum depth of the octree.
366  * \return depth_arg: maximum depth of octree
367  */
368  inline uindex_t
369  getTreeDepth() const
370  {
371  return this->octree_depth_;
372  }
373 
374  /** \brief Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
375  * \note If leaf node already exist, this method returns the existing node
376  * \param idx_x_arg: index of leaf node in the X axis.
377  * \param idx_y_arg: index of leaf node in the Y axis.
378  * \param idx_z_arg: index of leaf node in the Z axis.
379  * \return pointer to new leaf node container.
380  */
381  LeafContainerT*
382  createLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg);
383 
384  /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
385  * \note If leaf node already exist, this method returns the existing node
386  * \param idx_x_arg: index of leaf node in the X axis.
387  * \param idx_y_arg: index of leaf node in the Y axis.
388  * \param idx_z_arg: index of leaf node in the Z axis.
389  * \return pointer to leaf node container if found, null pointer otherwise.
390  */
391  LeafContainerT*
392  findLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg);
393 
394  /** \brief Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
395  * \param idx_x_arg: index of leaf node in the X axis.
396  * \param idx_y_arg: index of leaf node in the Y axis.
397  * \param idx_z_arg: index of leaf node in the Z axis.
398  * \return "true" if leaf node search is successful, otherwise it returns "false".
399  */
400  bool
401  existLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) const;
402 
403  /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
404  * \param idx_x_arg: index of leaf node in the X axis.
405  * \param idx_y_arg: index of leaf node in the Y axis.
406  * \param idx_z_arg: index of leaf node in the Z axis.
407  */
408  void
409  removeLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg);
410 
411  /** \brief Return the amount of existing leafs in the octree.
412  * \return amount of registered leaf nodes.
413  */
414  inline std::size_t
415  getLeafCount() const
416  {
417  return (leaf_count_);
418  }
419 
420  /** \brief Return the amount of existing branches in the octree.
421  * \return amount of branch nodes.
422  */
423  inline std::size_t
425  {
426  return (branch_count_);
427  }
428 
429  /** \brief Delete the octree structure and its leaf nodes.
430  */
431  void
432  deleteTree();
433 
434  /** \brief Delete octree structure of previous buffer. */
435  inline void
437  {
439  }
440 
441  /** \brief Delete the octree structure in the current buffer. */
442  inline void
444  {
447  leaf_count_ = 0;
448  }
449 
450  /** \brief Switch buffers and reset current octree structure. */
451  void
452  switchBuffers();
453 
454  /** \brief Serialize octree into a binary output vector describing its branch node
455  * structure.
456  * \param binary_tree_out_arg: reference to output vector for writing binary
457  * tree structure.
458  * \param do_XOR_encoding_arg: select if binary tree structure should be generated
459  * based on current octree (false) of based on a XOR comparison between current and
460  * previous octree
461  **/
462  void
463  serializeTree(std::vector<char>& binary_tree_out_arg,
464  bool do_XOR_encoding_arg = false);
465 
466  /** \brief Serialize octree into a binary output vector describing its branch node
467  * structure and and push all DataT elements stored in the octree to a vector.
468  * \param binary_tree_out_arg: reference to output vector for writing binary tree
469  * structure.
470  * \param leaf_container_vector_arg: pointer to all LeafContainerT objects in the
471  * octree
472  * \param do_XOR_encoding_arg: select if binary tree structure should be
473  * generated based on current octree (false) of based on a XOR comparison between
474  * current and previous octree
475  **/
476  void
477  serializeTree(std::vector<char>& binary_tree_out_arg,
478  std::vector<LeafContainerT*>& leaf_container_vector_arg,
479  bool do_XOR_encoding_arg = false);
480 
481  /** \brief Outputs a vector of all DataT elements that are stored within the octree
482  * leaf nodes.
483  * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects
484  * in the octree
485  */
486  void
487  serializeLeafs(std::vector<LeafContainerT*>& leaf_container_vector_arg);
488 
489  /** \brief Outputs a vector of all DataT elements from leaf nodes, that do not exist
490  * in the previous octree buffer.
491  * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects
492  * in the octree
493  */
494  void
495  serializeNewLeafs(std::vector<LeafContainerT*>& leaf_container_vector_arg);
496 
497  /** \brief Deserialize a binary octree description vector and create a corresponding
498  * octree structure. Leaf nodes are initialized with getDataTByKey(..).
499  * \param binary_tree_in_arg: reference to input vector for reading binary tree
500  * structure.
501  * \param do_XOR_decoding_arg: select if binary tree structure is based on current
502  * octree (false) of based on a XOR comparison between current and previous octree
503  */
504  void
505  deserializeTree(std::vector<char>& binary_tree_in_arg,
506  bool do_XOR_decoding_arg = false);
507 
508  /** \brief Deserialize a binary octree description and create a corresponding octree
509  * structure. Leaf nodes are initialized with DataT elements from the dataVector.
510  * \param binary_tree_in_arg: reference to inpvectoream for reading binary tree
511  * structure.
512  * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects
513  * in the octree
514  * \param do_XOR_decoding_arg: select if binary tree structure is based on current
515  * octree (false) of based on a XOR comparison between current and previous octree
516  */
517  void
518  deserializeTree(std::vector<char>& binary_tree_in_arg,
519  std::vector<LeafContainerT*>& leaf_container_vector_arg,
520  bool do_XOR_decoding_arg = false);
521 
522 protected:
523  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
524  // Protected octree methods based on octree keys
525  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
526 
527  /** \brief Retrieve root node */
528  OctreeNode*
529  getRootNode() const
530  {
531  return (this->root_node_);
532  }
533 
534  /** \brief Find leaf node
535  * \param key_arg: octree key addressing a leaf node.
536  * \return pointer to leaf container. If leaf node is not found, this pointer returns
537  * 0.
538  */
539  inline LeafContainerT*
540  findLeaf(const OctreeKey& key_arg) const
541  {
542  LeafContainerT* result = nullptr;
543  findLeafRecursive(key_arg, depth_mask_, root_node_, result);
544  return result;
545  }
546 
547  /** \brief Create a leaf node.
548  * \note If the leaf node at the given octree node does not exist, it will be created
549  * and added to the tree.
550  * \param key_arg: octree key addressing a leaf node.
551  * \return pointer to an existing or created leaf container.
552  */
553  inline LeafContainerT*
554  createLeaf(const OctreeKey& key_arg)
555  {
556  LeafNode* leaf_node;
557  BranchNode* leaf_node_parent;
558 
560  key_arg, depth_mask_, root_node_, leaf_node, leaf_node_parent, false);
561 
562  LeafContainerT* ret = leaf_node->getContainerPtr();
563 
564  return ret;
565  }
566 
567  /** \brief Check if leaf doesn't exist in the octree
568  * \param key_arg: octree key addressing a leaf node.
569  * \return "true" if leaf node is found; "false" otherwise
570  */
571  inline bool
572  existLeaf(const OctreeKey& key_arg) const
573  {
574  return (findLeaf(key_arg) != nullptr);
575  }
576 
577  /** \brief Remove leaf node from octree
578  * \param key_arg: octree key addressing a leaf node.
579  */
580  inline void
581  removeLeaf(const OctreeKey& key_arg)
582  {
583  if (key_arg <= max_key_) {
585 
586  // we changed the octree structure -> dirty
587  tree_dirty_flag_ = true;
588  }
589  }
590 
591  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
592  // Branch node accessor inline functions
593  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
594 
595  /** \brief Check if branch is pointing to a particular child node
596  * \param branch_arg: reference to octree branch class
597  * \param child_idx_arg: index to child node
598  * \return "true" if pointer to child node exists; "false" otherwise
599  */
600  inline bool
601  branchHasChild(const BranchNode& branch_arg, unsigned char child_idx_arg) const
602  {
603  // test occupancyByte for child existence
604  return (branch_arg.getChildPtr(buffer_selector_, child_idx_arg) != nullptr);
605  }
606 
607  /** \brief Retrieve a child node pointer for child node at child_idx.
608  * \param branch_arg: reference to octree branch class
609  * \param child_idx_arg: index to child node
610  * \return pointer to octree child node class
611  */
612  inline OctreeNode*
613  getBranchChildPtr(const BranchNode& branch_arg, unsigned char child_idx_arg) const
614  {
615  return branch_arg.getChildPtr(buffer_selector_, child_idx_arg);
616  }
617 
618  /** \brief Assign new child node to branch
619  * \param branch_arg: reference to octree branch class
620  * \param child_idx_arg: index to child node
621  * \param new_child_arg: pointer to new child node
622  */
623  inline void
625  unsigned char child_idx_arg,
626  OctreeNode* new_child_arg)
627  {
628  branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_child_arg);
629  }
630 
631  /** \brief Generate bit pattern reflecting the existence of child node pointers for
632  * current buffer
633  * \param branch_arg: reference to octree branch class
634  * \return a single byte with 8 bits of child node information
635  */
636  inline char
637  getBranchBitPattern(const BranchNode& branch_arg) const
638  {
639  char node_bits;
640 
641  // create bit pattern
642  node_bits = 0;
643  for (unsigned char i = 0; i < 8; i++) {
644  const OctreeNode* child = branch_arg.getChildPtr(buffer_selector_, i);
645  node_bits |= static_cast<char>((!!child) << i);
646  }
647 
648  return (node_bits);
649  }
650 
651  /** \brief Generate bit pattern reflecting the existence of child node pointers in
652  * specific buffer
653  * \param branch_arg: reference to octree branch class
654  * \param bufferSelector_arg: buffer selector
655  * \return a single byte with 8 bits of child node information
656  */
657  inline char
658  getBranchBitPattern(const BranchNode& branch_arg,
659  unsigned char bufferSelector_arg) const
660  {
661  char node_bits;
662 
663  // create bit pattern
664  node_bits = 0;
665  for (unsigned char i = 0; i < 8; i++) {
666  const OctreeNode* child = branch_arg.getChildPtr(bufferSelector_arg, i);
667  node_bits |= static_cast<char>((!!child) << i);
668  }
669 
670  return (node_bits);
671  }
672 
673  /** \brief Generate XOR bit pattern reflecting differences between the two octree
674  * buffers
675  * \param branch_arg: reference to octree branch class
676  * \return a single byte with 8 bits of child node XOR difference information
677  */
678  inline char
679  getBranchXORBitPattern(const BranchNode& branch_arg) const
680  {
681  char node_bits[2];
682 
683  // create bit pattern for both buffers
684  node_bits[0] = node_bits[1] = 0;
685 
686  for (unsigned char i = 0; i < 8; i++) {
687  const OctreeNode* childA = branch_arg.getChildPtr(0, i);
688  const OctreeNode* childB = branch_arg.getChildPtr(1, i);
689 
690  node_bits[0] |= static_cast<char>((!!childA) << i);
691  node_bits[1] |= static_cast<char>((!!childB) << i);
692  }
693 
694  return node_bits[0] ^ node_bits[1];
695  }
696 
697  /** \brief Test if branch changed between previous and current buffer
698  * \param branch_arg: reference to octree branch class
699  * \return "true", if child node information differs between current and previous
700  * octree buffer
701  */
702  inline bool
703  hasBranchChanges(const BranchNode& branch_arg) const
704  {
705  return (getBranchXORBitPattern(branch_arg) > 0);
706  }
707 
708  /** \brief Delete child node and all its subchilds from octree in specific buffer
709  * \param branch_arg: reference to octree branch class
710  * \param buffer_selector_arg: buffer selector
711  * \param child_idx_arg: index to child node
712  */
713  inline void
715  unsigned char buffer_selector_arg,
716  unsigned char child_idx_arg)
717  {
718  if (branch_arg.hasChild(buffer_selector_arg, child_idx_arg)) {
719  OctreeNode* branchChild =
720  branch_arg.getChildPtr(buffer_selector_arg, child_idx_arg);
721 
722  switch (branchChild->getNodeType()) {
723  case BRANCH_NODE: {
724  // free child branch recursively
725  deleteBranch(*static_cast<BranchNode*>(branchChild));
726 
727  // delete unused branch
728  delete (branchChild);
729  break;
730  }
731 
732  case LEAF_NODE: {
733  // push unused leaf to branch pool
734  delete (branchChild);
735  break;
736  }
737  default:
738  break;
739  }
740 
741  // set branch child pointer to 0
742  branch_arg.setChildPtr(buffer_selector_arg, child_idx_arg, nullptr);
743  }
744  }
745 
746  /** \brief Delete child node and all its subchilds from octree in current buffer
747  * \param branch_arg: reference to octree branch class
748  * \param child_idx_arg: index to child node
749  */
750  inline void
751  deleteBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg)
752  {
753  deleteBranchChild(branch_arg, buffer_selector_, child_idx_arg);
754  }
755 
756  /** \brief Delete branch and all its subchilds from octree (both buffers)
757  * \param branch_arg: reference to octree branch class
758  */
759  inline void
761  {
762  // delete all branch node children
763  for (char i = 0; i < 8; i++) {
764 
765  if (branch_arg.getChildPtr(0, i) == branch_arg.getChildPtr(1, i)) {
766  // reference was copied - there is only one child instance to be deleted
767  deleteBranchChild(branch_arg, 0, i);
768 
769  // remove pointers from both buffers
770  branch_arg.setChildPtr(0, i, nullptr);
771  branch_arg.setChildPtr(1, i, nullptr);
772  }
773  else {
774  deleteBranchChild(branch_arg, 0, i);
775  deleteBranchChild(branch_arg, 1, i);
776  }
777  }
778  }
779 
780  /** \brief Fetch and add a new branch child to a branch class in current buffer
781  * \param branch_arg: reference to octree branch class
782  * \param child_idx_arg: index to child node
783  * \return pointer of new branch child to this reference
784  */
785  inline BranchNode*
786  createBranchChild(BranchNode& branch_arg, unsigned char child_idx_arg)
787  {
788  BranchNode* new_branch_child = new BranchNode();
789 
790  branch_arg.setChildPtr(
791  buffer_selector_, child_idx_arg, static_cast<OctreeNode*>(new_branch_child));
792 
793  return new_branch_child;
794  }
795 
796  /** \brief Fetch and add a new leaf child to a branch class
797  * \param branch_arg: reference to octree branch class
798  * \param child_idx_arg: index to child node
799  * \return pointer of new leaf child to this reference
800  */
801  inline LeafNode*
802  createLeafChild(BranchNode& branch_arg, unsigned char child_idx_arg)
803  {
804  LeafNode* new_leaf_child = new LeafNode();
805 
806  branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_leaf_child);
807 
808  return new_leaf_child;
809  }
810 
811  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
812  // Recursive octree methods
813  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
814 
815  /** \brief Create a leaf node at octree key. If leaf node does already exist, it is
816  * returned.
817  * \param key_arg: reference to an octree key
818  * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth
819  * indicator
820  * \param branch_arg: current branch node
821  * \param return_leaf_arg: return pointer to leaf container
822  * \param parent_of_leaf_arg: return pointer to parent of leaf node
823  * \param branch_reset_arg: Reset pointer array of current branch
824  * \return depth mask at which leaf node was created/found
825  **/
826  uindex_t
827  createLeafRecursive(const OctreeKey& key_arg,
828  uindex_t depth_mask_arg,
829  BranchNode* branch_arg,
830  LeafNode*& return_leaf_arg,
831  BranchNode*& parent_of_leaf_arg,
832  bool branch_reset_arg = false);
833 
834  /** \brief Recursively search for a given leaf node and return a pointer.
835  * \note If leaf node does not exist, a 0 pointer is returned.
836  * \param key_arg: reference to an octree key
837  * \param depth_mask_arg: depth mask used for octree key analysis and for branch
838  * depth indicator
839  * \param branch_arg: current branch node
840  * \param result_arg: pointer to leaf container class
841  **/
842  void
843  findLeafRecursive(const OctreeKey& key_arg,
844  uindex_t depth_mask_arg,
845  BranchNode* branch_arg,
846  LeafContainerT*& result_arg) const;
847 
848  /** \brief Recursively search and delete leaf node
849  * \param key_arg: reference to an octree key
850  * \param depth_mask_arg: depth mask used for octree key analysis and branch depth
851  * indicator
852  * \param branch_arg: current branch node
853  * \return "true" if branch does not contain any childs; "false" otherwise. This
854  * indicates if current branch can be deleted.
855  **/
856  bool
857  deleteLeafRecursive(const OctreeKey& key_arg,
858  uindex_t depth_mask_arg,
859  BranchNode* branch_arg);
860 
861  /** \brief Recursively explore the octree and output binary octree description
862  * together with a vector of leaf node DataT content.
863  * \param branch_arg: current branch node
864  * \param key_arg: reference to an octree key
865  * \param binary_tree_out_arg: binary output vector
866  * \param leaf_container_vector_arg: vector to return pointers to all leaf container
867  * in the tree.
868  * \param do_XOR_encoding_arg: select if binary tree structure should be generated
869  * based on current octree (false) of based on a XOR comparison between current and
870  * previous octree
871  * \param new_leafs_filter_arg: execute callback only for leaf nodes that did not
872  * exist in preceding buffer
873  **/
874  void
876  BranchNode* branch_arg,
877  OctreeKey& key_arg,
878  std::vector<char>* binary_tree_out_arg,
879  typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
880  bool do_XOR_encoding_arg = false,
881  bool new_leafs_filter_arg = false);
882 
883  /** \brief Rebuild an octree based on binary XOR octree description and DataT objects
884  * for leaf node initialization.
885  * \param branch_arg: current branch node
886  * \param depth_mask_arg: depth mask used for octree key analysis and branch depth
887  * indicator
888  * \param key_arg: reference to an octree key
889  * \param binary_tree_in_it_arg iterator of binary input data
890  * \param binary_tree_in_it_end_arg
891  * \param leaf_container_vector_it_arg: iterator pointing to leaf container pointers
892  * to be added to a leaf node
893  * \param leaf_container_vector_it_end_arg: iterator pointing to leaf container
894  * pointers pointing to last object in input container.
895  * \param branch_reset_arg: Reset pointer array of current branch
896  * \param do_XOR_decoding_arg: select if binary tree structure is based on current
897  * octree (false) of based on a XOR comparison between current and previous octree
898  **/
899  void
901  BranchNode* branch_arg,
902  uindex_t depth_mask_arg,
903  OctreeKey& key_arg,
904  typename std::vector<char>::const_iterator& binary_tree_in_it_arg,
905  typename std::vector<char>::const_iterator& binary_tree_in_it_end_arg,
906  typename std::vector<LeafContainerT*>::const_iterator*
907  leaf_container_vector_it_arg,
908  typename std::vector<LeafContainerT*>::const_iterator*
909  leaf_container_vector_it_end_arg,
910  bool branch_reset_arg = false,
911  bool do_XOR_decoding_arg = false);
912 
913  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
914  // Serialization callbacks
915  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
916 
917  /** \brief Callback executed for every leaf node data during serialization
918  **/
919  virtual void
920  serializeTreeCallback(LeafContainerT&, const OctreeKey&)
921  {}
922 
923  /** \brief Callback executed for every leaf node data during deserialization
924  **/
925  virtual void
926  deserializeTreeCallback(LeafContainerT&, const OctreeKey&)
927  {}
928 
929  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
930  // Helpers
931  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
932 
933  /** \brief Recursively explore the octree and remove unused branch and leaf nodes
934  * \param branch_arg: current branch node
935  **/
936  void
937  treeCleanUpRecursive(BranchNode* branch_arg);
938 
939  /** \brief Test if octree is able to dynamically change its depth. This is required
940  * for adaptive bounding box adjustment.
941  * \return "false" - not resizeable due to XOR serialization
942  **/
943  inline bool
945  {
946  return (false);
947  }
948 
949  /** \brief Prints binary representation of a byte - used for debugging
950  * \param data_arg - byte to be printed to stdout
951  **/
952  inline void
953  printBinary(char data_arg)
954  {
955  unsigned char mask = 1; // Bit mask
956 
957  // Extract the bits
958  for (int i = 0; i < 8; i++) {
959  // Mask each bit in the byte and print it
960  std::cout << ((data_arg & (mask << i)) ? "1" : "0");
961  }
962  std::cout << std::endl;
963  }
964 
965  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
966  // Globals
967  //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
968 
969  /** \brief Amount of leaf nodes **/
970  std::size_t leaf_count_;
971 
972  /** \brief Amount of branch nodes **/
973  std::size_t branch_count_;
974 
975  /** \brief Pointer to root branch node of octree **/
977 
978  /** \brief Depth mask based on octree depth **/
980 
981  /** \brief key range */
983 
984  /** \brief Currently active octree buffer **/
985  unsigned char buffer_selector_;
986 
987  /** \brief flags indicating if unused branches and leafs might exist in previous
988  * buffer **/
990 
991  /** \brief Octree depth */
993 
994  /** \brief Enable dynamic_depth
995  * \note Note that this parameter is ignored in octree2buf! */
997 };
998 } // namespace octree
999 } // namespace pcl
1000 
1001 #ifdef PCL_NO_PRECOMPILE
1002 #include <pcl/octree/impl/octree2buf_base.hpp>
1003 #endif
BufferedBranchNode(const BufferedBranchNode &source)
Copy constructor.
ContainerT & getContainer()
Get reference to container.
OctreeNode * child_node_array_[2][8]
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.
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
Octree container class that does not store any information.
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:80
const ContainerT * getContainerPtr() const
Get const pointer to container.
Definition: octree_nodes.h:153
Abstract octree node class
Definition: octree_nodes.h:58
virtual node_type_t getNodeType() const =0
Pure virtual method for receiving the type of octree node (branch or leaf)
virtual OctreeNode * deepCopy() const =0
Pure virtual method to perform a deep copy of the octree.
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.