00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #ifndef OCTREE_BASE_HPP
00040 #define OCTREE_BASE_HPP
00041
00042 #include <vector>
00043
00044 #include <pcl/impl/instantiate.hpp>
00045 #include <pcl/point_types.h>
00046 #include <pcl/octree/octree.h>
00047
00048
00049 #define OCT_MAXTREEDEPTH ( sizeof(size_t) * 8 )
00050
00051 namespace pcl
00052 {
00053 namespace octree
00054 {
00056 template<typename DataT, typename LeafT, typename BranchT>
00057 OctreeBase<DataT, LeafT, BranchT>::OctreeBase () :
00058 leafCount_ (0),
00059 branchCount_ (1),
00060 objectCount_ (0),
00061 rootNode_ (new BranchNode ()),
00062 maxObjsPerLeaf_(0),
00063 depthMask_ (0),
00064 octreeDepth_ (0),
00065 maxKey_ (),
00066 branchNodePool_ (),
00067 leafNodePool_ ()
00068 {
00069 }
00070
00072 template<typename DataT, typename LeafT, typename BranchT>
00073 OctreeBase<DataT, LeafT, BranchT>::~OctreeBase ()
00074 {
00075
00076 deleteTree ();
00077 delete (rootNode_);
00078 poolCleanUp ();
00079 }
00080
00082 template<typename DataT, typename LeafT, typename BranchT> void
00083 OctreeBase<DataT, LeafT, BranchT>::setMaxVoxelIndex (unsigned int maxVoxelIndex_arg)
00084 {
00085 unsigned int treeDepth;
00086
00087 assert (maxVoxelIndex_arg>0);
00088
00089
00090 treeDepth = std::max ((std::min (static_cast<unsigned int> (OCT_MAXTREEDEPTH),
00091 static_cast<unsigned int> (std::ceil (Log2 (maxVoxelIndex_arg))))),
00092 static_cast<unsigned int> (0));
00093
00094
00095 depthMask_ = (1 << (treeDepth - 1));
00096 }
00097
00099 template<typename DataT, typename LeafT, typename BranchT>
00100 void
00101 OctreeBase<DataT, LeafT, BranchT>::setTreeDepth (unsigned int depth_arg)
00102 {
00103 assert(depth_arg>0);
00104
00105
00106 octreeDepth_ = depth_arg;
00107
00108
00109 depthMask_ = (1 << (depth_arg - 1));
00110
00111
00112 maxKey_.x = maxKey_.y = maxKey_.z = (1 << depth_arg) - 1;
00113 }
00114
00116 template<typename DataT, typename LeafT, typename BranchT> void
00117 OctreeBase<DataT, LeafT, BranchT>::addData (unsigned int idxX_arg, unsigned int idxY_arg,
00118 unsigned int idxZ_arg, const DataT& data_arg)
00119 {
00120
00121 OctreeKey key (idxX_arg, idxY_arg, idxZ_arg);
00122
00123
00124 addData (key, data_arg);
00125 }
00126
00128 template<typename DataT, typename LeafT, typename BranchT>bool
00129 OctreeBase<DataT, LeafT, BranchT>::getData (unsigned int idxX_arg, unsigned int idxY_arg,
00130 unsigned int idxZ_arg, DataT& data_arg) const
00131 {
00132
00133 OctreeKey key (idxX_arg, idxY_arg, idxZ_arg);
00134
00135
00136 LeafNode* leaf = findLeaf (key);
00137 if (leaf)
00138 {
00139
00140 leaf->getData (data_arg);
00141 }
00142
00143
00144 return (leaf != 0);
00145 }
00146
00148 template<typename DataT, typename LeafT, typename BranchT> bool
00149 OctreeBase<DataT, LeafT, BranchT>::existLeaf (unsigned int idxX_arg, unsigned int idxY_arg,
00150 unsigned int idxZ_arg) const
00151 {
00152
00153 OctreeKey key (idxX_arg, idxY_arg, idxZ_arg);
00154
00155
00156 return ( existLeaf (key));
00157 }
00158
00160 template<typename DataT, typename LeafT, typename BranchT> void
00161 OctreeBase<DataT, LeafT, BranchT>::removeLeaf (unsigned int idxX_arg, unsigned int idxY_arg,
00162 unsigned int idxZ_arg)
00163 {
00164
00165 OctreeKey key (idxX_arg, idxY_arg, idxZ_arg);
00166
00167
00168 deleteLeafRecursive (key, depthMask_, rootNode_);
00169 }
00170
00172 template<typename DataT, typename LeafT, typename BranchT> void
00173 OctreeBase<DataT, LeafT, BranchT>::deleteTree (bool freeMemory_arg )
00174 {
00175
00176 if (rootNode_)
00177 {
00178
00179 deleteBranch (*rootNode_);
00180 leafCount_ = 0;
00181 branchCount_ = 1;
00182 objectCount_ = 0;
00183
00184 }
00185
00186
00187 if (freeMemory_arg)
00188 poolCleanUp ();
00189
00190 }
00191
00193 template<typename DataT, typename LeafT, typename BranchT> void
00194 OctreeBase<DataT, LeafT, BranchT>::serializeTree (std::vector<char>& binaryTreeOut_arg)
00195 {
00196
00197
00198 assert (!maxObjsPerLeaf_);
00199
00200 OctreeKey newKey;
00201
00202
00203 binaryTreeOut_arg.clear ();
00204 binaryTreeOut_arg.reserve (this->branchCount_);
00205
00206 serializeTreeRecursive (rootNode_, newKey, &binaryTreeOut_arg, 0 );
00207 }
00208
00210 template<typename DataT, typename LeafT, typename BranchT> void
00211 OctreeBase<DataT, LeafT, BranchT>::serializeTree (std::vector<char>& binaryTreeOut_arg, std::vector<DataT>& dataVector_arg)
00212 {
00213
00214
00215 assert (!maxObjsPerLeaf_);
00216
00217 OctreeKey newKey;
00218
00219
00220 binaryTreeOut_arg.clear ();
00221 dataVector_arg.clear ();
00222
00223 dataVector_arg.reserve (this->objectCount_);
00224 binaryTreeOut_arg.reserve (this->branchCount_);
00225
00226 serializeTreeRecursive (rootNode_, newKey, &binaryTreeOut_arg, &dataVector_arg );
00227 }
00228
00230 template<typename DataT, typename LeafT, typename BranchT> void
00231 OctreeBase<DataT, LeafT, BranchT>::serializeLeafs (std::vector<DataT>& dataVector_arg)
00232 {
00233
00234
00235 assert (!maxObjsPerLeaf_);
00236
00237 OctreeKey newKey;
00238
00239
00240 dataVector_arg.clear ();
00241
00242 dataVector_arg.reserve(this->objectCount_);
00243
00244 serializeTreeRecursive (rootNode_, newKey, 0, &dataVector_arg );
00245 }
00246
00248 template<typename DataT, typename LeafT, typename BranchT> void
00249 OctreeBase<DataT, LeafT, BranchT>::deserializeTree (std::vector<char>& binaryTreeIn_arg)
00250 {
00251
00252
00253 assert (!maxObjsPerLeaf_);
00254
00255 OctreeKey newKey;
00256
00257
00258 deleteTree ();
00259
00260
00261 std::vector<char>::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin ();
00262 std::vector<char>::const_iterator binaryTreeVectorIteratorEnd = binaryTreeIn_arg.end ();
00263
00264 deserializeTreeRecursive (rootNode_, depthMask_, newKey,
00265 binaryTreeVectorIterator, binaryTreeVectorIteratorEnd, 0, 0);
00266
00267 }
00268
00270 template<typename DataT, typename LeafT, typename BranchT> void
00271 OctreeBase<DataT, LeafT, BranchT>::deserializeTree (std::vector<char>& binaryTreeIn_arg,
00272 std::vector<DataT>& dataVector_arg)
00273 {
00274
00275
00276 assert (!maxObjsPerLeaf_);
00277
00278 OctreeKey newKey;
00279
00280
00281 typename std::vector<DataT>::const_iterator dataVectorIterator = dataVector_arg.begin ();
00282
00283
00284 typename std::vector<DataT>::const_iterator dataVectorEndIterator = dataVector_arg.end ();
00285
00286
00287 deleteTree ();
00288
00289
00290 std::vector<char>::const_iterator binaryTreeVectorIterator = binaryTreeIn_arg.begin ();
00291 std::vector<char>::const_iterator binaryTreeVectorIteratorEnd = binaryTreeIn_arg.end ();
00292
00293 deserializeTreeRecursive (rootNode_, depthMask_, newKey,
00294 binaryTreeVectorIterator, binaryTreeVectorIteratorEnd, &dataVectorIterator, &dataVectorEndIterator);
00295
00296 }
00297
00299 template<typename DataT, typename LeafT, typename BranchT> void OctreeBase<
00300 DataT, LeafT, BranchT>::addDataToLeafRecursive (
00301 const OctreeKey& key_arg, unsigned int depthMask_arg,
00302 const DataT& data_arg, BranchNode* branch_arg)
00303 {
00304
00305 unsigned char childIdx;
00306
00307
00308 childIdx = key_arg.getChildIdxWithDepthMask(depthMask_arg);
00309
00310
00311 branch_arg->setData (data_arg);
00312
00313 OctreeNode* childNode = (*branch_arg)[childIdx];
00314
00315 if (!childNode)
00316 {
00317 if ((!maxObjsPerLeaf_) && (depthMask_arg > 1)) {
00318
00319 BranchNode* childBranch;
00320 createBranchChild (*branch_arg, childIdx, childBranch);
00321
00322 branchCount_++;
00323
00324
00325 addDataToLeafRecursive (key_arg, depthMask_arg / 2, data_arg, childBranch);
00326
00327 } else {
00328 LeafNode* childLeaf;
00329
00330
00331 createLeafChild (*branch_arg, childIdx, childLeaf);
00332 leafCount_++;
00333
00334
00335 childLeaf->setData (data_arg);
00336 objectCount_++;
00337 }
00338 } else {
00339
00340
00341 switch (childNode->getNodeType()) {
00342 case BRANCH_NODE:
00343
00344 addDataToLeafRecursive (key_arg, depthMask_arg / 2, data_arg, static_cast<BranchNode*> (childNode));
00345 break;
00346
00347 case LEAF_NODE:
00348 LeafNode* childLeaf = static_cast<LeafNode*> (childNode);
00349
00350 if ( (!maxObjsPerLeaf_) || (!depthMask_arg) )
00351 {
00352
00353 childLeaf->setData (data_arg);
00354 objectCount_++;
00355 } else {
00356 size_t leafObjCount = childLeaf->getSize();
00357
00358 if (leafObjCount<maxObjsPerLeaf_) {
00359
00360 childLeaf->setData (data_arg);
00361 objectCount_++;
00362 } else {
00363
00364
00365
00366 std::vector<DataT> leafData;
00367 leafData.reserve(leafObjCount);
00368
00369 childLeaf->getData (leafData);
00370
00371
00372 deleteBranchChild(*branch_arg,childIdx);
00373 leafCount_ --;
00374
00375
00376 BranchNode* childBranch;
00377 createBranchChild (*branch_arg, childIdx, childBranch);
00378 branchCount_ ++;
00379
00380 typename std::vector<DataT>::const_iterator lData = leafData.begin();
00381 typename std::vector<DataT>::const_iterator lDataEnd = leafData.end();
00382
00383
00384 OctreeKey dataKey;
00385 while (lData!=lDataEnd) {
00386
00387 const DataT& data = *lData++;
00388
00389
00390 if (this->genOctreeKeyForDataT(data, dataKey)) {
00391 addDataToLeafRecursive (dataKey, depthMask_arg / 2, data, childBranch);
00392 }
00393 }
00394
00395
00396 addDataToLeafRecursive (key_arg, depthMask_arg / 2, data_arg, childBranch);
00397
00398
00399 objectCount_ -= leafObjCount;
00400 }
00401 }
00402 break;
00403 }
00404 }
00405 }
00406
00408 template<typename DataT, typename LeafT, typename BranchT> void
00409 OctreeBase<DataT, LeafT, BranchT>::findLeafRecursive (
00410 const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg, LeafNode*& result_arg) const
00411 {
00412
00413 unsigned char childIdx;
00414
00415
00416 childIdx = key_arg.getChildIdxWithDepthMask(depthMask_arg);
00417
00418 OctreeNode* childNode = (*branch_arg)[childIdx];
00419
00420 if (childNode) {
00421 switch (childNode->getNodeType()) {
00422 case BRANCH_NODE:
00423
00424 BranchNode* childBranch;
00425 childBranch = static_cast<BranchNode*> (childNode);
00426
00427 findLeafRecursive (key_arg, depthMask_arg / 2, childBranch, result_arg);
00428 break;
00429
00430 case LEAF_NODE:
00431
00432 LeafNode* childLeaf;
00433 childLeaf = static_cast<LeafNode*> (childNode);
00434 result_arg = childLeaf;
00435 break;
00436 }
00437 }
00438 }
00439
00441 template<typename DataT, typename LeafT, typename BranchT> bool
00442 OctreeBase<DataT, LeafT, BranchT>::deleteLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg,
00443 BranchNode* branch_arg)
00444 {
00445
00446 unsigned char childIdx;
00447
00448 bool bNoChilds;
00449
00450
00451 childIdx = key_arg.getChildIdxWithDepthMask(depthMask_arg);
00452
00453 OctreeNode* childNode = (*branch_arg)[childIdx];
00454
00455 if (childNode) {
00456 switch (childNode->getNodeType()) {
00457
00458 case BRANCH_NODE:
00459 BranchNode* childBranch;
00460 childBranch = static_cast<BranchNode*> (childNode);
00461
00462
00463 bNoChilds = deleteLeafRecursive (key_arg, depthMask_arg / 2, childBranch);
00464
00465 if (!bNoChilds)
00466 {
00467
00468 deleteBranchChild(*branch_arg, childIdx);
00469 branchCount_--;
00470 }
00471 break;
00472
00473 case LEAF_NODE:
00474
00475
00476
00477 deleteBranchChild (*branch_arg, childIdx);
00478 leafCount_--;
00479 break;
00480 }
00481 }
00482
00483
00484 bNoChilds = false;
00485 for (childIdx = 0; (!bNoChilds) && (childIdx < 8); childIdx++)
00486 {
00487 bNoChilds = branch_arg->hasChild(childIdx);
00488 }
00489
00490 return (bNoChilds);
00491 }
00492
00494 template<typename DataT, typename LeafT, typename BranchT> void OctreeBase<
00495 DataT, LeafT, BranchT>::serializeTreeRecursive (const BranchNode* branch_arg, OctreeKey& key_arg,
00496 std::vector<char>* binaryTreeOut_arg,
00497 typename std::vector<DataT>* dataVector_arg) const
00498 {
00499
00500
00501 unsigned char childIdx;
00502 char nodeBitPattern;
00503
00504
00505 nodeBitPattern = getBranchBitPattern (*branch_arg);
00506
00507
00508 if (binaryTreeOut_arg)
00509 binaryTreeOut_arg->push_back (nodeBitPattern);
00510
00511
00512 for (childIdx = 0; childIdx < 8; childIdx++)
00513 {
00514
00515
00516 if (branch_arg->hasChild(childIdx))
00517 {
00518
00519 key_arg.pushBranch(childIdx);
00520
00521 const OctreeNode *childNode = branch_arg->getChildPtr(childIdx);
00522
00523 switch (childNode->getNodeType ())
00524 {
00525 case BRANCH_NODE:
00526 {
00527
00528 serializeTreeRecursive (
00529 static_cast<const BranchNode*> (childNode), key_arg,
00530 binaryTreeOut_arg, dataVector_arg);
00531 break;
00532 }
00533 case LEAF_NODE:
00534 {
00535 const LeafNode* childLeaf = static_cast<const LeafNode*> (childNode);
00536
00537 if (dataVector_arg)
00538 childLeaf->getData (*dataVector_arg);
00539
00540
00541 serializeTreeCallback (*childLeaf, key_arg);
00542 break;
00543 }
00544 default:
00545 break;
00546 }
00547
00548
00549 key_arg.popBranch();
00550 }
00551 }
00552 }
00553
00554
00555
00557 template<typename DataT, typename LeafT, typename BranchT> void
00558 OctreeBase<DataT, LeafT, BranchT>::deserializeTreeRecursive (BranchNode* branch_arg,
00559 unsigned int depthMask_arg, OctreeKey& key_arg,
00560 typename std::vector<char>::const_iterator& binaryTreeIT_arg,
00561 typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
00562 typename std::vector<DataT>::const_iterator* dataVectorIterator_arg,
00563 typename std::vector<DataT>::const_iterator* dataVectorEndIterator_arg)
00564 {
00565
00566 unsigned char childIdx;
00567 char nodeBits;
00568
00569 if (binaryTreeIT_arg != binaryTreeIT_End_arg ) {
00570
00571 nodeBits = (*binaryTreeIT_arg);
00572 binaryTreeIT_arg++;
00573
00574
00575 for (childIdx = 0; childIdx < 8; childIdx++)
00576 {
00577
00578 if (nodeBits & (1 << childIdx))
00579 {
00580
00581 key_arg.pushBranch(childIdx);
00582
00583 if (depthMask_arg > 1)
00584 {
00585
00586 BranchNode * newBranch;
00587
00588
00589 createBranchChild (*branch_arg, childIdx, newBranch);
00590
00591 branchCount_++;
00592
00593
00594 deserializeTreeRecursive (newBranch, depthMask_arg / 2, key_arg, binaryTreeIT_arg,binaryTreeIT_End_arg, dataVectorIterator_arg,
00595 dataVectorEndIterator_arg);
00596 }
00597 else
00598 {
00599
00600
00601 LeafNode* childLeaf;
00602
00603
00604 createLeafChild (*branch_arg, childIdx, childLeaf);
00605
00606 OctreeKey dataKey;
00607 bool bKeyBasedEncoding = false;
00608
00609 if (dataVectorIterator_arg
00610 && (*dataVectorIterator_arg != *dataVectorEndIterator_arg))
00611 {
00612
00613
00614 while ( ( (*dataVectorIterator_arg)
00615 != (*dataVectorEndIterator_arg))
00616 && (this->genOctreeKeyForDataT (**dataVectorIterator_arg,
00617 dataKey) && (dataKey == key_arg)))
00618 {
00619 childLeaf->setData (**dataVectorIterator_arg);
00620 (*dataVectorIterator_arg)++;
00621 bKeyBasedEncoding = true;
00622 objectCount_++;
00623 }
00624
00625
00626 if (!bKeyBasedEncoding)
00627 {
00628 childLeaf->setData (**dataVectorIterator_arg);
00629 (*dataVectorIterator_arg)++;
00630 objectCount_++;
00631 }
00632 }
00633
00634 leafCount_++;
00635
00636
00637 deserializeTreeCallback (*childLeaf, key_arg);
00638 }
00639
00640
00641 key_arg.popBranch();
00642 }
00643 }
00644 }
00645 }
00646
00647 }
00648 }
00649
00650 #define PCL_INSTANTIATE_OctreeBase(T) template class PCL_EXPORTS pcl::octree::OctreeBase<T>;
00651
00652 #endif
00653