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