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-2011, 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 3749 2011-12-31 22:58:01Z rusu $
 */

#ifndef OCTREE_TREE_BASE_H
#define OCTREE_TREE_BASE_H

#include <cstddef>
#include <vector>

#include "octree_nodes.h"

#include "octree_iterator.h"

namespace pcl
{
  namespace octree
  {


    template<typename DataT, typename LeafT = OctreeLeafDataT<DataT> >
      class OctreeBase
      {

        friend class OctreeNodeIterator<DataT, LeafT, OctreeBase> ;
        friend class OctreeLeafNodeIterator<DataT, LeafT, OctreeBase> ;

      public:

        // Octree iterators
        typedef OctreeNodeIterator<DataT, LeafT, OctreeBase> Iterator;
        typedef const OctreeNodeIterator<DataT, LeafT, OctreeBase> ConstIterator;

        // Octree iterators
        typedef OctreeLeafNodeIterator<DataT, LeafT, OctreeBase> LeafNodeIterator;
        typedef const OctreeLeafNodeIterator<DataT, LeafT, OctreeBase> ConstLeafNodeIterator;

        OctreeBase ();

        virtual
        ~OctreeBase ();

        void
        setMaxVoxelIndex (unsigned int maxVoxelIndex_arg);

        void
        setTreeDepth (unsigned int depth_arg);

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

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

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

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

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

        inline unsigned int
        getLeafCount () const
        {
          return leafCount_;
        }

        inline unsigned int
        getBranchCount () const
        {
          return branchCount_;
        }

        void
        deleteTree ( bool freeMemory_arg = false );

        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);

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

      protected:



        class OctreeKey
        {
        public:

          bool
          operator == (const OctreeKey& b) const
          {
            return ((b.x == this->x) && (b.y == this->y) && (b.z == this->z));
          }

          // Indices addressing a voxel at (X, Y, Z)
          unsigned int x;unsigned int y;unsigned int z;
        };



        class OctreeBranch : public OctreeNode
        {
          // OctreeBase is a friend!
          friend class OctreeBase;

        public:

          OctreeBranch ()
          {
            memset (this->subNodes_, 0, sizeof(this->subNodes_));
          }

          virtual
          ~OctreeBranch ()
          {
          }

          virtual node_type_t
          getNodeType () const
          {
            return BRANCH_NODE;
          }

        private:

          const OctreeNode * subNodes_[8];

        };

        typedef LeafT OctreeLeaf;

        // Protected octree methods based on octree keys


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

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

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

        inline void
        genOctreeKeyByIntIdx (const unsigned int idxX_arg, const unsigned int idxY_arg, const unsigned int idxZ_arg,
                              OctreeKey & key_arg) const
        {
          // copy data to octree key class
          key_arg.x = idxX_arg;
          key_arg.y = idxY_arg;
          key_arg.z = idxZ_arg;
        }

        inline void
        add (const OctreeKey& key_arg, const DataT& data_arg)
        {
          // request a (new) leaf from tree
          LeafT* leaf = getLeaf (key_arg);

          // assign data to leaf
          if (leaf)
          {
            leaf->setData (data_arg);
            objectCount_++;
          }
        }

        void
        add (const std::vector<OctreeKey>& key_vector_arg, const std::vector<DataT>& data_vector_arg);

        inline LeafT*
        findLeaf (const OctreeKey& key_arg) const
        {
          return findLeafRecursive (key_arg, depthMask_, rootNode_);
        }

        inline LeafT*
        getLeaf (const OctreeKey& key_arg)
        {
          return getLeafRecursive (key_arg, depthMask_, rootNode_);
        }

        inline bool
        existLeaf (const OctreeKey& key_arg) const
        {
          return (findLeafRecursive (key_arg, depthMask_, rootNode_) != 0);
        }

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

        // Branch node accessor inline functions

        inline const OctreeNode*
        getBranchChild (const OctreeBranch& branch_arg, const unsigned char childIdx_arg) const
        {
          return branch_arg.subNodes_[childIdx_arg];
        }

        inline bool
        branchHasChild (const OctreeBranch& branch_arg, const unsigned char childIdx_arg) const
        {
          return (branch_arg.subNodes_[childIdx_arg] != 0);
        }

        inline char
        getBranchBitPattern (const OctreeBranch& branch_arg) const
        {
          unsigned char i;
          char nodeBits;

          // create bit pattern
          nodeBits = 0;
          for (i = 0; i < 8; i++)
          {
            nodeBits |= (!!branch_arg.subNodes_[i]) << i;
          }

          return nodeBits;
        }

        inline void
        setBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, const OctreeNode * newChild_arg)
        {
          branch_arg.subNodes_[childIdx_arg] = newChild_arg;
        }

        inline void
        deleteBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg)
        {
          if (branchHasChild (branch_arg, childIdx_arg))
          {
            const OctreeNode* branchChild;
            branchChild = getBranchChild (branch_arg, childIdx_arg);

            switch (branchChild->getNodeType ())
            {
              case BRANCH_NODE:
                // free child branch recursively
                deleteBranch (*(OctreeBranch*)branchChild);

                // push unused branch to branch pool
                unusedBranchesPool_.push_back ((OctreeBranch*)branchChild);
                break;

              case LEAF_NODE:

                // push unused leaf to branch pool
                unusedLeafsPool_.push_back ((OctreeLeaf*)branchChild);
                break;
            }

            // set branch child pointer to 0
            setBranchChild (branch_arg, childIdx_arg, 0);
          }
        }

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

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

        inline void
        createBranch (OctreeBranch*& newBranchChild_arg)
        {
          if (!unusedBranchesPool_.size ())
          {
            // branch pool is empty
            // we need to create a new octree branch class
            newBranchChild_arg = (OctreeBranch*)new OctreeBranch ();
          }
          else
          {
            // reuse branch from branch pool
            newBranchChild_arg = unusedBranchesPool_.back ();
            unusedBranchesPool_.pop_back ();
            branchReset (*newBranchChild_arg);
          }
        }

        inline void
        createBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg,
                           OctreeBranch*& newBranchChild_arg)
        {
          createBranch ( newBranchChild_arg );
          setBranchChild (branch_arg, childIdx_arg, (OctreeNode*)newBranchChild_arg);
        }


        inline void
        createLeafChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, OctreeLeaf*& newLeafChild_arg)
        {

          if (!unusedLeafsPool_.size ())
          {
            // leaf pool is empty
            // we need to create a new octree leaf class
            newLeafChild_arg = (OctreeLeaf*)new OctreeLeaf ();
          }
          else
          {
            // reuse leaf node from branch pool
            newLeafChild_arg = unusedLeafsPool_.back ();
            unusedLeafsPool_.pop_back ();
          }

          newLeafChild_arg->reset ();

          setBranchChild (branch_arg, childIdx_arg, (OctreeNode*)newLeafChild_arg);

        }

        inline void
        branchReset (OctreeBranch& branch_arg)
        {
          memset (branch_arg.subNodes_, 0, sizeof(branch_arg.subNodes_));
        }

        inline void
        poolCleanUp ()
        {
          // delete all branch instances from branch pool
          while (!unusedBranchesPool_.empty ())
          {
            delete (unusedBranchesPool_.back ());
            unusedBranchesPool_.pop_back ();
          }

          // delete all leaf instances from leaf pool
          while (!unusedLeafsPool_.empty ())
          {
            delete (unusedLeafsPool_.back ());
            unusedLeafsPool_.pop_back ();
          }
        }

        // Recursive octree methods


        LeafT*
        getLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg);

        LeafT*
        findLeafRecursive (const OctreeKey& key_arg, const unsigned int depthMask_arg, OctreeBranch* branch_arg) const;

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

        void
        serializeTreeRecursive (std::vector<char>& binaryTreeOut_arg, const OctreeBranch* branch_arg, const OctreeKey& key_arg);

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

        void
        serializeLeafsRecursive (const OctreeBranch* branch_arg, const OctreeKey& key_arg,
                                 typename std::vector<DataT>& dataVector_arg);

        void
        deserializeTreeRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg,
                                  OctreeBranch* branch_arg, const unsigned int depthMask_arg, const OctreeKey& key_arg);

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

        void
        deserializeTreeAndOutputLeafDataRecursive (typename std::vector<char>::const_iterator& binaryTreeIn_arg,
                                                   OctreeBranch* branch_arg, const unsigned int depthMask_arg,
                                                   const OctreeKey& key_arg,
                                                   typename std::vector<DataT>& dataVector_arg);

        // Serialization callbacks

        virtual void
        serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg);

        virtual void
        serializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg, std::vector<DataT>& dataVector_arg);

        virtual void
        deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg);

        virtual void
        deserializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey& key_arg,
                                 typename std::vector<DataT>::const_iterator& dataVectorIterator_arg,
                                 typename std::vector<DataT>::const_iterator& dataVectorEndIterator_arg);

        virtual void
        deserializeTreeAndSerializeLeafCallback (OctreeLeaf& leaf_arg, const OctreeKey & key_arg,
                                                 std::vector<DataT>& dataVector_arg);

        // Helpers

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

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

        // Globals

        unsigned int leafCount_;

        unsigned int branchCount_;

        unsigned int objectCount_;

        OctreeBranch* rootNode_;

        unsigned int depthMask_;

        unsigned int octreeDepth_;

        std::vector<OctreeBranch*> unusedBranchesPool_;

        std::vector<LeafT*> unusedLeafsPool_;

      };
  }
}

//#include "impl/octree_base.hpp"

#endif