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