Point Cloud Library (PCL) 1.13.1
Loading...
Searching...
No Matches
octree_base.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, 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_BASE_HPP
40#define PCL_OCTREE_BASE_HPP
41
42#include <vector>
43
44namespace pcl {
45namespace octree {
46//////////////////////////////////////////////////////////////////////////////////////////////
47template <typename LeafContainerT, typename BranchContainerT>
49: leaf_count_(0)
50, branch_count_(1)
51, root_node_(new BranchNode())
52, depth_mask_(0)
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 tree_depth;
73
74 if (max_voxel_index_arg <= 0) {
75 PCL_ERROR("[pcl::octree::OctreeBase::setMaxVoxelIndex] Max voxel index (%lu) must "
76 "be > 0!\n",
77 max_voxel_index_arg);
78 return;
79 }
80
81 // tree depth == bitlength of maxVoxels
82 tree_depth =
83 std::min(static_cast<uindex_t>(OctreeKey::maxDepth),
84 static_cast<uindex_t>(std::ceil(std::log2(max_voxel_index_arg))));
85
86 setTreeDepth(tree_depth);
87}
88
89//////////////////////////////////////////////////////////////////////////////////////////////
90template <typename LeafContainerT, typename BranchContainerT>
91void
93{
94 if (depth_arg <= 0) {
95 PCL_ERROR("[pcl::octree::OctreeBase::setTreeDepth] Tree depth (%lu) must be > 0!\n",
96 depth_arg);
97 return;
98 }
99 if (depth_arg > OctreeKey::maxDepth) {
100 PCL_ERROR("[pcl::octree::OctreeBase::setTreeDepth] Tree depth (%lu) must be <= max "
101 "depth(%lu)!\n",
102 depth_arg,
104 return;
105 }
106
107 // set octree depth
108 octree_depth_ = depth_arg;
109
110 // define depth_mask_ by setting a single bit to 1 at bit position == tree depth
111 depth_mask_ = (1 << (depth_arg - 1));
112
113 // define max_key_
114 max_key_.x = max_key_.y = max_key_.z = (1 << depth_arg) - 1;
115}
116
117//////////////////////////////////////////////////////////////////////////////////////////////
118template <typename LeafContainerT, typename BranchContainerT>
119LeafContainerT*
121 uindex_t idx_y_arg,
122 uindex_t idx_z_arg) const
123{
124 // generate key
125 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
126
127 // find the leaf node addressed by key
128 return (findLeaf(key));
129}
130
131//////////////////////////////////////////////////////////////////////////////////////////////
132template <typename LeafContainerT, typename BranchContainerT>
133LeafContainerT*
135 uindex_t idx_y_arg,
136 uindex_t idx_z_arg)
137{
138 // generate key
139 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
140
141 // create a leaf node addressed by key
142 return (createLeaf(key));
143}
144
145//////////////////////////////////////////////////////////////////////////////////////////////
146template <typename LeafContainerT, typename BranchContainerT>
147bool
149 uindex_t idx_y_arg,
150 uindex_t idx_z_arg) const
151{
152 // generate key
153 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
154
155 // check if key exist in octree
156 return (existLeaf(key));
157}
158
159//////////////////////////////////////////////////////////////////////////////////////////////
160template <typename LeafContainerT, typename BranchContainerT>
161void
163 uindex_t idx_y_arg,
164 uindex_t idx_z_arg)
165{
166 // generate key
167 OctreeKey key(idx_x_arg, idx_y_arg, idx_z_arg);
168
169 // delete the leaf node addressed by key
170 deleteLeafRecursive(key, depth_mask_, root_node_);
171}
172
173//////////////////////////////////////////////////////////////////////////////////////////////
174template <typename LeafContainerT, typename BranchContainerT>
175void
177{
178
179 if (root_node_) {
180 // reset octree
181 deleteBranch(*root_node_);
182 leaf_count_ = 0;
183 branch_count_ = 1;
184 }
185}
186
187//////////////////////////////////////////////////////////////////////////////////////////////
188template <typename LeafContainerT, typename BranchContainerT>
189void
191 std::vector<char>& binary_tree_out_arg) const
192{
193
194 OctreeKey new_key;
195
196 // clear binary vector
197 binary_tree_out_arg.clear();
198 binary_tree_out_arg.reserve(this->branch_count_);
199
200 serializeTreeRecursive(root_node_, new_key, &binary_tree_out_arg, nullptr);
201}
202
203//////////////////////////////////////////////////////////////////////////////////////////////
204template <typename LeafContainerT, typename BranchContainerT>
205void
207 std::vector<char>& binary_tree_out_arg,
208 std::vector<LeafContainerT*>& leaf_container_vector_arg) const
209{
210
211 OctreeKey new_key;
212
213 // clear output vectors
214 binary_tree_out_arg.clear();
215 leaf_container_vector_arg.clear();
216
217 binary_tree_out_arg.reserve(this->branch_count_);
218 leaf_container_vector_arg.reserve(this->leaf_count_);
219
220 serializeTreeRecursive(
221 root_node_, new_key, &binary_tree_out_arg, &leaf_container_vector_arg);
222}
223
224//////////////////////////////////////////////////////////////////////////////////////////////
225template <typename LeafContainerT, typename BranchContainerT>
226void
228 std::vector<LeafContainerT*>& leaf_container_vector_arg)
229{
230 OctreeKey new_key;
231
232 // clear output vector
233 leaf_container_vector_arg.clear();
234
235 leaf_container_vector_arg.reserve(this->leaf_count_);
236
237 serializeTreeRecursive(root_node_, new_key, nullptr, &leaf_container_vector_arg);
238}
239
240//////////////////////////////////////////////////////////////////////////////////////////////
241template <typename LeafContainerT, typename BranchContainerT>
242void
244 std::vector<char>& binary_tree_out_arg)
245{
246 OctreeKey new_key;
247
248 // free existing tree before tree rebuild
249 deleteTree();
250
251 // iterator for binary tree structure vector
252 std::vector<char>::const_iterator binary_tree_out_it = binary_tree_out_arg.begin();
253 std::vector<char>::const_iterator binary_tree_out_it_end = binary_tree_out_arg.end();
254
255 deserializeTreeRecursive(root_node_,
256 depth_mask_,
257 new_key,
258 binary_tree_out_it,
259 binary_tree_out_it_end,
260 nullptr,
261 nullptr);
262}
263
264//////////////////////////////////////////////////////////////////////////////////////////////
265template <typename LeafContainerT, typename BranchContainerT>
266void
268 std::vector<char>& binary_tree_in_arg,
269 std::vector<LeafContainerT*>& leaf_container_vector_arg)
270{
271 OctreeKey new_key;
272
273 // set data iterator to first element
274 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it =
275 leaf_container_vector_arg.begin();
276
277 // set data iterator to last element
278 typename std::vector<LeafContainerT*>::const_iterator leaf_vector_it_end =
279 leaf_container_vector_arg.end();
280
281 // free existing tree before tree rebuild
282 deleteTree();
283
284 // iterator for binary tree structure vector
285 std::vector<char>::const_iterator binary_tree_input_it = binary_tree_in_arg.begin();
286 std::vector<char>::const_iterator binary_tree_input_it_end = binary_tree_in_arg.end();
287
288 deserializeTreeRecursive(root_node_,
289 depth_mask_,
290 new_key,
291 binary_tree_input_it,
292 binary_tree_input_it_end,
293 &leaf_vector_it,
294 &leaf_vector_it_end);
295}
296
297//////////////////////////////////////////////////////////////////////////////////////////////
298template <typename LeafContainerT, typename BranchContainerT>
301 const OctreeKey& key_arg,
302 uindex_t depth_mask_arg,
303 BranchNode* branch_arg,
304 LeafNode*& return_leaf_arg,
305 BranchNode*& parent_of_leaf_arg)
306{
307 // index to branch child
308 unsigned char child_idx;
309
310 // find branch child from key
311 child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
312
313 OctreeNode* child_node = (*branch_arg)[child_idx];
315 if (!child_node) {
316 if ((!dynamic_depth_enabled_) && (depth_mask_arg > 1)) {
317 // if required branch does not exist -> create it
318 BranchNode* childBranch = createBranchChild(*branch_arg, child_idx);
319
320 branch_count_++;
321
322 // recursively proceed with indexed child branch
323 return createLeafRecursive(key_arg,
324 depth_mask_arg >> 1,
325 childBranch,
326 return_leaf_arg,
327 parent_of_leaf_arg);
328 }
329 // if leaf node at child_idx does not exist
330 LeafNode* leaf_node = createLeafChild(*branch_arg, child_idx);
331 return_leaf_arg = leaf_node;
332 parent_of_leaf_arg = branch_arg;
333 this->leaf_count_++;
334 }
335 else {
336 // Node exists already
337 switch (child_node->getNodeType()) {
338 case BRANCH_NODE:
339 // recursively proceed with indexed child branch
340 return createLeafRecursive(key_arg,
341 depth_mask_arg >> 1,
342 static_cast<BranchNode*>(child_node),
343 return_leaf_arg,
344 parent_of_leaf_arg);
345 break;
346
348 return_leaf_arg = static_cast<LeafNode*>(child_node);
349 parent_of_leaf_arg = branch_arg;
350 break;
351 }
352 }
354 return (depth_mask_arg >> 1);
355}
356
357//////////////////////////////////////////////////////////////////////////////////////////////
358template <typename LeafContainerT, typename BranchContainerT>
359void
361 const OctreeKey& key_arg,
362 uindex_t depth_mask_arg,
363 BranchNode* branch_arg,
364 LeafContainerT*& result_arg) const
365{
366 // index to branch child
367 unsigned char child_idx;
368
369 // find branch child from key
370 child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
371
372 OctreeNode* child_node = (*branch_arg)[child_idx];
373
374 if (child_node) {
375 switch (child_node->getNodeType()) {
376 case BRANCH_NODE:
377 // we have not reached maximum tree depth
378 BranchNode* child_branch;
379 child_branch = static_cast<BranchNode*>(child_node);
380
381 findLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch, result_arg);
382 break;
383
384 case LEAF_NODE:
385 // return existing leaf node
386 LeafNode* child_leaf;
387 child_leaf = static_cast<LeafNode*>(child_node);
388
389 result_arg = child_leaf->getContainerPtr();
390 break;
391 }
393}
394
395//////////////////////////////////////////////////////////////////////////////////////////////
396template <typename LeafContainerT, typename BranchContainerT>
397bool
399 const OctreeKey& key_arg, uindex_t depth_mask_arg, BranchNode* branch_arg)
401 // index to branch child
402 unsigned char child_idx;
403 // indicates if branch has children, if so, it can't be removed
404 bool b_has_children;
405
406 // find branch child from key
407 child_idx = key_arg.getChildIdxWithDepthMask(depth_mask_arg);
408
409 OctreeNode* child_node = (*branch_arg)[child_idx];
410
411 if (child_node) {
412 switch (child_node->getNodeType()) {
413
414 case BRANCH_NODE:
415 BranchNode* child_branch;
416 child_branch = static_cast<BranchNode*>(child_node);
417
418 // recursively explore the indexed child branch
419 b_has_children = deleteLeafRecursive(key_arg, depth_mask_arg >> 1, child_branch);
420
421 if (!b_has_children) {
422 // child branch does not own any sub-child nodes anymore -> delete child branch
423 deleteBranchChild(*branch_arg, child_idx);
424 branch_count_--;
425 }
426 break;
427
428 case LEAF_NODE:
429 // return existing leaf node
430
431 // our child is a leaf node -> delete it
432 deleteBranchChild(*branch_arg, child_idx);
433 this->leaf_count_--;
434 break;
435 }
436 }
437
438 // check if current branch still owns children
439 b_has_children = false;
440 for (child_idx = 0; (!b_has_children) && (child_idx < 8); child_idx++) {
441 b_has_children = branch_arg->hasChild(child_idx);
442 }
443 // return false if current branch can be deleted
444 return (b_has_children);
445}
446
447//////////////////////////////////////////////////////////////////////////////////////////////
448template <typename LeafContainerT, typename BranchContainerT>
449void
451 const BranchNode* branch_arg,
452 OctreeKey& key_arg,
453 std::vector<char>* binary_tree_out_arg,
454 typename std::vector<LeafContainerT*>* leaf_container_vector_arg) const
455{
456 char node_bit_pattern;
457
458 // branch occupancy bit pattern
459 node_bit_pattern = getBranchBitPattern(*branch_arg);
460
461 // write bit pattern to output vector
462 if (binary_tree_out_arg)
463 binary_tree_out_arg->push_back(node_bit_pattern);
464
465 // iterate over all children
466 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
468 // if child exist
469 if (branch_arg->hasChild(child_idx)) {
470 // add current branch voxel to key
471 key_arg.pushBranch(child_idx);
472
473 OctreeNode* childNode = branch_arg->getChildPtr(child_idx);
474
475 switch (childNode->getNodeType()) {
476 case BRANCH_NODE: {
477 // recursively proceed with indexed child branch
478 serializeTreeRecursive(static_cast<const BranchNode*>(childNode),
479 key_arg,
480 binary_tree_out_arg,
481 leaf_container_vector_arg);
482 break;
483 }
484 case LEAF_NODE: {
485 auto* child_leaf = static_cast<LeafNode*>(childNode);
486
487 if (leaf_container_vector_arg)
488 leaf_container_vector_arg->push_back(child_leaf->getContainerPtr());
489
490 // we reached a leaf node -> execute serialization callback
491 serializeTreeCallback(**child_leaf, key_arg);
492 break;
493 }
494 default:
495 break;
496 }
497
498 // pop current branch voxel from key
499 key_arg.popBranch();
500 }
501 }
502}
503
504//////////////////////////////////////////////////////////////////////////////////////////////
505template <typename LeafContainerT, typename BranchContainerT>
506void
508 BranchNode* branch_arg,
509 uindex_t depth_mask_arg,
510 OctreeKey& key_arg,
511 typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
512 typename std::vector<char>::const_iterator& binary_tree_input_it_end_arg,
513 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
514 typename std::vector<LeafContainerT*>::const_iterator*
515 leaf_container_vector_it_end_arg)
516{
517
518 if (binary_tree_input_it_arg != binary_tree_input_it_end_arg) {
519 // read branch occupancy bit pattern from input vector
520 char node_bits = (*binary_tree_input_it_arg);
521 binary_tree_input_it_arg++;
522
523 // iterate over all children
524 for (unsigned char child_idx = 0; child_idx < 8; child_idx++) {
525 // if occupancy bit for child_idx is set..
526 if (node_bits & (1 << child_idx)) {
527 // add current branch voxel to key
528 key_arg.pushBranch(child_idx);
529
530 if (depth_mask_arg > 1) {
531 // we have not reached maximum tree depth
532 BranchNode* newBranch = createBranchChild(*branch_arg, child_idx);
533
534 branch_count_++;
535
536 // recursively proceed with new child branch
537 deserializeTreeRecursive(newBranch,
538 depth_mask_arg >> 1,
539 key_arg,
540 binary_tree_input_it_arg,
541 binary_tree_input_it_end_arg,
542 leaf_container_vector_it_arg,
543 leaf_container_vector_it_end_arg);
544 }
545 else {
546 // we reached leaf node level
547
548 LeafNode* child_leaf = createLeafChild(*branch_arg, child_idx);
549
550 if (leaf_container_vector_it_arg &&
551 (*leaf_container_vector_it_arg != *leaf_container_vector_it_end_arg)) {
552 LeafContainerT& container = **child_leaf;
553 LeafContainerT* src_container_ptr = **leaf_container_vector_it_arg;
554 container = *src_container_ptr;
555 ++*leaf_container_vector_it_arg;
556 }
557
558 leaf_count_++;
559
560 // execute deserialization callback
561 deserializeTreeCallback(**child_leaf, key_arg);
562 }
563
564 // pop current branch voxel from key
565 key_arg.popBranch();
566 }
567 }
568 }
569}
570
571} // namespace octree
572} // namespace pcl
573
574#define PCL_INSTANTIATE_OctreeBase(T) \
575 template class PCL_EXPORTS pcl::octree::OctreeBase<T>;
576
577#endif
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.
void setTreeDepth(uindex_t max_depth_arg)
Set the maximum depth of the octree.
void deserializeTreeRecursive(BranchNode *branch_arg, uindex_t depth_mask_arg, OctreeKey &key_arg, typename std::vector< char >::const_iterator &binary_tree_input_it_arg, typename std::vector< char >::const_iterator &binary_tree_input_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)
Recursive method for deserializing octree structure.
void serializeLeafs(std::vector< LeafContainerT * > &leaf_container_vector_arg)
Outputs a vector of all LeafContainerT elements that are stored within the octree leaf nodes.
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).
virtual ~OctreeBase()
Empty deconstructor.
bool deleteLeafRecursive(const OctreeKey &key_arg, uindex_t depth_mask_arg, BranchNode *branch_arg)
Recursively search and delete leaf node.
uindex_t createLeafRecursive(const OctreeKey &key_arg, uindex_t depth_mask_arg, BranchNode *branch_arg, LeafNode *&return_leaf_arg, BranchNode *&parent_of_leaf_arg)
Create a leaf node at octree key.
void deserializeTree(std::vector< char > &binary_tree_input_arg)
Deserialize a binary octree description vector and create a corresponding octree structure.
void deleteTree()
Delete the octree structure and its leaf nodes.
void serializeTree(std::vector< char > &binary_tree_out_arg) const
Serialize octree into a binary output vector describing its branch node structure.
bool existLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) const
idx_x_arg for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
void serializeTreeRecursive(const BranchNode *branch_arg, OctreeKey &key_arg, std::vector< char > *binary_tree_out_arg, typename std::vector< LeafContainerT * > *leaf_container_vector_arg) const
Recursively explore the octree and output binary octree description together with a vector of leaf no...
LeafContainerT * findLeaf(uindex_t idx_x_arg, uindex_t idx_y_arg, uindex_t idx_z_arg) const
Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg).
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).
void setMaxVoxelIndex(uindex_t max_voxel_index_arg)
Set the maximum amount of voxels per dimension.
OctreeBase()
Empty constructor.
Abstract octree branch class
bool hasChild(unsigned char child_idx_arg) const
Check if branch is pointing to a particular child node.
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