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 4702 2012-02-23 09:39:33Z gedikli $
 */

#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>, typename OctreeBranchT = OctreeBranch >
    class OctreeBase
    {

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

    public:
      typedef OctreeBranchT OctreeBranch;

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

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

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

      OctreeBase ();

      virtual
      ~OctreeBase ();

      OctreeBase (const OctreeBase& source)
      {
        leafCount_ = source.leafCount_;
        branchCount_ = source.branchCount_;
        objectCount_ = source.objectCount_;
        rootNode_ = new (OctreeBranch) (*(source.rootNode_));
        depthMask_ = source.depthMask_;
        octreeDepth_ = source.octreeDepth_;
      }

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

      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_++;
        }
      }

      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[childIdx_arg];
      }

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

      inline bool
      branchHasChild (const OctreeBranch& branch_arg, const unsigned char childIdx_arg) const
      {
        return (branch_arg[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[i]) << i;
        }

        return nodeBits;
      }
       
      inline void
      setBranchChild (OctreeBranch& branch_arg, const unsigned char childIdx_arg, const OctreeNode * newChild_arg)
      {
        branch_arg[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;
              
            default:
              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)
      {
        branch_arg.reset ();
      }

      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