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