Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
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
50namespace pcl {
51namespace octree {
52
53template <typename ContainerT>
55
56public:
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 {
71 for (unsigned char b = 0; b < 2; ++b) {
72 for (unsigned char i = 0; i < 8; ++i) {
73 if (source_arg.child_node_array_[b][i]) {
74 child_node_array_[b][i] = source_arg.child_node_array_[b][i]->deepCopy();
75 }
76 }
77 }
78 return (*this);
79 }
80
81 /** \brief Empty constructor. */
82 ~BufferedBranchNode() override = default;
83
84 /** \brief Method to perform a deep copy of the octree */
86 deepCopy() const override
87 {
88 return new BufferedBranchNode(*this);
89 }
90
91 /** \brief Get child pointer in current branch node
92 * \param buffer_arg: buffer selector, must be less than 2
93 * \param index_arg: index of child in node, must be less than 8
94 * \return pointer to child node
95 */
96 inline OctreeNode*
97 getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const
98 {
99 assert((buffer_arg < 2) && (index_arg < 8));
100 return child_node_array_[buffer_arg][index_arg];
101 }
102
103 /** \brief Set child pointer in current branch node
104 * \param buffer_arg: buffer selector, must be less than 2
105 * \param index_arg: index of child in node, must be less than 8
106 * \param newNode_arg: pointer to new child node
107 */
108 inline void
109 setChildPtr(unsigned char buffer_arg,
110 unsigned char index_arg,
111 OctreeNode* newNode_arg)
112 {
113 assert((buffer_arg < 2) && (index_arg < 8));
114 child_node_array_[buffer_arg][index_arg] = newNode_arg;
115 }
116
117 /** \brief Check if branch is pointing to a particular child node
118 * \param buffer_arg: buffer selector, must be less than 2
119 * \param index_arg: index of child in node, must be less than 8
120 * \return "true" if pointer to child node exists; "false" otherwise
121 */
122 inline bool
123 hasChild(unsigned char buffer_arg, unsigned char index_arg) const
124 {
125 assert((buffer_arg < 2) && (index_arg < 8));
126 return (child_node_array_[buffer_arg][index_arg] != nullptr);
127 }
128
129 /** \brief Get the type of octree node. Returns LEAVE_NODE type */
131 getNodeType() const override
132 {
133 return BRANCH_NODE;
134 }
135
136 /** \brief Reset branch node container for every branch buffer. */
137 inline void
139 {
141 }
142
143 /** \brief Get const pointer to container */
144 const ContainerT*
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&
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
199protected:
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 */
220template <typename LeafContainerT = index_t,
221 typename BranchContainerT = OctreeContainerEmpty>
223
224public:
226
227 // iterators are friends
228 friend class OctreeIteratorBase<OctreeT>;
229 friend class OctreeDepthFirstIterator<OctreeT>;
233
236
237 using BranchContainer = BranchContainerT;
238 using LeafContainer = LeafContainerT;
239
240 // Octree default iterators
244 begin(uindex_t max_depth_arg = 0)
245 {
246 return Iterator(this, max_depth_arg);
247 };
248 const Iterator
250 {
251 return Iterator();
252 };
253
254 // Octree leaf node iterators
255 // The previous deprecated names
256 // LeafNodeIterator and ConstLeafNodeIterator are deprecated.
257 // Please use LeafNodeDepthFirstIterator and ConstLeafNodeDepthFirstIterator instead.
260
261 // The currently valid names
266 leaf_depth_begin(uindex_t max_depth_arg = 0)
267 {
268 return LeafNodeDepthFirstIterator(this, max_depth_arg);
269 };
270
273 {
275 };
276
277 // Octree depth-first iterators
281 depth_begin(uindex_t maxDepth_arg = 0)
282 {
283 return DepthFirstIterator(this, maxDepth_arg);
284 };
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. */
325
326 /** \brief Empty deconstructor. */
327 virtual ~Octree2BufBase();
328
329 /** \brief Copy constructor. */
341
342 /** \brief Copy constructor. */
343 inline Octree2BufBase&
345 {
346 leaf_count_ = source.leaf_count_;
348 root_node_ = new (BranchNode)(*(source.root_node_));
349 depth_mask_ = source.depth_mask_;
350 max_key_ = source.max_key_;
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
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
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
445
446 /** \brief Delete the octree structure in the current buffer. */
447 inline void
454
455 /** \brief Switch buffers and reset current octree structure. */
456 void
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
527protected:
528 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
529 // Protected octree methods based on octree keys
530 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
531
532 /** \brief Retrieve root node */
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
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 **/
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
BufferedBranchNode * deepCopy() const override
Method to perform a deep copy of the octree.
OctreeMatrix< OctreeNode *, 2, 8 > child_node_array_
BufferedBranchNode & operator=(const BufferedBranchNode &source_arg)
Copy operator.
BufferedBranchNode(const BufferedBranchNode &source)
Copy constructor.
ContainerT & operator*()
Get reference to container.
const ContainerT & getContainer() const
Get const reference to container.
std::array< std::array< T, COL >, ROW > OctreeMatrix
ContainerT * getContainerPtr()
Get pointer to container.
ContainerT & getContainer()
Get reference to container.
node_type_t getNodeType() const override
Get the type of octree node.
OctreeNode * getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const
Get child pointer in current branch node.
const ContainerT & operator*() const
Get const reference to container.
const ContainerT * operator->() const
Get const pointer to container.
~BufferedBranchNode() override=default
Empty constructor.
const ContainerT * getContainerPtr() const
Get const pointer to container.
void reset()
Reset branch node container for every branch buffer.
void setChildPtr(unsigned char buffer_arg, unsigned char index_arg, OctreeNode *newNode_arg)
Set child pointer in current branch node.
BufferedBranchNode()
Empty constructor.
ContainerT * operator->()
Get pointer to container.
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
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.
void serializeLeafs(std::vector< LeafContainerT * > &leaf_container_vector_arg)
Outputs a vector of all DataT elements that are stored within the octree leaf nodes.
BranchNode * createBranchChild(BranchNode &branch_arg, unsigned char child_idx_arg)
Fetch and add a new branch child to a branch class in current buffer.
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.
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.
LeafContainerT * findLeaf(const OctreeKey &key_arg) const
Find leaf node.
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.
LeafNode * createLeafChild(BranchNode &branch_arg, unsigned char child_idx_arg)
Fetch and add a new leaf child to a branch class.
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
OctreeNode * getBranchChildPtr(const BranchNode &branch_arg, unsigned char child_idx_arg) const
Retrieve a child node pointer for child node at child_idx.
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)
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
Octree2BufBase< LeafContainerT, BranchContainerT > OctreeT
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()
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).
bool existLeaf(const OctreeKey &key_arg) const
Check if leaf doesn't exist in the octree.
Octree2BufBase & operator=(const Octree2BufBase &source)
Copy constructor.
uindex_t depth_mask_
Depth mask based on octree depth
OctreeNode * getRootNode() const
Retrieve root node.
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.
LeafContainerT * createLeaf(const OctreeKey &key_arg)
Create a leaf node.
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
Abstract octree leaf class
const ContainerT * getContainerPtr() const
Get const pointer to container.
Abstract octree node class
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.