Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id$ 00037 */ 00038 00039 #ifndef PCL_OCTREE_TREE_2BUF_BASE_H 00040 #define PCL_OCTREE_TREE_2BUF_BASE_H 00041 00042 #include <vector> 00043 00044 #include "octree_nodes.h" 00045 #include "octree_container.h" 00046 #include "octree_key.h" 00047 #include "octree_iterator.h" 00048 00049 #include <stdio.h> 00050 #include <string.h> 00051 00052 namespace pcl 00053 { 00054 namespace octree 00055 { 00056 00057 template<typename ContainerT> 00058 class BufferedBranchNode : public OctreeNode 00059 { 00060 00061 public: 00062 /** \brief Empty constructor. */ 00063 BufferedBranchNode () : OctreeNode() 00064 { 00065 reset (); 00066 } 00067 00068 /** \brief Copy constructor. */ 00069 BufferedBranchNode (const BufferedBranchNode& source) : OctreeNode() 00070 { 00071 *this = source; 00072 } 00073 00074 /** \brief Copy operator. */ 00075 inline BufferedBranchNode& 00076 operator = (const BufferedBranchNode &source_arg) 00077 { 00078 00079 unsigned char i, b; 00080 00081 memset (child_node_array_, 0, sizeof(child_node_array_)); 00082 00083 for (b = 0; b < 2; ++b) 00084 for (i = 0; i < 8; ++i) 00085 if (source_arg.child_node_array_[b][i]) 00086 child_node_array_[b][i] = source_arg.child_node_array_[b][i]->deepCopy (); 00087 00088 return (*this); 00089 00090 } 00091 00092 /** \brief Empty constructor. */ 00093 virtual ~BufferedBranchNode () 00094 { 00095 } 00096 00097 /** \brief Method to perform a deep copy of the octree */ 00098 virtual BufferedBranchNode* 00099 deepCopy () const 00100 { 00101 return new BufferedBranchNode (*this); 00102 } 00103 00104 /** \brief Get child pointer in current branch node 00105 * \param buffer_arg: buffer selector 00106 * \param index_arg: index of child in node 00107 * \return pointer to child node 00108 * */ 00109 inline OctreeNode* 00110 getChildPtr (unsigned char buffer_arg, unsigned char index_arg) const 00111 { 00112 assert( (buffer_arg<2) && (index_arg<8)); 00113 return child_node_array_[buffer_arg][index_arg]; 00114 } 00115 00116 /** \brief Set child pointer in current branch node 00117 * \param buffer_arg: buffer selector 00118 * \param index_arg: index of child in node 00119 * \param newNode_arg: pointer to new child node 00120 * */ 00121 inline void setChildPtr (unsigned char buffer_arg, unsigned char index_arg, 00122 OctreeNode* newNode_arg) 00123 { 00124 assert( (buffer_arg<2) && (index_arg<8)); 00125 child_node_array_[buffer_arg][index_arg] = newNode_arg; 00126 } 00127 00128 /** \brief Check if branch is pointing to a particular child node 00129 * \param buffer_arg: buffer selector 00130 * \param index_arg: index of child in node 00131 * \return "true" if pointer to child node exists; "false" otherwise 00132 * */ 00133 inline bool hasChild (unsigned char buffer_arg, unsigned char index_arg) const 00134 { 00135 assert( (buffer_arg<2) && (index_arg<8)); 00136 return (child_node_array_[buffer_arg][index_arg] != 0); 00137 } 00138 00139 /** \brief Get the type of octree node. Returns LEAVE_NODE type */ 00140 virtual node_type_t getNodeType () const 00141 { 00142 return BRANCH_NODE; 00143 } 00144 00145 /** \brief Reset branch node container for every branch buffer. */ 00146 inline void reset () 00147 { 00148 memset (&child_node_array_[0][0], 0, sizeof(OctreeNode*) * 8 * 2); 00149 } 00150 00151 /** \brief Get const pointer to container */ 00152 const ContainerT* 00153 operator->() const 00154 { 00155 return &container_; 00156 } 00157 00158 /** \brief Get pointer to container */ 00159 ContainerT* 00160 operator-> () 00161 { 00162 return &container_; 00163 } 00164 00165 /** \brief Get const reference to container */ 00166 const ContainerT& 00167 operator* () const 00168 { 00169 return container_; 00170 } 00171 00172 /** \brief Get reference to container */ 00173 ContainerT& 00174 operator* () 00175 { 00176 return container_; 00177 } 00178 00179 /** \brief Get const reference to container */ 00180 const ContainerT& 00181 getContainer () const 00182 { 00183 return container_; 00184 } 00185 00186 /** \brief Get reference to container */ 00187 ContainerT& 00188 getContainer () 00189 { 00190 return container_; 00191 } 00192 00193 /** \brief Get const pointer to container */ 00194 const ContainerT* 00195 getContainerPtr() const 00196 { 00197 return &container_; 00198 } 00199 00200 /** \brief Get pointer to container */ 00201 ContainerT* 00202 getContainerPtr () 00203 { 00204 return &container_; 00205 } 00206 00207 protected: 00208 ContainerT container_; 00209 00210 OctreeNode* child_node_array_[2][8]; 00211 }; 00212 00213 /** \brief @b Octree double buffer class 00214 * 00215 * \note This octree implementation keeps two separate octree structures 00216 * in memory. 00217 * 00218 * \note This allows for differentially compare the octree structures (change detection, differential encoding). 00219 * \note The tree depth defines the maximum amount of octree voxels / leaf nodes (should be initially defined). 00220 * \note All leaf nodes are addressed by integer indices. 00221 * \note Note: The tree depth equates to the bit length of the voxel indices. 00222 * \ingroup octree 00223 * \author Julius Kammerl (julius@kammerl.de) 00224 */ 00225 template<typename LeafContainerT = int, 00226 typename BranchContainerT = OctreeContainerEmpty > 00227 class Octree2BufBase 00228 { 00229 00230 public: 00231 00232 typedef Octree2BufBase<LeafContainerT, BranchContainerT> OctreeT; 00233 00234 // iterators are friends 00235 friend class OctreeIteratorBase<OctreeT> ; 00236 friend class OctreeDepthFirstIterator<OctreeT> ; 00237 friend class OctreeBreadthFirstIterator<OctreeT> ; 00238 friend class OctreeLeafNodeIterator<OctreeT> ; 00239 00240 typedef BufferedBranchNode<BranchContainerT> BranchNode; 00241 typedef OctreeLeafNode<LeafContainerT> LeafNode; 00242 00243 typedef BranchContainerT BranchContainer; 00244 typedef LeafContainerT LeafContainer; 00245 00246 // Octree default iterators 00247 typedef OctreeDepthFirstIterator<OctreeT> Iterator; 00248 typedef const OctreeDepthFirstIterator<OctreeT> ConstIterator; 00249 Iterator begin(unsigned int max_depth_arg = 0) {return Iterator(this, max_depth_arg);}; 00250 const Iterator end() {return Iterator();}; 00251 00252 // Octree leaf node iterators 00253 typedef OctreeLeafNodeIterator<OctreeT> LeafNodeIterator; 00254 typedef const OctreeLeafNodeIterator<OctreeT> ConstLeafNodeIterator; 00255 LeafNodeIterator leaf_begin(unsigned int max_depth_arg = 0) {return LeafNodeIterator(this, max_depth_arg);}; 00256 const LeafNodeIterator leaf_end() {return LeafNodeIterator();}; 00257 00258 // Octree depth-first iterators 00259 typedef OctreeDepthFirstIterator<OctreeT> DepthFirstIterator; 00260 typedef const OctreeDepthFirstIterator<OctreeT> ConstDepthFirstIterator; 00261 DepthFirstIterator depth_begin(unsigned int maxDepth_arg = 0) {return DepthFirstIterator(this, maxDepth_arg);}; 00262 const DepthFirstIterator depth_end() {return DepthFirstIterator();}; 00263 00264 // Octree breadth-first iterators 00265 typedef OctreeBreadthFirstIterator<OctreeT> BreadthFirstIterator; 00266 typedef const OctreeBreadthFirstIterator<OctreeT> ConstBreadthFirstIterator; 00267 BreadthFirstIterator breadth_begin(unsigned int max_depth_arg = 0) {return BreadthFirstIterator(this, max_depth_arg);}; 00268 const BreadthFirstIterator breadth_end() {return BreadthFirstIterator();}; 00269 00270 /** \brief Empty constructor. */ 00271 Octree2BufBase (); 00272 00273 /** \brief Empty deconstructor. */ 00274 virtual 00275 ~Octree2BufBase (); 00276 00277 /** \brief Copy constructor. */ 00278 Octree2BufBase (const Octree2BufBase& source) : 00279 leaf_count_ (source.leaf_count_), 00280 branch_count_ (source.branch_count_), 00281 root_node_ (new (BranchNode) (*(source.root_node_))), 00282 depth_mask_ (source.depth_mask_), 00283 max_key_ (source.max_key_), 00284 buffer_selector_ (source.buffer_selector_), 00285 tree_dirty_flag_ (source.tree_dirty_flag_), 00286 octree_depth_ (source.octree_depth_), 00287 dynamic_depth_enabled_(source.dynamic_depth_enabled_) 00288 { 00289 } 00290 00291 /** \brief Copy constructor. */ 00292 inline Octree2BufBase& 00293 operator = (const Octree2BufBase& source) 00294 { 00295 leaf_count_ = source.leaf_count_; 00296 branch_count_ = source.branch_count_; 00297 root_node_ = new (BranchNode) (* (source.root_node_)); 00298 depth_mask_ = source.depth_mask_; 00299 max_key_ = source.max_key_; 00300 buffer_selector_ = source.buffer_selector_; 00301 tree_dirty_flag_ = source.tree_dirty_flag_; 00302 octree_depth_ = source.octree_depth_; 00303 dynamic_depth_enabled_ = source.dynamic_depth_enabled_; 00304 return (*this); 00305 } 00306 00307 /** \brief Set the maximum amount of voxels per dimension. 00308 * \param max_voxel_index_arg: maximum amount of voxels per dimension 00309 * */ 00310 void 00311 setMaxVoxelIndex (unsigned int max_voxel_index_arg); 00312 00313 /** \brief Set the maximum depth of the octree. 00314 * \param depth_arg: maximum depth of octree 00315 * */ 00316 void 00317 setTreeDepth (unsigned int depth_arg); 00318 00319 /** \brief Get the maximum depth of the octree. 00320 * \return depth_arg: maximum depth of octree 00321 * */ 00322 inline unsigned int getTreeDepth () const 00323 { 00324 return this->octree_depth_; 00325 } 00326 00327 /** \brief Create new leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). 00328 * \note If leaf node already exist, this method returns the existing node 00329 * \param idx_x_arg: index of leaf node in the X axis. 00330 * \param idx_y_arg: index of leaf node in the Y axis. 00331 * \param idx_z_arg: index of leaf node in the Z axis. 00332 * \return pointer to new leaf node container. 00333 * */ 00334 LeafContainerT* 00335 createLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); 00336 00337 /** \brief Find leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). 00338 * \note If leaf node already exist, this method returns the existing node 00339 * \param idx_x_arg: index of leaf node in the X axis. 00340 * \param idx_y_arg: index of leaf node in the Y axis. 00341 * \param idx_z_arg: index of leaf node in the Z axis. 00342 * \return pointer to leaf node container if found, null pointer otherwise. 00343 * */ 00344 LeafContainerT* 00345 findLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); 00346 00347 /** \brief Check for the existence of leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). 00348 * \param idx_x_arg: index of leaf node in the X axis. 00349 * \param idx_y_arg: index of leaf node in the Y axis. 00350 * \param idx_z_arg: index of leaf node in the Z axis. 00351 * \return "true" if leaf node search is successful, otherwise it returns "false". 00352 * */ 00353 bool 00354 existLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const; 00355 00356 /** \brief Remove leaf node at (idx_x_arg, idx_y_arg, idx_z_arg). 00357 * \param idx_x_arg: index of leaf node in the X axis. 00358 * \param idx_y_arg: index of leaf node in the Y axis. 00359 * \param idx_z_arg: index of leaf node in the Z axis. 00360 * */ 00361 void 00362 removeLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg); 00363 00364 /** \brief Return the amount of existing leafs in the octree. 00365 * \return amount of registered leaf nodes. 00366 * */ 00367 inline std::size_t getLeafCount () const 00368 { 00369 return (leaf_count_); 00370 } 00371 00372 /** \brief Return the amount of existing branches in the octree. 00373 * \return amount of branch nodes. 00374 * */ 00375 inline std::size_t getBranchCount () const 00376 { 00377 return (branch_count_); 00378 } 00379 00380 /** \brief Delete the octree structure and its leaf nodes. 00381 * */ 00382 void 00383 deleteTree (); 00384 00385 /** \brief Delete octree structure of previous buffer. */ 00386 inline void deletePreviousBuffer () 00387 { 00388 treeCleanUpRecursive (root_node_); 00389 } 00390 00391 /** \brief Delete the octree structure in the current buffer. */ 00392 inline void deleteCurrentBuffer () 00393 { 00394 buffer_selector_ = !buffer_selector_; 00395 treeCleanUpRecursive (root_node_); 00396 leaf_count_ = 0; 00397 } 00398 00399 /** \brief Switch buffers and reset current octree structure. */ 00400 void 00401 switchBuffers (); 00402 00403 /** \brief Serialize octree into a binary output vector describing its branch node structure. 00404 * \param binary_tree_out_arg: reference to output vector for writing binary tree structure. 00405 * \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree 00406 * */ 00407 void 00408 serializeTree (std::vector<char>& binary_tree_out_arg, 00409 bool do_XOR_encoding_arg = false); 00410 00411 /** \brief Serialize octree into a binary output vector describing its branch node structure and and push all DataT elements stored in the octree to a vector. 00412 * \param binary_tree_out_arg: reference to output vector for writing binary tree structure. 00413 * \param leaf_container_vector_arg: pointer to all LeafContainerT objects in the octree 00414 * \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree 00415 * */ 00416 void 00417 serializeTree (std::vector<char>& binary_tree_out_arg, 00418 std::vector<LeafContainerT*>& leaf_container_vector_arg, 00419 bool do_XOR_encoding_arg = false); 00420 00421 /** \brief Outputs a vector of all DataT elements that are stored within the octree leaf nodes. 00422 * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree 00423 * */ 00424 void 00425 serializeLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg); 00426 00427 /** \brief Outputs a vector of all DataT elements from leaf nodes, that do not exist in the previous octree buffer. 00428 * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree 00429 * */ 00430 void 00431 serializeNewLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg); 00432 00433 /** \brief Deserialize a binary octree description vector and create a corresponding octree structure. Leaf nodes are initialized with getDataTByKey(..). 00434 * \param binary_tree_in_arg: reference to input vector for reading binary tree structure. 00435 * \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree 00436 * */ 00437 void 00438 deserializeTree (std::vector<char>& binary_tree_in_arg, 00439 bool do_XOR_decoding_arg = false); 00440 00441 /** \brief Deserialize a binary octree description and create a corresponding octree structure. Leaf nodes are initialized with DataT elements from the dataVector. 00442 * \param binary_tree_in_arg: reference to inpvectoream for reading binary tree structure. 00443 * \param leaf_container_vector_arg: vector of pointers to all LeafContainerT objects in the octree 00444 * \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree 00445 * */ 00446 void 00447 deserializeTree (std::vector<char>& binary_tree_in_arg, 00448 std::vector<LeafContainerT*>& leaf_container_vector_arg, 00449 bool do_XOR_decoding_arg = false); 00450 00451 protected: 00452 00453 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00454 // Protected octree methods based on octree keys 00455 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00456 00457 /** \brief Retrieve root node */ 00458 OctreeNode* 00459 getRootNode () const 00460 { 00461 return (this->root_node_); 00462 } 00463 00464 /** \brief Find leaf node 00465 * \param key_arg: octree key addressing a leaf node. 00466 * \return pointer to leaf container. If leaf node is not found, this pointer returns 0. 00467 * */ 00468 inline LeafContainerT* 00469 findLeaf (const OctreeKey& key_arg) const 00470 { 00471 LeafContainerT* result = 0; 00472 findLeafRecursive (key_arg, depth_mask_, root_node_, result); 00473 return result; 00474 } 00475 00476 /** \brief Create a leaf node. 00477 * \note If the leaf node at the given octree node does not exist, it will be created and added to the tree. 00478 * \param key_arg: octree key addressing a leaf node. 00479 * \return pointer to an existing or created leaf container. 00480 * */ 00481 inline LeafContainerT* 00482 createLeaf (const OctreeKey& key_arg) 00483 { 00484 LeafNode* leaf_node; 00485 BranchNode* leaf_node_parent; 00486 00487 createLeafRecursive (key_arg, depth_mask_ ,root_node_, leaf_node, leaf_node_parent, false); 00488 00489 LeafContainerT* ret = leaf_node->getContainerPtr(); 00490 00491 return ret; 00492 } 00493 00494 /** \brief Check for leaf not existance in the octree 00495 * \param key_arg: octree key addressing a leaf node. 00496 * \return "true" if leaf node is found; "false" otherwise 00497 * */ 00498 inline bool existLeaf (const OctreeKey& key_arg) const 00499 { 00500 return (findLeaf(key_arg) != 0); 00501 } 00502 00503 /** \brief Remove leaf node from octree 00504 * \param key_arg: octree key addressing a leaf node. 00505 * */ 00506 inline void removeLeaf (const OctreeKey& key_arg) 00507 { 00508 if (key_arg <= max_key_) 00509 { 00510 deleteLeafRecursive (key_arg, depth_mask_, root_node_); 00511 00512 // we changed the octree structure -> dirty 00513 tree_dirty_flag_ = true; 00514 } 00515 } 00516 00517 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00518 // Branch node accessor inline functions 00519 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00520 00521 /** \brief Check if branch is pointing to a particular child node 00522 * \param branch_arg: reference to octree branch class 00523 * \param child_idx_arg: index to child node 00524 * \return "true" if pointer to child node exists; "false" otherwise 00525 * */ 00526 inline bool 00527 branchHasChild (const BranchNode& branch_arg, unsigned char child_idx_arg) const 00528 { 00529 // test occupancyByte for child existence 00530 return (branch_arg.getChildPtr(buffer_selector_, child_idx_arg) != 0); 00531 } 00532 00533 /** \brief Retrieve a child node pointer for child node at child_idx. 00534 * \param branch_arg: reference to octree branch class 00535 * \param child_idx_arg: index to child node 00536 * \return pointer to octree child node class 00537 */ 00538 inline OctreeNode* 00539 getBranchChildPtr (const BranchNode& branch_arg, 00540 unsigned char child_idx_arg) const 00541 { 00542 return branch_arg.getChildPtr(buffer_selector_, child_idx_arg); 00543 } 00544 00545 /** \brief Assign new child node to branch 00546 * \param branch_arg: reference to octree branch class 00547 * \param child_idx_arg: index to child node 00548 * \param new_child_arg: pointer to new child node 00549 * */ 00550 inline void 00551 setBranchChildPtr (BranchNode& branch_arg, unsigned char child_idx_arg, OctreeNode* new_child_arg) 00552 { 00553 branch_arg.setChildPtr (buffer_selector_, child_idx_arg, new_child_arg); 00554 } 00555 00556 /** \brief Generate bit pattern reflecting the existence of child node pointers for current buffer 00557 * \param branch_arg: reference to octree branch class 00558 * \return a single byte with 8 bits of child node information 00559 * */ 00560 inline char getBranchBitPattern (const BranchNode& branch_arg) const 00561 { 00562 unsigned char i; 00563 char node_bits; 00564 00565 // create bit pattern 00566 node_bits = 0; 00567 for (i = 0; i < 8; i++) 00568 { 00569 const OctreeNode* child = branch_arg.getChildPtr(buffer_selector_, i); 00570 node_bits |= static_cast<char> ( (!!child) << i); 00571 } 00572 00573 return (node_bits); 00574 } 00575 00576 /** \brief Generate bit pattern reflecting the existence of child node pointers in specific buffer 00577 * \param branch_arg: reference to octree branch class 00578 * \param bufferSelector_arg: buffer selector 00579 * \return a single byte with 8 bits of child node information 00580 * */ 00581 inline char getBranchBitPattern (const BranchNode& branch_arg, 00582 unsigned char bufferSelector_arg) const 00583 { 00584 unsigned char i; 00585 char node_bits; 00586 00587 // create bit pattern 00588 node_bits = 0; 00589 for (i = 0; i < 8; i++) 00590 { 00591 const OctreeNode* child = branch_arg.getChildPtr(bufferSelector_arg, i); 00592 node_bits |= static_cast<char> ( (!!child) << i); 00593 } 00594 00595 return (node_bits); 00596 } 00597 00598 /** \brief Generate XOR bit pattern reflecting differences between the two octree buffers 00599 * \param branch_arg: reference to octree branch class 00600 * \return a single byte with 8 bits of child node XOR difference information 00601 * */ 00602 inline char getBranchXORBitPattern ( 00603 const BranchNode& branch_arg) const 00604 { 00605 unsigned char i; 00606 char node_bits[2]; 00607 00608 // create bit pattern for both buffers 00609 node_bits[0] = node_bits[1] = 0; 00610 00611 for (i = 0; i < 8; i++) 00612 { 00613 const OctreeNode* childA = branch_arg.getChildPtr(0, i); 00614 const OctreeNode* childB = branch_arg.getChildPtr(1, i); 00615 00616 node_bits[0] |= static_cast<char> ( (!!childA) << i); 00617 node_bits[1] |= static_cast<char> ( (!!childB) << i); 00618 } 00619 00620 return node_bits[0] ^ node_bits[1]; 00621 } 00622 00623 /** \brief Test if branch changed between previous and current buffer 00624 * \param branch_arg: reference to octree branch class 00625 * \return "true", if child node information differs between current and previous octree buffer 00626 * */ 00627 inline bool hasBranchChanges (const BranchNode& branch_arg) const 00628 { 00629 return (getBranchXORBitPattern (branch_arg) > 0); 00630 } 00631 00632 /** \brief Delete child node and all its subchilds from octree in specific buffer 00633 * \param branch_arg: reference to octree branch class 00634 * \param buffer_selector_arg: buffer selector 00635 * \param child_idx_arg: index to child node 00636 * */ 00637 inline void deleteBranchChild (BranchNode& branch_arg, 00638 unsigned char buffer_selector_arg, 00639 unsigned char child_idx_arg) 00640 { 00641 if (branch_arg.hasChild(buffer_selector_arg, child_idx_arg)) 00642 { 00643 OctreeNode* branchChild = branch_arg.getChildPtr(buffer_selector_arg, child_idx_arg); 00644 00645 switch (branchChild->getNodeType ()) 00646 { 00647 case BRANCH_NODE: 00648 { 00649 // free child branch recursively 00650 deleteBranch (*static_cast<BranchNode*> (branchChild)); 00651 00652 // delete unused branch 00653 delete (branchChild); 00654 break; 00655 } 00656 00657 case LEAF_NODE: 00658 { 00659 // push unused leaf to branch pool 00660 delete (branchChild); 00661 break; 00662 } 00663 default: 00664 break; 00665 } 00666 00667 // set branch child pointer to 0 00668 branch_arg.setChildPtr(buffer_selector_arg, child_idx_arg, 0); 00669 } 00670 } 00671 00672 /** \brief Delete child node and all its subchilds from octree in current buffer 00673 * \param branch_arg: reference to octree branch class 00674 * \param child_idx_arg: index to child node 00675 * */ 00676 inline void deleteBranchChild (BranchNode& branch_arg, unsigned char child_idx_arg) 00677 { 00678 deleteBranchChild(branch_arg, buffer_selector_, child_idx_arg); 00679 } 00680 00681 /** \brief Delete branch and all its subchilds from octree (both buffers) 00682 * \param branch_arg: reference to octree branch class 00683 * */ 00684 inline void deleteBranch (BranchNode& branch_arg) 00685 { 00686 char i; 00687 00688 // delete all branch node children 00689 for (i = 0; i < 8; i++) 00690 { 00691 00692 if (branch_arg.getChildPtr(0, i) == branch_arg.getChildPtr(1, i)) 00693 { 00694 // reference was copied - there is only one child instance to be deleted 00695 deleteBranchChild (branch_arg, 0, i); 00696 00697 // remove pointers from both buffers 00698 branch_arg.setChildPtr(0, i, 0); 00699 branch_arg.setChildPtr(1, i, 0); 00700 } 00701 else 00702 { 00703 deleteBranchChild (branch_arg, 0, i); 00704 deleteBranchChild (branch_arg, 1, i); 00705 } 00706 } 00707 } 00708 00709 /** \brief Fetch and add a new branch child to a branch class in current buffer 00710 * \param branch_arg: reference to octree branch class 00711 * \param child_idx_arg: index to child node 00712 * \return pointer of new branch child to this reference 00713 * */ 00714 inline BranchNode* createBranchChild (BranchNode& branch_arg, 00715 unsigned char child_idx_arg) 00716 { 00717 BranchNode* new_branch_child = new BranchNode(); 00718 00719 branch_arg.setChildPtr (buffer_selector_, child_idx_arg, 00720 static_cast<OctreeNode*> (new_branch_child)); 00721 00722 return new_branch_child; 00723 } 00724 00725 /** \brief Fetch and add a new leaf child to a branch class 00726 * \param branch_arg: reference to octree branch class 00727 * \param child_idx_arg: index to child node 00728 * \return pointer of new leaf child to this reference 00729 * */ 00730 inline LeafNode* 00731 createLeafChild (BranchNode& branch_arg, unsigned char child_idx_arg) 00732 { 00733 LeafNode* new_leaf_child = new LeafNode(); 00734 00735 branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_leaf_child); 00736 00737 return new_leaf_child; 00738 } 00739 00740 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00741 // Recursive octree methods 00742 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00743 00744 /** \brief Create a leaf node at octree key. If leaf node does already exist, it is returned. 00745 * \param key_arg: reference to an octree key 00746 * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator 00747 * \param branch_arg: current branch node 00748 * \param return_leaf_arg: return pointer to leaf container 00749 * \param parent_of_leaf_arg: return pointer to parent of leaf node 00750 * \param branch_reset_arg: Reset pointer array of current branch 00751 * \return depth mask at which leaf node was created/found 00752 **/ 00753 unsigned int 00754 createLeafRecursive (const OctreeKey& key_arg, 00755 unsigned int depth_mask_arg, 00756 BranchNode* branch_arg, 00757 LeafNode*& return_leaf_arg, 00758 BranchNode*& parent_of_leaf_arg, 00759 bool branch_reset_arg = false); 00760 00761 00762 /** \brief Recursively search for a given leaf node and return a pointer. 00763 * \note If leaf node does not exist, a 0 pointer is returned. 00764 * \param key_arg: reference to an octree key 00765 * \param depth_mask_arg: depth mask used for octree key analysis and for branch depth indicator 00766 * \param branch_arg: current branch node 00767 * \param result_arg: pointer to leaf container class 00768 **/ 00769 void 00770 findLeafRecursive (const OctreeKey& key_arg, 00771 unsigned int depth_mask_arg, 00772 BranchNode* branch_arg, 00773 LeafContainerT*& result_arg) const; 00774 00775 00776 /** \brief Recursively search and delete leaf node 00777 * \param key_arg: reference to an octree key 00778 * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator 00779 * \param branch_arg: current branch node 00780 * \return "true" if branch does not contain any childs; "false" otherwise. This indicates if current branch can be deleted. 00781 **/ 00782 bool 00783 deleteLeafRecursive (const OctreeKey& key_arg, 00784 unsigned int depth_mask_arg, 00785 BranchNode* branch_arg); 00786 00787 /** \brief Recursively explore the octree and output binary octree description together with a vector of leaf node DataT content. 00788 * \param branch_arg: current branch node 00789 * \param key_arg: reference to an octree key 00790 * \param binary_tree_out_arg: binary output vector 00791 * \param leaf_container_vector_arg: vector to return pointers to all leaf container in the tree. 00792 * \param do_XOR_encoding_arg: select if binary tree structure should be generated based on current octree (false) of based on a XOR comparison between current and previous octree 00793 * \param new_leafs_filter_arg: execute callback only for leaf nodes that did not exist in preceding buffer 00794 **/ 00795 void 00796 serializeTreeRecursive (BranchNode* branch_arg, 00797 OctreeKey& key_arg, 00798 std::vector<char>* binary_tree_out_arg, 00799 typename std::vector<LeafContainerT*>* leaf_container_vector_arg, 00800 bool do_XOR_encoding_arg = false, 00801 bool new_leafs_filter_arg = false); 00802 00803 /** \brief Rebuild an octree based on binary XOR octree description and DataT objects for leaf node initialization. 00804 * \param branch_arg: current branch node 00805 * \param depth_mask_arg: depth mask used for octree key analysis and branch depth indicator 00806 * \param key_arg: reference to an octree key 00807 * \param binary_tree_in_it_arg iterator of binary input data 00808 * \param leaf_container_vector__it_end_arg end iterator of binary input data 00809 * \param leaf_container_vector_it_arg: iterator pointing to leaf containter pointers to be added to a leaf node 00810 * \param leaf_container_vector_it_end_arg: iterator pointing to leaf containter pointers pointing to last object in input container. 00811 * \param branch_reset_arg: Reset pointer array of current branch 00812 * \param do_XOR_decoding_arg: select if binary tree structure is based on current octree (false) of based on a XOR comparison between current and previous octree 00813 **/ 00814 void 00815 deserializeTreeRecursive (BranchNode* branch_arg, 00816 unsigned int depth_mask_arg, 00817 OctreeKey& key_arg, 00818 typename std::vector<char>::const_iterator& binary_tree_in_it_arg, 00819 typename std::vector<char>::const_iterator& binary_tree_in_it_end_arg, 00820 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg, 00821 typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_end_arg, 00822 bool branch_reset_arg = false, 00823 bool do_XOR_decoding_arg = false); 00824 00825 00826 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00827 // Serialization callbacks 00828 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00829 00830 /** \brief Callback executed for every leaf node data during serialization 00831 **/ 00832 virtual void serializeTreeCallback (LeafContainerT &, const OctreeKey &) 00833 { 00834 00835 } 00836 00837 /** \brief Callback executed for every leaf node data during deserialization 00838 **/ 00839 virtual void deserializeTreeCallback (LeafContainerT&, const OctreeKey&) 00840 { 00841 00842 } 00843 00844 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00845 // Helpers 00846 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00847 00848 /** \brief Recursively explore the octree and remove unused branch and leaf nodes 00849 * \param branch_arg: current branch node 00850 **/ 00851 void 00852 treeCleanUpRecursive (BranchNode* branch_arg); 00853 00854 /** \brief Helper function to calculate the binary logarithm 00855 * \param n_arg: some value 00856 * \return binary logarithm (log2) of argument n_arg 00857 */ 00858 inline double Log2 (double n_arg) 00859 { 00860 return log (n_arg) / log (2.0); 00861 } 00862 00863 /** \brief Test if octree is able to dynamically change its depth. This is required for adaptive bounding box adjustment. 00864 * \return "false" - not resizeable due to XOR serialization 00865 **/ 00866 inline bool octreeCanResize () 00867 { 00868 return (false); 00869 } 00870 00871 /** \brief Prints binary representation of a byte - used for debugging 00872 * \param data_arg - byte to be printed to stdout 00873 **/ 00874 inline void printBinary (char data_arg) 00875 { 00876 unsigned char mask = 1; // Bit mask 00877 00878 // Extract the bits 00879 for (int i = 0; i < 8; i++) 00880 { 00881 // Mask each bit in the byte and print it 00882 std::cout << ((data_arg & (mask << i)) ? "1" : "0"); 00883 } 00884 std::cout << std::endl; 00885 } 00886 00887 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00888 // Globals 00889 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00890 00891 /** \brief Amount of leaf nodes **/ 00892 std::size_t leaf_count_; 00893 00894 /** \brief Amount of branch nodes **/ 00895 std::size_t branch_count_; 00896 00897 /** \brief Pointer to root branch node of octree **/ 00898 BranchNode* root_node_; 00899 00900 /** \brief Depth mask based on octree depth **/ 00901 unsigned int depth_mask_; 00902 00903 /** \brief key range */ 00904 OctreeKey max_key_; 00905 00906 /** \brief Currently active octree buffer **/ 00907 unsigned char buffer_selector_; 00908 00909 // flags indicating if unused branches and leafs might exist in previous buffer 00910 bool tree_dirty_flag_; 00911 00912 /** \brief Octree depth */ 00913 unsigned int octree_depth_; 00914 00915 /** \brief Enable dynamic_depth 00916 * \note Note that this parameter is ignored in octree2buf! */ 00917 bool dynamic_depth_enabled_; 00918 00919 }; 00920 } 00921 } 00922 00923 //#include "impl/octree2buf_base.hpp" 00924 00925 #endif 00926