octree_base.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2012, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: octree_base.h 6201 2012-07-06 11:53:17Z jkammerl $
 */

#ifndef OCTREE_TREE_BASE_H
#define OCTREE_TREE_BASE_H

#include <cstddef>
#include <vector>

#include "octree_nodes.h"
#include "octree_container.h"
#include "octree_key.h"
#include "octree_iterator.h"
#include "octree_node_pool.h"

namespace pcl
{
  namespace octree
  {
    template<typename DataT, typename LeafT = OctreeContainerDataT<DataT>,
        typename BranchT = OctreeContainerEmpty<DataT> >
    class OctreeBase
    {

      public:

        typedef OctreeBase<DataT, OctreeContainerDataT<DataT>, OctreeContainerEmpty<DataT> > SingleObjLeafContainer;
        typedef OctreeBase<DataT, OctreeContainerDataTVector<DataT>, OctreeContainerEmpty<DataT> > MultipleObjsLeafContainer;

        typedef OctreeBase<DataT, LeafT, BranchT> OctreeT;

        // iterators are friends
        friend class OctreeIteratorBase<DataT, OctreeT> ;
        friend class OctreeDepthFirstIterator<DataT, OctreeT> ;
        friend class OctreeBreadthFirstIterator<DataT, OctreeT> ;
        friend class OctreeLeafNodeIterator<DataT, OctreeT> ;

        typedef OctreeBranchNode<BranchT> BranchNode;
        typedef OctreeLeafNode<LeafT> LeafNode;

        // Octree iterators
        typedef OctreeDepthFirstIterator<DataT, OctreeT> Iterator;
        typedef const OctreeDepthFirstIterator<DataT, OctreeT> ConstIterator;

        typedef OctreeLeafNodeIterator<DataT, OctreeT> LeafNodeIterator;
        typedef const OctreeLeafNodeIterator<DataT, OctreeT> ConstLeafNodeIterator;

        typedef OctreeDepthFirstIterator<DataT, OctreeT> DepthFirstIterator;
        typedef const OctreeDepthFirstIterator<DataT, OctreeT> ConstDepthFirstIterator;
        typedef OctreeBreadthFirstIterator<DataT, OctreeT> BreadthFirstIterator;
        typedef const OctreeBreadthFirstIterator<DataT, OctreeT> ConstBreadthFirstIterator;


        OctreeBase ();

        virtual
        ~OctreeBase ();

        OctreeBase (const OctreeBase& source) :
          leafCount_ (source.leafCount_),
          branchCount_ (source.branchCount_),
          objectCount_ (source.objectCount_),
          rootNode_ (new (BranchNode) (*(source.rootNode_))),
          depthMask_ (source.depthMask_),
          octreeDepth_ (source.octreeDepth_),
          maxKey_ (source.maxKey_),
          branchNodePool_ (),
          leafNodePool_ ()
        {
        }

        inline OctreeBase& 
        operator = (const OctreeBase &source)
        {
          leafCount_ = source.leafCount_;
          branchCount_ = source.branchCount_;
          objectCount_ = source.objectCount_;
          rootNode_ = new (BranchNode) (*(source.rootNode_));
          depthMask_ = source.depthMask_;
          maxKey_ = source.maxKey_;
          octreeDepth_ = source.octreeDepth_;
          return (*this);
        }

        void
        setMaxVoxelIndex (unsigned int maxVoxelIndex_arg);

        void
        setTreeDepth (unsigned int depth_arg);

        inline unsigned int
        getTreeDepth () const
        {
          return this->octreeDepth_;
        }

        inline void
        enableDynamicDepth ( size_t maxObjsPerLeaf )
        {
          assert(leafCount_==0);
          maxObjsPerLeaf_ = maxObjsPerLeaf;
        }

        void
        addData (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg,
             const DataT& data_arg);

        bool
        getData (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg, DataT& data_arg) const ;

        bool
        existLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg) const ;

        void
        removeLeaf (unsigned int idxX_arg, unsigned int idxY_arg, unsigned int idxZ_arg);

        inline std::size_t
        getLeafCount () const
        {
          return leafCount_;
        }

        inline std::size_t
        getBranchCount () const
        {
          return branchCount_;
        }

        void
        deleteTree ( bool freeMemory_arg = true );

        void
        serializeTree (std::vector<char>& binaryTreeOut_arg);

        void
        serializeTree (std::vector<char>& binaryTreeOut_arg, std::vector<DataT>& dataVector_arg);

        void
        serializeLeafs (std::vector<DataT>& dataVector_arg);

        void
        deserializeTree (std::vector<char>& binaryTreeIn_arg);

        void
        deserializeTree (std::vector<char>& binaryTreeIn_arg, std::vector<DataT>& dataVector_arg);

      protected:
        
        // Protected octree methods based on octree keys

        virtual bool
        genOctreeKeyForDataT (const DataT &, OctreeKey &) const
        {
          // this class cannot relate DataT objects to octree keys
          return (false);
        }

        virtual bool
        genDataTByOctreeKey (const OctreeKey &, DataT &) const
        {
          // this class cannot relate DataT objects to octree keys
          return (false);
        }

        inline void
        addData (const OctreeKey& key_arg, const DataT& data_arg)
        {
          addDataToLeafRecursive (key_arg, depthMask_,data_arg, rootNode_);
        }

        inline LeafNode*
        findLeaf (const OctreeKey& key_arg) const
        {
          LeafNode* result = 0;
          findLeafRecursive (key_arg, depthMask_, rootNode_, result);
          return result;
        }

        inline bool
        existLeaf (const OctreeKey& key_arg) const
        {
          LeafNode* leafNode = 0;
          if (key_arg <= maxKey_)
            findLeafRecursive (key_arg, depthMask_, rootNode_, leafNode);
          return (leafNode != 0);
        }

        inline void
        removeLeaf (const OctreeKey& key_arg)
        {
          if (key_arg <= maxKey_)
            deleteLeafRecursive (key_arg, depthMask_, rootNode_);
        }

        // Branch node accessor inline functions

        OctreeNode*
        getRootNode () const
        {
          return this->rootNode_;
        }

        inline bool
        branchHasChild (const BranchNode& branch_arg, unsigned char childIdx_arg) const
        {
          // test occupancyByte for child existence
          return (branch_arg.getChildPtr(childIdx_arg) != 0);
        }

        inline OctreeNode*
        getBranchChildPtr (const BranchNode& branch_arg,
            unsigned char childIdx_arg) const
        {
          return branch_arg.getChildPtr(childIdx_arg);
        }

        inline void setBranchChildPtr (BranchNode& branch_arg,
            unsigned char childIdx_arg, OctreeNode* newChild_arg)
        {
          branch_arg[childIdx_arg] = newChild_arg;
        }

        inline void getDataFromOctreeNode (const OctreeNode* node_arg,
            DataT& data_arg)
        {
          if (node_arg->getNodeType () == LEAF_NODE)
          {
            const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
            leafContainer->getData (data_arg);
          }
          else
          {
            const BranchT* branchContainer =
                dynamic_cast<const BranchT*> (node_arg);
            branchContainer->getData (data_arg);
          }
        }

        inline void getDataFromOctreeNode (const OctreeNode* node_arg,
            std::vector<DataT>& data_arg)
        {
          if (node_arg->getNodeType () == LEAF_NODE)
          {
            const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
            leafContainer->getData (data_arg);
          }
          else
          {
            const BranchT* branchContainer =
                dynamic_cast<const BranchT*> (node_arg);
            branchContainer->getData (data_arg);
          }
        }

        inline size_t getDataSizeFromOctreeNode (const OctreeNode* node_arg)
        {
          size_t nodeSize;
          if (node_arg->getNodeType () == LEAF_NODE)
          {
            const LeafT* leafContainer = dynamic_cast<const LeafT*> (node_arg);
            nodeSize = leafContainer->getSize ();
          }
          else
          {
            const BranchT* branchContainer =
                dynamic_cast<const BranchT*> (node_arg);
            nodeSize = branchContainer->getSize ();
          }
          return nodeSize;
        }
        inline char
        getBranchBitPattern (const BranchNode& branch_arg) const
        {
          unsigned char i;
          char nodeBits;

          // create bit pattern
          nodeBits = 0;
          for (i = 0; i < 8; i++) {
            const OctreeNode* child = branch_arg.getChildPtr(i);
            nodeBits |= static_cast<char> ((!!child) << i);
          }

          return (nodeBits);
        }

        inline void
        deleteBranchChild (BranchNode& branch_arg, unsigned char childIdx_arg)
        {
          if (branch_arg.hasChild(childIdx_arg))
          {
            OctreeNode* branchChild = branch_arg[childIdx_arg];
            
            switch (branchChild->getNodeType ())
            {
              case BRANCH_NODE:
              {
                // free child branch recursively
                deleteBranch (*static_cast<BranchNode*> (branchChild));
                // push unused branch to branch pool
                branchNodePool_.pushNode (
                    static_cast<BranchNode*> (branchChild));
              }
                break;

              case LEAF_NODE:
              {
                // push unused leaf to branch pool
                leafNodePool_.pushNode (static_cast<LeafNode*> (branchChild));
                break;
              }
              default:
                break;
            }

            // set branch child pointer to 0
            branch_arg[childIdx_arg] = 0;
          }
        }

        inline void
        deleteBranch (BranchNode& branch_arg)
        {
          char i;

          // delete all branch node children
          for (i = 0; i < 8; i++)
            deleteBranchChild (branch_arg, i);
        }

        inline void createBranchChild (BranchNode& branch_arg,
            unsigned char childIdx_arg, BranchNode*& newBranchChild_arg)
        {
          newBranchChild_arg = branchNodePool_.popNode ();
          branch_arg[childIdx_arg] =
              static_cast<OctreeNode*> (newBranchChild_arg);
        }

        inline void
        createLeafChild (BranchNode& branch_arg, unsigned char childIdx_arg, LeafNode*& newLeafChild_arg)
        {
          newLeafChild_arg = leafNodePool_.popNode();

          branch_arg[childIdx_arg] = static_cast<OctreeNode*> (newLeafChild_arg);
        }

        inline void
        branchReset (BranchNode& branch_arg)
        {
          branch_arg.reset ();
        }

        inline void
        poolCleanUp ()
        {
          branchNodePool_.deletePool();
          leafNodePool_.deletePool();
        }

        // Recursive octree methods

        void
        addDataToLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, const DataT& data_arg, BranchNode* branch_arg);

        void
        findLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg, LeafNode*& result_arg) const;

        bool
        deleteLeafRecursive (const OctreeKey& key_arg, unsigned int depthMask_arg, BranchNode* branch_arg);

        void
        serializeTreeRecursive (const BranchNode* branch_arg, OctreeKey& key_arg,
            std::vector<char>* binaryTreeOut_arg,
            typename std::vector<DataT>* dataVector_arg) const;

         void
         deserializeTreeRecursive (BranchNode* branch_arg,
             unsigned int depthMask_arg, OctreeKey& key_arg,
             typename std::vector<char>::const_iterator& binaryTreeIT_arg,
             typename std::vector<char>::const_iterator& binaryTreeIT_End_arg,
             typename std::vector<DataT>::const_iterator* dataVectorIterator_arg,
             typename std::vector<DataT>::const_iterator* dataVectorEndIterator_arg);


        // Serialization callbacks

        virtual void serializeTreeCallback (const LeafNode &,
            const OctreeKey &) const
        {

        }

        virtual void deserializeTreeCallback (LeafNode&, const OctreeKey&)
        {

        }

        // Helpers

        inline double
        Log2 (double n_arg)
        {
          return log( n_arg ) / log( 2.0 );
        }

        inline bool
        octreeCanResize ()
        {
          return (true);
        }

        // Globals

        std::size_t leafCount_;

        std::size_t branchCount_;

        std::size_t objectCount_;

        BranchNode* rootNode_;

        std::size_t maxObjsPerLeaf_;

        unsigned int depthMask_;

        unsigned int octreeDepth_;

        OctreeKey maxKey_;

        OctreeNodePool<BranchNode> branchNodePool_;

        OctreeNodePool<LeafNode> leafNodePool_;
    };
  }
}

//#include "impl/octree_base.hpp"

#endif