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