Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
octree2buf_base.hpp
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_2BUF_BASE_HPP
40#define PCL_OCTREE_2BUF_BASE_HPP
41
42namespace pcl {
43namespace octree {
44//////////////////////////////////////////////////////////////////////////////////////////////
45template <typename LeafContainerT, typename BranchContainerT>
49
50//////////////////////////////////////////////////////////////////////////////////////////////
51template <typename LeafContainerT, typename BranchContainerT>
53{
54 // deallocate tree structure
55 deleteTree();
56 delete (root_node_);
57}
58
59//////////////////////////////////////////////////////////////////////////////////////////////
60template <typename LeafContainerT, typename BranchContainerT>
61void
63 uindex_t max_voxel_index_arg)
64{
65 uindex_t treeDepth;
66
67 if (max_voxel_index_arg <= 0) {
68 PCL_ERROR("[pcl::octree::Octree2BufBase::setMaxVoxelIndex] Max voxel index (%lu) "
69 "must be > 0!\n",
70 max_voxel_index_arg);
71 return;
72 }
73
74 // tree depth == amount of bits of maxVoxels
75 treeDepth =
76 std::max<uindex_t>(std::min<uindex_t>(OctreeKey::maxDepth,
77 std::ceil(std::log2(max_voxel_index_arg))),
78 0);
79
80 // define depthMask_ by setting a single bit to 1 at bit position == tree depth
81 depth_mask_ = (1 << (treeDepth - 1));
82}
83
84//////////////////////////////////////////////////////////////////////////////////////////////
85template <typename LeafContainerT, typename BranchContainerT>
86void
88{
89 if (depth_arg <= 0) {
90 PCL_ERROR(
91 "[pcl::octree::Octree2BufBase::setTreeDepth] Tree depth (%lu) must be > 0!\n",
92 depth_arg);
93 return;
94 }
95
96 // set octree depth
97 octree_depth_ = depth_arg;
98
99 // define depthMask_ by setting a single bit to 1 at bit position == tree depth
100 depth_mask_ = (1 << (depth_arg - 1));
101
102 // define max. keys
103 max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
104}
105
106//////////////////////////////////////////////////////////////////////////////////////////////
107template <typename LeafContainerT, typename BranchContainerT>
108LeafContainerT*
110 uindex_t idx_y_arg,
111 uindex_t idx_z_arg)
112{
113 // generate key
114 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
115
116 // check if key exist in octree
117 return (findLeaf(key));
118}
119
120//////////////////////////////////////////////////////////////////////////////////////////////
121template <typename LeafContainerT, typename BranchContainerT>
122LeafContainerT*
124 uindex_t idx_y_arg,
125 uindex_t idx_z_arg)
126{
127 // generate key
128 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
129
130 // check if key exist in octree
131 return (createLeaf(key));
132}
133
134//////////////////////////////////////////////////////////////////////////////////////////////
135template <typename LeafContainerT, typename BranchContainerT>
136bool
138 uindex_t idx_y_arg,
139 uindex_t idx_z_arg) const
140{
141 // generate key
142 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
143
144 // check if key exist in octree
145 return existLeaf(key);
146}
147
148//////////////////////////////////////////////////////////////////////////////////////////////
149template <typename LeafContainerT, typename BranchContainerT>
150void
152 uindex_t idx_y_arg,
153 uindex_t idx_z_arg)
154{
155 // generate key
156 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
157
158 // free voxel at key
159 return (this->removeLeaf(key));
160}
161
162//////////////////////////////////////////////////////////////////////////////////////////////
163template <typename LeafContainerT, typename BranchContainerT>
164void
166{
167 if (root_node_) {
168 // reset octree
169 deleteBranch(*root_node_);
170 leaf_count_ = 0;
171 branch_count_ = 1;
172
173 tree_dirty_flag_ = false;
174 depth_mask_ = 0;
175 octree_depth_ = 0;
176 }
177}
178
179//////////////////////////////////////////////////////////////////////////////////////////////
180template <typename LeafContainerT, typename BranchContainerT>
181void
183{
184 if (tree_dirty_flag_) {
185 // make sure that all unused branch nodes from previous buffer are deleted
186 treeCleanUpRecursive(root_node_);
187 }
188
189 // switch butter selector
190 buffer_selector_ = !buffer_selector_;
191
192 // reset flags
193 tree_dirty_flag_ = true;
194 leaf_count_ = 0;
195 branch_count_ = 1;
196
197 // we can safely remove children references of root node
198 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
199 root_node_->setChildPtr(buffer_selector_, child_idx, nullptr);
200 }
201}
202
203//////////////////////////////////////////////////////////////////////////////////////////////
204template <typename LeafContainerT, typename BranchContainerT>
205void
207 std::vector<char>& binary_tree_out_arg, bool do_XOR_encoding_arg)
208{
209 OctreeKey new_key;
210
211 // clear binary vector
212 binary_tree_out_arg.clear();
213 binary_tree_out_arg.reserve(this->branch_count_);
214
215 serializeTreeRecursive(
216 root_node_, new_key, &binary_tree_out_arg, nullptr, do_XOR_encoding_arg, false);
217
218 // serializeTreeRecursive cleans-up unused octree nodes in previous octree
219 tree_dirty_flag_ = false;
220}
221
222//////////////////////////////////////////////////////////////////////////////////////////////
223template <typename LeafContainerT, typename BranchContainerT>
224void
226 std::vector<char>& binary_tree_out_arg,
227 std::vector<LeafContainerT*>& leaf_container_vector_arg,
228 bool do_XOR_encoding_arg)
229{
230 OctreeKey new_key;
231
232 // clear output vectors
233 binary_tree_out_arg.clear();
234 leaf_container_vector_arg.clear();
235
236 leaf_container_vector_arg.reserve(leaf_count_);
237 binary_tree_out_arg.reserve(this->branch_count_);
238
239 serializeTreeRecursive(root_node_,
240 new_key,
241 &binary_tree_out_arg,
242 &leaf_container_vector_arg,
243 do_XOR_encoding_arg,
244 false);
245
246 // serializeTreeRecursive cleans-up unused octree nodes in previous octree
247 tree_dirty_flag_ = false;
248}
249
250//////////////////////////////////////////////////////////////////////////////////////////////
251template <typename LeafContainerT, typename BranchContainerT>
252void
254 std::vector<LeafContainerT*>& leaf_container_vector_arg)
255{
256 OctreeKey new_key;
257
258 // clear output vector
259 leaf_container_vector_arg.clear();
260
261 leaf_container_vector_arg.reserve(leaf_count_);
262
263 serializeTreeRecursive(
264 root_node_, new_key, nullptr, &leaf_container_vector_arg, false, false);
265
266 // serializeLeafsRecursive cleans-up unused octree nodes in previous octree
267 tree_dirty_flag_ = false;
268}
269
270//////////////////////////////////////////////////////////////////////////////////////////////
271template <typename LeafContainerT, typename BranchContainerT>
272void
274 std::vector<char>& binary_tree_in_arg, bool do_XOR_decoding_arg)
275{
276 OctreeKey new_key;
277
278 // we will rebuild an octree -> reset leafCount
279 leaf_count_ = 0;
280
281 // iterator for binary tree structure vector
282 auto binary_tree_in_it = binary_tree_in_arg.cbegin();
283 auto binary_tree_in_it_end = binary_tree_in_arg.cend();
284
285 deserializeTreeRecursive(root_node_,
286 depth_mask_,
287 new_key,
288 binary_tree_in_it,
289 binary_tree_in_it_end,
290 nullptr,
291 nullptr,
292 false,
293 do_XOR_decoding_arg);
294
295 // we modified the octree structure -> clean-up/tree-reset might be required
296 tree_dirty_flag_ = false;
297}
298
299//////////////////////////////////////////////////////////////////////////////////////////////
300template <typename LeafContainerT, typename BranchContainerT>
301void
303 std::vector<char>& binary_tree_in_arg,
304 std::vector<LeafContainerT*>& leaf_container_vector_arg,
305 bool do_XOR_decoding_arg)
306{
307 OctreeKey new_key;
308
309 // set data iterator to first element
310 auto leaf_container_vector_it = leaf_container_vector_arg.cbegin();
311
312 // set data iterator to last element
313 auto leaf_container_vector_it_end = leaf_container_vector_arg.cend();
314
315 // we will rebuild an octree -> reset leafCount
316 leaf_count_ = 0;
317
318 // iterator for binary tree structure vector
319 auto binary_tree_in_it = binary_tree_in_arg.cbegin();
320 auto binary_tree_in_it_end = binary_tree_in_arg.cend();
321
322 deserializeTreeRecursive(root_node_,
323 depth_mask_,
324 new_key,
325 binary_tree_in_it,
326 binary_tree_in_it_end,
327 &leaf_container_vector_it,
328 &leaf_container_vector_it_end,
329 false,
330 do_XOR_decoding_arg);
331
332 // we modified the octree structure -> clean-up/tree-reset might be required
333 tree_dirty_flag_ = false;
334}
335
336//////////////////////////////////////////////////////////////////////////////////////////////
337template <typename LeafContainerT, typename BranchContainerT>
338void
340 std::vector<LeafContainerT*>& leaf_container_vector_arg)
341{
342 OctreeKey new_key;
343
344 // clear output vector
345 leaf_container_vector_arg.clear();
346 leaf_container_vector_arg.reserve(leaf_count_);
347
348 serializeTreeRecursive(
349 root_node_, new_key, nullptr, &leaf_container_vector_arg, false, true);
350
351 // serializeLeafsRecursive cleans-up unused octree nodes in previous octree buffer
352 tree_dirty_flag_ = false;
353}
354
355//////////////////////////////////////////////////////////////////////////////////////////////
356template <typename LeafContainerT, typename BranchContainerT>
359 const OctreeKey& key_arg,
360 uindex_t depth_mask_arg,
361 BranchNode* branch_arg,
362 LeafNode*& return_leaf_arg,
363 BranchNode*& parent_of_leaf_arg,
364 bool branch_reset_arg)
365{
366 // branch reset -> this branch has been taken from previous buffer
367 if (branch_reset_arg) {
368 // we can safely remove children references
369 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
370 branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
371 }
372 }
373
374 // find branch child from key
375 unsigned char child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
376
377 if (depth_mask_arg > 1) {
378 // we have not reached maximum tree depth
379 BranchNode* child_branch;
380 bool doNodeReset;
381
382 doNodeReset = false;
383
384 // if required branch does not exist
385 if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
386 // check if we find a branch node reference in previous buffer
388 if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
389 OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx);
390
391 if (child_node->getNodeType() == BRANCH_NODE) {
392 child_branch = static_cast<BranchNode*>(child_node);
393 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
394 }
395 else {
396 // depth has changed.. child in preceding buffer is a leaf node.
397 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
398 child_branch = createBranchChild(*branch_arg, child_idx);
399 }
400
401 // take child branch from previous buffer
402 doNodeReset = true; // reset the branch pointer array of stolen child node
403 }
404 else {
405 // if required branch does not exist -> create it
406 child_branch = createBranchChild(*branch_arg, child_idx);
407 }
408
409 branch_count_++;
410 }
411 // required branch node already exists - use it
412 else
413 child_branch = static_cast<BranchNode*>(
414 branch_arg->getChildPtr(buffer_selector_, child_idx));
415
416 // recursively proceed with indexed child branch
417 return createLeafRecursive(key_arg,
418 depth_mask_arg >> 1,
419 child_branch,
420 return_leaf_arg,
421 parent_of_leaf_arg,
422 doNodeReset);
423 }
424
425 // branch childs are leaf nodes
426 LeafNode* child_leaf;
427 if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
428 // leaf node at child_idx does not exist
429
430 // check if we can take copy a reference from previous buffer
431 if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
432
433 OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx);
434 if (child_node->getNodeType() == LEAF_NODE) {
435 child_leaf = static_cast<LeafNode*>(child_node);
436 child_leaf->getContainer() = LeafContainer(); // Clear contents of leaf
437 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
438 }
439 else {
440 // depth has changed.. child in preceding buffer is a leaf node.
441 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
442 child_leaf = createLeafChild(*branch_arg, child_idx);
443 }
444 leaf_count_++;
445 }
446 else {
447 // if required leaf does not exist -> create it
448 child_leaf = createLeafChild(*branch_arg, child_idx);
449 leaf_count_++;
450 }
451
452 // return leaf node
453 return_leaf_arg = child_leaf;
454 parent_of_leaf_arg = branch_arg;
455 }
456 else {
457 // leaf node already exist
458 return_leaf_arg =
459 static_cast<LeafNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
460 parent_of_leaf_arg = branch_arg;
461 }
462
463 return depth_mask_arg;
464}
465
466//////////////////////////////////////////////////////////////////////////////////////////////
467template <typename LeafContainerT, typename BranchContainerT>
468void
470 const OctreeKey& key_arg,
471 uindex_t depth_mask_arg,
472 BranchNode* branch_arg,
473 LeafContainerT*& result_arg) const
474{
475 // return leaf node
476 unsigned char child_idx;
477
478 // find branch child from key
479 child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
480
481 if (depth_mask_arg > 1) {
482 // we have not reached maximum tree depth
483 BranchNode* child_branch;
484 child_branch =
485 static_cast<BranchNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
486
487 if (child_branch)
488 // recursively proceed with indexed child branch
489 findLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch, result_arg);
490 }
491 else {
492 // we reached leaf node level
493 if (branch_arg->hasChild(buffer_selector_, child_idx)) {
494 // return existing leaf node
495 auto* leaf_node =
496 static_cast<LeafNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
497 result_arg = leaf_node->getContainerPtr();
498 }
499 }
501
502//////////////////////////////////////////////////////////////////////////////////////////////
503template <typename LeafContainerT, typename BranchContainerT>
504bool
506 const OctreeKey& key_arg, uindex_t depth_mask_arg, BranchNode* branch_arg)
507{
508 // index to branch child
509 unsigned char child_idx;
510 // indicates if branch is empty and can be safely removed
511 bool bNoChilds;
512
513 // find branch child from key
514 child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
515
516 if (depth_mask_arg > 1) {
517 // we have not reached maximum tree depth
518 BranchNode* child_branch;
519
520 // next branch child on our path through the tree
521 child_branch =
522 static_cast<BranchNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
524 if (child_branch) {
525 // recursively explore the indexed child branch
526 bool bBranchOccupied =
527 deleteLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch);
528
529 if (!bBranchOccupied) {
530 // child branch does not own any sub-child nodes anymore -> delete child branch
531 deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
532 branch_count_--;
533 }
534 }
535 }
536 else {
537 // our child is a leaf node -> delete it
538 deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
539 leaf_count_--;
540 }
541
542 // check if current branch still owns childs
543 bNoChilds = false;
544 for (child_idx = 0; child_idx < 8; child_idx++) {
545 bNoChilds = branch_arg->hasChild(buffer_selector_, child_idx);
546 if (bNoChilds)
547 break;
548 }
549
550 // return true if current branch can be deleted
551 return (bNoChilds);
552}
553
554//////////////////////////////////////////////////////////////////////////////////////////////
555template <typename LeafContainerT, typename BranchContainerT>
556void
558 BranchNode* branch_arg,
559 OctreeKey& key_arg,
560 std::vector<char>* binary_tree_out_arg,
561 typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
562 bool do_XOR_encoding_arg,
563 bool new_leafs_filter_arg)
564{
565 if (binary_tree_out_arg) {
566 // occupancy bit patterns of branch node (current octree buffer)
567 const char branch_bit_pattern_curr_buffer =
568 getBranchBitPattern(*branch_arg, buffer_selector_);
569 if (do_XOR_encoding_arg) {
570 // occupancy bit patterns of branch node (previous octree buffer)
571 const char branch_bit_pattern_prev_buffer =
572 getBranchBitPattern(*branch_arg, !buffer_selector_);
573 // XOR of current and previous occupancy bit patterns
574 const char node_XOR_bit_pattern =
575 branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
576 // write XOR bit pattern to output vector
577 binary_tree_out_arg->push_back(node_XOR_bit_pattern);
578 }
579 else {
580 // write bit pattern of current buffer to output vector
581 binary_tree_out_arg->push_back(branch_bit_pattern_curr_buffer);
582 }
583 }
584
585 // iterate over all children
586 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
587 if (branch_arg->hasChild(buffer_selector_, child_idx)) {
588 // add current branch voxel to key
589 key_arg.pushBranch(child_idx);
590
591 OctreeNode* child_node = branch_arg->getChildPtr(buffer_selector_, child_idx);
592
593 switch (child_node->getNodeType()) {
594 case BRANCH_NODE: {
595 // recursively proceed with indexed child branch
596 serializeTreeRecursive(static_cast<BranchNode*>(child_node),
597 key_arg,
598 binary_tree_out_arg,
599 leaf_container_vector_arg,
600 do_XOR_encoding_arg,
601 new_leafs_filter_arg);
602 break;
603 }
604 case LEAF_NODE: {
605 auto* child_leaf = static_cast<LeafNode*>(child_node);
606
607 if (new_leafs_filter_arg) {
608 if (!branch_arg->hasChild(!buffer_selector_, child_idx)) {
609 if (leaf_container_vector_arg)
610 leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
611
612 serializeTreeCallback(**child_leaf, key_arg);
613 }
614 }
615 else {
616
617 if (leaf_container_vector_arg)
618 leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
619
620 serializeTreeCallback(**child_leaf, key_arg);
621 }
622
623 break;
624 }
625 default:
626 break;
627 }
628
629 // pop current branch voxel from key
630 key_arg.popBranch();
631 }
632 else if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
633 // delete branch, free memory
634 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
635 }
636 }
637}
638
639//////////////////////////////////////////////////////////////////////////////////////////////
640template <typename LeafContainerT, typename BranchContainerT>
641void
643 BranchNode* branch_arg,
644 uindex_t depth_mask_arg,
645 OctreeKey& key_arg,
646 typename std::vector<char>::const_iterator& binaryTreeIT_arg,
647 typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
648 typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg,
649 typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg,
650 bool branch_reset_arg,
651 bool do_XOR_decoding_arg)
652{
653
654 // branch reset -> this branch has been taken from previous buffer
655 if (branch_reset_arg) {
656 // we can safely remove children references
657 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
658 branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
659 }
660 }
661
662 if (binaryTreeIT_arg != binaryTreeIT_End_arg) {
663 // read branch occupancy bit pattern from vector
664 char nodeBits = *binaryTreeIT_arg++;
665
666 // recover branch occupancy bit pattern
667 char recoveredNodeBits;
668 if (do_XOR_decoding_arg) {
669 recoveredNodeBits =
670 getBranchBitPattern(*branch_arg, !buffer_selector_) ^ nodeBits;
671 }
672 else {
673 recoveredNodeBits = nodeBits;
674 }
675
676 // iterate over all children
677 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
678 // if occupancy bit for child_idx is set..
679 if (recoveredNodeBits & (1 << child_idx)) {
680 // add current branch voxel to key
681 key_arg.pushBranch(child_idx);
682
683 if (depth_mask_arg > 1) {
684 // we have not reached maximum tree depth
685
686 bool doNodeReset = false;
687
688 BranchNode* child_branch;
689
690 // check if we find a branch node reference in previous buffer
691 if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
692
693 if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
694 OctreeNode* child_node =
695 branch_arg->getChildPtr(!buffer_selector_, child_idx);
696
697 if (child_node->getNodeType() == BRANCH_NODE) {
698 child_branch = static_cast<BranchNode*>(child_node);
699 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
700 }
701 else {
702 // depth has changed.. child in preceding buffer is a leaf node.
703 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
704 child_branch = createBranchChild(*branch_arg, child_idx);
705 }
706
707 // take child branch from previous buffer
708 doNodeReset = true; // reset the branch pointer array of stolen child node
709 }
710 else {
711 // if required branch does not exist -> create it
712 child_branch = createBranchChild(*branch_arg, child_idx);
713 }
714
715 branch_count_++;
716 }
717 else {
718 // required branch node already exists - use it
719 child_branch = static_cast<BranchNode*>(
720 branch_arg->getChildPtr(buffer_selector_, child_idx));
721 }
722
723 // recursively proceed with indexed child branch
724 deserializeTreeRecursive(child_branch,
725 depth_mask_arg >> 1,
726 key_arg,
727 binaryTreeIT_arg,
728 binaryTreeIT_End_arg,
729 dataVectorIterator_arg,
730 dataVectorEndIterator_arg,
731 doNodeReset,
732 do_XOR_decoding_arg);
733 }
734 else {
735 // branch childs are leaf nodes
736 LeafNode* child_leaf;
737
738 // check if we can take copy a reference pointer from previous buffer
739 if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
740 // take child leaf node from previous buffer
741 OctreeNode* child_node =
742 branch_arg->getChildPtr(!buffer_selector_, child_idx);
743 if (child_node->getNodeType() == LEAF_NODE) {
744 child_leaf = static_cast<LeafNode*>(child_node);
745 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
746 }
747 else {
748 // depth has changed.. child in preceding buffer is a leaf node.
749 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
750 child_leaf = createLeafChild(*branch_arg, child_idx);
751 }
752 }
753 else {
754 // if required leaf does not exist -> create it
755 child_leaf = createLeafChild(*branch_arg, child_idx);
756 }
757
758 // we reached leaf node level
759
760 if (dataVectorIterator_arg &&
761 (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) {
762 LeafContainerT& container = **child_leaf;
763 container = ***dataVectorIterator_arg;
764 ++*dataVectorIterator_arg;
765 }
766
767 leaf_count_++;
768
769 // execute deserialization callback
770 deserializeTreeCallback(**child_leaf, key_arg);
771 }
772
773 // pop current branch voxel from key
774 key_arg.popBranch();
775 }
776 else if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
777 // remove old branch pointer information in current branch
778 branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
779
780 // remove unused branches in previous buffer
781 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
782 }
783 }
784 }
785}
786
787//////////////////////////////////////////////////////////////////////////////////////////////
788template <typename LeafContainerT, typename BranchContainerT>
789void
791 BranchNode* branch_arg)
792{
793 // occupancy bit pattern of branch node (previous octree buffer)
794 char occupied_children_bit_pattern_prev_buffer =
795 getBranchBitPattern(*branch_arg, !buffer_selector_);
796
797 // XOR of current and previous occupancy bit patterns
798 char node_XOR_bit_pattern = getBranchXORBitPattern(*branch_arg);
799
800 // bit pattern indicating unused octree nodes in previous branch
801 char unused_branches_bit_pattern =
802 node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer;
803
804 // iterate over all children
805 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
806 if (branch_arg->hasChild(buffer_selector_, child_idx)) {
807 OctreeNode* child_node = branch_arg->getChildPtr(buffer_selector_, child_idx);
808
809 switch (child_node->getNodeType()) {
810 case BRANCH_NODE: {
811 // recursively proceed with indexed child branch
812 treeCleanUpRecursive(static_cast<BranchNode*>(child_node));
813 break;
814 }
815 case LEAF_NODE:
816 // leaf level - nothing to do..
817 break;
818 default:
819 break;
820 }
821 }
822
823 // check for unused branches in previous buffer
824 if (unused_branches_bit_pattern & (1 << child_idx)) {
825 // delete branch, free memory
826 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
827 }
828 }
829}
830} // namespace octree
831} // namespace pcl
833#define PCL_INSTANTIATE_Octree2BufBase(T) \
834 template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;
835
836#endif
OctreeNode * getChildPtr(unsigned char buffer_arg, unsigned char index_arg) const
Get child pointer in current branch node.
void setChildPtr(unsigned char buffer_arg, unsigned char index_arg, OctreeNode *newNode_arg)
Set 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.
void serializeLeafs(std::vector< LeafContainerT * > &leaf_container_vector_arg)
Outputs a vector of all DataT elements that are stored within the octree leaf nodes.
void switchBuffers()
Switch buffers and reset current octree structure.
uindex_t createLeafRecursive(const OctreeKey &key_arg, uindex_t depth_mask_arg, BranchNode *branch_arg, LeafNode *&return_leaf_arg, BranchNode *&parent_of_leaf_arg, bool branch_reset_arg=false)
Create a leaf node at octree key.
void 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.
void deleteTree()
Delete the octree structure and its leaf nodes.
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 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.
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 setTreeDepth(uindex_t depth_arg)
Set the maximum depth of the octree.
void setMaxVoxelIndex(uindex_t max_voxel_index_arg)
Set the maximum amount of voxels per dimension.
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).
bool deleteLeafRecursive(const OctreeKey &key_arg, uindex_t depth_mask_arg, BranchNode *branch_arg)
Recursively search and delete leaf node.
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).
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...
Octree2BufBase()
Empty constructor.
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...
Octree key class
Definition octree_key.h:54
void popBranch()
pop child node from octree key
Definition octree_key.h:122
static const unsigned char maxDepth
Definition octree_key.h:142
void pushBranch(unsigned char childIndex)
push a child node to the octree key
Definition octree_key.h:112
unsigned char getChildIdxWithDepthMask(uindex_t depthMask) const
get child node index using depthMask
Definition octree_key.h:134
Abstract octree leaf class
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