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