Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/octree/include/pcl/octree/octree2buf_base.h
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