Point Cloud Library (PCL) 1.14.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 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
283 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();
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 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it =
311 leaf_container_vector_arg.begin();
312
313 // set data iterator to last element
314 typename std::vector<LeafContainerT*>::const_iterator leaf_container_vector_it_end =
315 leaf_container_vector_arg.end();
316
317 // we will rebuild an octree -> reset leafCount
318 leaf_count_ = 0;
319
320 // iterator for binary tree structure vector
321 std::vector<char>::const_iterator binary_tree_in_it = binary_tree_in_arg.begin();
322 std::vector<char>::const_iterator binary_tree_in_it_end = binary_tree_in_arg.end();
323
324 deserializeTreeRecursive(root_node_,
325 depth_mask_,
326 new_key,
327 binary_tree_in_it,
328 binary_tree_in_it_end,
329 &leaf_container_vector_it,
330 &leaf_container_vector_it_end,
331 false,
332 do_XOR_decoding_arg);
333
334 // we modified the octree structure -> clean-up/tree-reset might be required
335 tree_dirty_flag_ = false;
336}
337
338//////////////////////////////////////////////////////////////////////////////////////////////
339template <typename LeafContainerT, typename BranchContainerT>
340void
342 std::vector<LeafContainerT*>& leaf_container_vector_arg)
343{
344 OctreeKey new_key;
345
346 // clear output vector
347 leaf_container_vector_arg.clear();
348 leaf_container_vector_arg.reserve(leaf_count_);
349
350 serializeTreeRecursive(
351 root_node_, new_key, nullptr, &leaf_container_vector_arg, false, true);
352
353 // serializeLeafsRecursive cleans-up unused octree nodes in previous octree buffer
354 tree_dirty_flag_ = false;
355}
356
357//////////////////////////////////////////////////////////////////////////////////////////////
358template <typename LeafContainerT, typename BranchContainerT>
361 const OctreeKey& key_arg,
362 uindex_t depth_mask_arg,
363 BranchNode* branch_arg,
364 LeafNode*& return_leaf_arg,
365 BranchNode*& parent_of_leaf_arg,
366 bool branch_reset_arg)
367{
368 // branch reset -> this branch has been taken from previous buffer
369 if (branch_reset_arg) {
370 // we can safely remove children references
371 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
372 branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
373 }
374 }
375
376 // find branch child from key
377 unsigned char child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
378
379 if (depth_mask_arg > 1) {
380 // we have not reached maximum tree depth
381 BranchNode* child_branch;
382 bool doNodeReset;
383
384 doNodeReset = false;
385
386 // if required branch does not exist
387 if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
388 // check if we find a branch node reference in previous buffer
389
390 if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
391 OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx);
392
393 if (child_node->getNodeType() == BRANCH_NODE) {
394 child_branch = static_cast<BranchNode*>(child_node);
395 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
396 }
397 else {
398 // depth has changed.. child in preceding buffer is a leaf node.
399 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
400 child_branch = createBranchChild(*branch_arg, child_idx);
401 }
402
403 // take child branch from previous buffer
404 doNodeReset = true; // reset the branch pointer array of stolen child node
405 }
406 else {
407 // if required branch does not exist -> create it
408 child_branch = createBranchChild(*branch_arg, child_idx);
409 }
410
411 branch_count_++;
412 }
413 // required branch node already exists - use it
414 else
415 child_branch = static_cast<BranchNode*>(
416 branch_arg->getChildPtr(buffer_selector_, child_idx));
417
418 // recursively proceed with indexed child branch
419 return createLeafRecursive(key_arg,
420 depth_mask_arg >> 1,
421 child_branch,
422 return_leaf_arg,
423 parent_of_leaf_arg,
424 doNodeReset);
425 }
426
427 // branch childs are leaf nodes
428 LeafNode* child_leaf;
429 if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
430 // leaf node at child_idx does not exist
431
432 // check if we can take copy a reference from previous buffer
433 if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
434
435 OctreeNode* child_node = branch_arg->getChildPtr(!buffer_selector_, child_idx);
436 if (child_node->getNodeType() == LEAF_NODE) {
437 child_leaf = static_cast<LeafNode*>(child_node);
438 child_leaf->getContainer() = LeafContainer(); // Clear contents of leaf
439 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
440 }
441 else {
442 // depth has changed.. child in preceding buffer is a leaf node.
443 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
444 child_leaf = createLeafChild(*branch_arg, child_idx);
445 }
446 leaf_count_++;
447 }
448 else {
449 // if required leaf does not exist -> create it
450 child_leaf = createLeafChild(*branch_arg, child_idx);
451 leaf_count_++;
452 }
453
454 // return leaf node
455 return_leaf_arg = child_leaf;
456 parent_of_leaf_arg = branch_arg;
458 else {
459 // leaf node already exist
460 return_leaf_arg =
461 static_cast<LeafNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
462 parent_of_leaf_arg = branch_arg;
463 }
464
465 return depth_mask_arg;
466}
467
468//////////////////////////////////////////////////////////////////////////////////////////////
469template <typename LeafContainerT, typename BranchContainerT>
470void
472 const OctreeKey& key_arg,
473 uindex_t depth_mask_arg,
474 BranchNode* branch_arg,
475 LeafContainerT*& result_arg) const
476{
477 // return leaf node
478 unsigned char child_idx;
479
480 // find branch child from key
481 child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
483 if (depth_mask_arg > 1) {
484 // we have not reached maximum tree depth
485 BranchNode* child_branch;
486 child_branch =
487 static_cast<BranchNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
488
489 if (child_branch)
490 // recursively proceed with indexed child branch
491 findLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch, result_arg);
493 else {
494 // we reached leaf node level
495 if (branch_arg->hasChild(buffer_selector_, child_idx)) {
496 // return existing leaf node
497 auto* leaf_node =
498 static_cast<LeafNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
499 result_arg = leaf_node->getContainerPtr();
501 }
502}
503
504//////////////////////////////////////////////////////////////////////////////////////////////
505template <typename LeafContainerT, typename BranchContainerT>
506bool
508 const OctreeKey& key_arg, uindex_t depth_mask_arg, BranchNode* branch_arg)
509{
510 // index to branch child
511 unsigned char child_idx;
512 // indicates if branch is empty and can be safely removed
513 bool bNoChilds;
514
515 // find branch child from key
516 child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
517
518 if (depth_mask_arg > 1) {
519 // we have not reached maximum tree depth
520 BranchNode* child_branch;
521
522 // next branch child on our path through the tree
523 child_branch =
524 static_cast<BranchNode*>(branch_arg->getChildPtr(buffer_selector_, child_idx));
525
526 if (child_branch) {
527 // recursively explore the indexed child branch
528 bool bBranchOccupied =
529 deleteLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch);
530
531 if (!bBranchOccupied) {
532 // child branch does not own any sub-child nodes anymore -> delete child branch
533 deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
534 branch_count_--;
535 }
536 }
537 }
538 else {
539 // our child is a leaf node -> delete it
540 deleteBranchChild(*branch_arg, buffer_selector_, child_idx);
541 leaf_count_--;
542 }
543
544 // check if current branch still owns childs
545 bNoChilds = false;
546 for (child_idx = 0; child_idx < 8; child_idx++) {
547 bNoChilds = branch_arg->hasChild(buffer_selector_, child_idx);
548 if (bNoChilds)
549 break;
550 }
551
552 // return true if current branch can be deleted
553 return (bNoChilds);
554}
555
556//////////////////////////////////////////////////////////////////////////////////////////////
557template <typename LeafContainerT, typename BranchContainerT>
558void
560 BranchNode* branch_arg,
561 OctreeKey& key_arg,
562 std::vector<char>* binary_tree_out_arg,
563 typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
564 bool do_XOR_encoding_arg,
565 bool new_leafs_filter_arg)
566{
567 if (binary_tree_out_arg) {
568 // occupancy bit patterns of branch node (current octree buffer)
569 const char branch_bit_pattern_curr_buffer =
570 getBranchBitPattern(*branch_arg, buffer_selector_);
571 if (do_XOR_encoding_arg) {
572 // occupancy bit patterns of branch node (previous octree buffer)
573 const char branch_bit_pattern_prev_buffer =
574 getBranchBitPattern(*branch_arg, !buffer_selector_);
575 // XOR of current and previous occupancy bit patterns
576 const char node_XOR_bit_pattern =
577 branch_bit_pattern_curr_buffer ^ branch_bit_pattern_prev_buffer;
578 // write XOR bit pattern to output vector
579 binary_tree_out_arg->push_back(node_XOR_bit_pattern);
580 }
581 else {
582 // write bit pattern of current buffer to output vector
583 binary_tree_out_arg->push_back(branch_bit_pattern_curr_buffer);
584 }
585 }
586
587 // iterate over all children
588 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
589 if (branch_arg->hasChild(buffer_selector_, child_idx)) {
590 // add current branch voxel to key
591 key_arg.pushBranch(child_idx);
592
593 OctreeNode* child_node = branch_arg->getChildPtr(buffer_selector_, child_idx);
594
595 switch (child_node->getNodeType()) {
596 case BRANCH_NODE: {
597 // recursively proceed with indexed child branch
598 serializeTreeRecursive(static_cast<BranchNode*>(child_node),
599 key_arg,
600 binary_tree_out_arg,
601 leaf_container_vector_arg,
602 do_XOR_encoding_arg,
603 new_leafs_filter_arg);
604 break;
605 }
606 case LEAF_NODE: {
607 auto* child_leaf = static_cast<LeafNode*>(child_node);
608
609 if (new_leafs_filter_arg) {
610 if (!branch_arg->hasChild(!buffer_selector_, child_idx)) {
611 if (leaf_container_vector_arg)
612 leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
613
614 serializeTreeCallback(**child_leaf, key_arg);
615 }
616 }
617 else {
618
619 if (leaf_container_vector_arg)
620 leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
621
622 serializeTreeCallback(**child_leaf, key_arg);
623 }
624
625 break;
626 }
627 default:
628 break;
629 }
630
631 // pop current branch voxel from key
632 key_arg.popBranch();
633 }
634 else if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
635 // delete branch, free memory
636 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
637 }
638 }
639}
640
641//////////////////////////////////////////////////////////////////////////////////////////////
642template <typename LeafContainerT, typename BranchContainerT>
643void
645 BranchNode* branch_arg,
646 uindex_t depth_mask_arg,
647 OctreeKey& key_arg,
648 typename std::vector<char>::const_iterator& binaryTreeIT_arg,
649 typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
650 typename std::vector<LeafContainerT*>::const_iterator* dataVectorIterator_arg,
651 typename std::vector<LeafContainerT*>::const_iterator* dataVectorEndIterator_arg,
652 bool branch_reset_arg,
653 bool do_XOR_decoding_arg)
654{
655
656 // branch reset -> this branch has been taken from previous buffer
657 if (branch_reset_arg) {
658 // we can safely remove children references
659 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
660 branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
661 }
662 }
663
664 if (binaryTreeIT_arg != binaryTreeIT_End_arg) {
665 // read branch occupancy bit pattern from vector
666 char nodeBits = *binaryTreeIT_arg++;
667
668 // recover branch occupancy bit pattern
669 char recoveredNodeBits;
670 if (do_XOR_decoding_arg) {
671 recoveredNodeBits =
672 getBranchBitPattern(*branch_arg, !buffer_selector_) ^ nodeBits;
673 }
674 else {
675 recoveredNodeBits = nodeBits;
676 }
677
678 // iterate over all children
679 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
680 // if occupancy bit for child_idx is set..
681 if (recoveredNodeBits & (1 << child_idx)) {
682 // add current branch voxel to key
683 key_arg.pushBranch(child_idx);
684
685 if (depth_mask_arg > 1) {
686 // we have not reached maximum tree depth
687
688 bool doNodeReset = false;
689
690 BranchNode* child_branch;
691
692 // check if we find a branch node reference in previous buffer
693 if (!branch_arg->hasChild(buffer_selector_, child_idx)) {
694
695 if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
696 OctreeNode* child_node =
697 branch_arg->getChildPtr(!buffer_selector_, child_idx);
698
699 if (child_node->getNodeType() == BRANCH_NODE) {
700 child_branch = static_cast<BranchNode*>(child_node);
701 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
702 }
703 else {
704 // depth has changed.. child in preceding buffer is a leaf node.
705 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
706 child_branch = createBranchChild(*branch_arg, child_idx);
707 }
708
709 // take child branch from previous buffer
710 doNodeReset = true; // reset the branch pointer array of stolen child node
711 }
712 else {
713 // if required branch does not exist -> create it
714 child_branch = createBranchChild(*branch_arg, child_idx);
715 }
716
717 branch_count_++;
718 }
719 else {
720 // required branch node already exists - use it
721 child_branch = static_cast<BranchNode*>(
722 branch_arg->getChildPtr(buffer_selector_, child_idx));
723 }
724
725 // recursively proceed with indexed child branch
726 deserializeTreeRecursive(child_branch,
727 depth_mask_arg >> 1,
728 key_arg,
729 binaryTreeIT_arg,
730 binaryTreeIT_End_arg,
731 dataVectorIterator_arg,
732 dataVectorEndIterator_arg,
733 doNodeReset,
734 do_XOR_decoding_arg);
735 }
736 else {
737 // branch childs are leaf nodes
738 LeafNode* child_leaf;
739
740 // check if we can take copy a reference pointer from previous buffer
741 if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
742 // take child leaf node from previous buffer
743 OctreeNode* child_node =
744 branch_arg->getChildPtr(!buffer_selector_, child_idx);
745 if (child_node->getNodeType() == LEAF_NODE) {
746 child_leaf = static_cast<LeafNode*>(child_node);
747 branch_arg->setChildPtr(buffer_selector_, child_idx, child_node);
748 }
749 else {
750 // depth has changed.. child in preceding buffer is a leaf node.
751 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
752 child_leaf = createLeafChild(*branch_arg, child_idx);
753 }
754 }
755 else {
756 // if required leaf does not exist -> create it
757 child_leaf = createLeafChild(*branch_arg, child_idx);
758 }
759
760 // we reached leaf node level
761
762 if (dataVectorIterator_arg &&
763 (*dataVectorIterator_arg != *dataVectorEndIterator_arg)) {
764 LeafContainerT& container = **child_leaf;
765 container = ***dataVectorIterator_arg;
766 ++*dataVectorIterator_arg;
767 }
768
769 leaf_count_++;
770
771 // execute deserialization callback
772 deserializeTreeCallback(**child_leaf, key_arg);
773 }
774
775 // pop current branch voxel from key
776 key_arg.popBranch();
777 }
778 else if (branch_arg->hasChild(!buffer_selector_, child_idx)) {
779 // remove old branch pointer information in current branch
780 branch_arg->setChildPtr(buffer_selector_, child_idx, nullptr);
781
782 // remove unused branches in previous buffer
783 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
784 }
785 }
786 }
787}
788
789//////////////////////////////////////////////////////////////////////////////////////////////
790template <typename LeafContainerT, typename BranchContainerT>
791void
793 BranchNode* branch_arg)
794{
795 // occupancy bit pattern of branch node (previous octree buffer)
796 char occupied_children_bit_pattern_prev_buffer =
797 getBranchBitPattern(*branch_arg, !buffer_selector_);
798
799 // XOR of current and previous occupancy bit patterns
800 char node_XOR_bit_pattern = getBranchXORBitPattern(*branch_arg);
801
802 // bit pattern indicating unused octree nodes in previous branch
803 char unused_branches_bit_pattern =
804 node_XOR_bit_pattern & occupied_children_bit_pattern_prev_buffer;
805
806 // iterate over all children
807 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
808 if (branch_arg->hasChild(buffer_selector_, child_idx)) {
809 OctreeNode* child_node = branch_arg->getChildPtr(buffer_selector_, child_idx);
810
811 switch (child_node->getNodeType()) {
812 case BRANCH_NODE: {
813 // recursively proceed with indexed child branch
814 treeCleanUpRecursive(static_cast<BranchNode*>(child_node));
815 break;
816 }
817 case LEAF_NODE:
818 // leaf level - nothing to do..
819 break;
820 default:
821 break;
822 }
823 }
824
825 // check for unused branches in previous buffer
826 if (unused_branches_bit_pattern & (1 << child_idx)) {
827 // delete branch, free memory
828 deleteBranchChild(*branch_arg, !buffer_selector_, child_idx);
829 }
830 }
831}
832} // namespace octree
833} // namespace pcl
834
835#define PCL_INSTANTIATE_Octree2BufBase(T) \
836 template class PCL_EXPORTS pcl::octree::Octree2BufBase<T>;
837
838#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 container class that does store a vector of point indices.
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