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
44 namespace pcl {
45 namespace octree {
46 //////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename LeafContainerT, typename BranchContainerT>
OctreeBase()48 OctreeBase<LeafContainerT, BranchContainerT>::OctreeBase()
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 //////////////////////////////////////////////////////////////////////////////////////////////
58 template <typename LeafContainerT, typename BranchContainerT>
~OctreeBase()59 OctreeBase<LeafContainerT, BranchContainerT>::~OctreeBase()
60 {
61 // deallocate tree structure
62 deleteTree();
63 delete (root_node_);
64 }
65
66 //////////////////////////////////////////////////////////////////////////////////////////////
67 template <typename LeafContainerT, typename BranchContainerT>
68 void
setMaxVoxelIndex(uindex_t max_voxel_index_arg)69 OctreeBase<LeafContainerT, BranchContainerT>::setMaxVoxelIndex(
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 //////////////////////////////////////////////////////////////////////////////////////////////
86 template <typename LeafContainerT, typename BranchContainerT>
87 void
setTreeDepth(uindex_t depth_arg)88 OctreeBase<LeafContainerT, BranchContainerT>::setTreeDepth(uindex_t depth_arg)
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 //////////////////////////////////////////////////////////////////////////////////////////////
103 template <typename LeafContainerT, typename BranchContainerT>
104 LeafContainerT*
105 OctreeBase<LeafContainerT, BranchContainerT>::findLeaf(uindex_t idx_x_arg,
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 //////////////////////////////////////////////////////////////////////////////////////////////
117 template <typename LeafContainerT, typename BranchContainerT>
118 LeafContainerT*
119 OctreeBase<LeafContainerT, BranchContainerT>::createLeaf(uindex_t idx_x_arg,
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 //////////////////////////////////////////////////////////////////////////////////////////////
131 template <typename LeafContainerT, typename BranchContainerT>
132 bool
existLeaf(uindex_t idx_x_arg,uindex_t idx_y_arg,uindex_t idx_z_arg) const133 OctreeBase<LeafContainerT, BranchContainerT>::existLeaf(uindex_t idx_x_arg,
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 //////////////////////////////////////////////////////////////////////////////////////////////
145 template <typename LeafContainerT, typename BranchContainerT>
146 void
removeLeaf(uindex_t idx_x_arg,uindex_t idx_y_arg,uindex_t idx_z_arg)147 OctreeBase<LeafContainerT, BranchContainerT>::removeLeaf(uindex_t idx_x_arg,
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 //////////////////////////////////////////////////////////////////////////////////////////////
159 template <typename LeafContainerT, typename BranchContainerT>
160 void
deleteTree()161 OctreeBase<LeafContainerT, BranchContainerT>::deleteTree()
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 //////////////////////////////////////////////////////////////////////////////////////////////
173 template <typename LeafContainerT, typename BranchContainerT>
174 void
serializeTree(std::vector<char> & binary_tree_out_arg)175 OctreeBase<LeafContainerT, BranchContainerT>::serializeTree(
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 //////////////////////////////////////////////////////////////////////////////////////////////
189 template <typename LeafContainerT, typename BranchContainerT>
190 void
serializeTree(std::vector<char> & binary_tree_out_arg,std::vector<LeafContainerT * > & leaf_container_vector_arg)191 OctreeBase<LeafContainerT, BranchContainerT>::serializeTree(
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 //////////////////////////////////////////////////////////////////////////////////////////////
210 template <typename LeafContainerT, typename BranchContainerT>
211 void
serializeLeafs(std::vector<LeafContainerT * > & leaf_container_vector_arg)212 OctreeBase<LeafContainerT, BranchContainerT>::serializeLeafs(
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 //////////////////////////////////////////////////////////////////////////////////////////////
226 template <typename LeafContainerT, typename BranchContainerT>
227 void
deserializeTree(std::vector<char> & binary_tree_out_arg)228 OctreeBase<LeafContainerT, BranchContainerT>::deserializeTree(
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 //////////////////////////////////////////////////////////////////////////////////////////////
250 template <typename LeafContainerT, typename BranchContainerT>
251 void
deserializeTree(std::vector<char> & binary_tree_in_arg,std::vector<LeafContainerT * > & leaf_container_vector_arg)252 OctreeBase<LeafContainerT, BranchContainerT>::deserializeTree(
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 //////////////////////////////////////////////////////////////////////////////////////////////
283 template <typename LeafContainerT, typename BranchContainerT>
284 uindex_t
createLeafRecursive(const OctreeKey & key_arg,uindex_t depth_mask_arg,BranchNode * branch_arg,LeafNode * & return_leaf_arg,BranchNode * & parent_of_leaf_arg)285 OctreeBase<LeafContainerT, BranchContainerT>::createLeafRecursive(
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);
297
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;
336 }
337 }
338
339 return (depth_mask_arg >> 1);
340 }
341
342 //////////////////////////////////////////////////////////////////////////////////////////////
343 template <typename LeafContainerT, typename BranchContainerT>
344 void
findLeafRecursive(const OctreeKey & key_arg,uindex_t depth_mask_arg,BranchNode * branch_arg,LeafContainerT * & result_arg) const345 OctreeBase<LeafContainerT, BranchContainerT>::findLeafRecursive(
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 //////////////////////////////////////////////////////////////////////////////////////////////
381 template <typename LeafContainerT, typename BranchContainerT>
382 bool
deleteLeafRecursive(const OctreeKey & key_arg,uindex_t depth_mask_arg,BranchNode * branch_arg)383 OctreeBase<LeafContainerT, BranchContainerT>::deleteLeafRecursive(
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 //////////////////////////////////////////////////////////////////////////////////////////////
433 template <typename LeafContainerT, typename BranchContainerT>
434 void
serializeTreeRecursive(const BranchNode * branch_arg,OctreeKey & key_arg,std::vector<char> * binary_tree_out_arg,typename std::vector<LeafContainerT * > * leaf_container_vector_arg) const435 OctreeBase<LeafContainerT, BranchContainerT>::serializeTreeRecursive(
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 //////////////////////////////////////////////////////////////////////////////////////////////
490 template <typename LeafContainerT, typename BranchContainerT>
491 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)492 OctreeBase<LeafContainerT, BranchContainerT>::deserializeTreeRecursive(
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
562