octree2buf_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: octree2buf_base.h 6201 2012-07-06 11:53:17Z jkammerl $
 */

#ifndef OCTREE_TREE_2BUF_BASE_H
#define OCTREE_TREE_2BUF_BASE_H

#include <vector>

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

#include <stdio.h>
#include <string.h>

namespace pcl
{
  namespace octree
  {

    template<typename ContainerT>
    class BufferedBranchNode : public OctreeNode, ContainerT
    {
        using ContainerT::getSize;
        using ContainerT::getData;
        using ContainerT::setData;

      public:
        BufferedBranchNode () : OctreeNode(), ContainerT(),  preBuf(0xFFFFFF), postBuf(0xFFFFFF)
        {
          reset ();
        }

        BufferedBranchNode (const BufferedBranchNode& source) : ContainerT(source)
        {
          *this = source;
        }

        inline BufferedBranchNode&
        operator = (const BufferedBranchNode &source_arg)
        {

          unsigned char i, b;

          memset (childNodeArray_, 0, sizeof(childNodeArray_));

          for (b = 0; b < 2; ++b)
          for (i = 0; i < 8; ++i)
            if (source_arg.childNodeArray_[b][i])
              childNodeArray_[b][i] = source_arg.childNodeArray_[b][i]->deepCopy ();

          return (*this);

        }

        virtual ~BufferedBranchNode ()
        {
        }

        virtual BufferedBranchNode*
        deepCopy () const
        {
          return new BufferedBranchNode (*this);
        }

        inline OctreeNode*
        getChildPtr (unsigned char buffer_arg, unsigned char index_arg) const
        {
          assert( (buffer_arg<2) && (index_arg<8));
          return childNodeArray_[buffer_arg][index_arg];
        }

        inline void setChildPtr (unsigned char buffer_arg, unsigned char index_arg,
            OctreeNode* newNode_arg)
        {
          assert( (buffer_arg<2) && (index_arg<8));
          childNodeArray_[buffer_arg][index_arg] = newNode_arg;
        }

        inline bool hasChild (unsigned char buffer_arg, unsigned char index_arg) const
        {
          assert( (buffer_arg<2) && (index_arg<8));
          return (childNodeArray_[buffer_arg][index_arg] != 0);
        }

        virtual node_type_t getNodeType () const
        {
          return BRANCH_NODE;
        }

        inline void reset ()
        {
          memset (&childNodeArray_[0][0], 0, sizeof(OctreeNode*) * 8 * 2);
          ContainerT::reset ();
        }

      protected:
        int preBuf;
        OctreeNode* childNodeArray_[2][8];
        int postBuf;

    };

    template<typename DataT, typename LeafT = OctreeContainerDataT<DataT>,
        typename BranchT = OctreeContainerEmpty<DataT> >
    class Octree2BufBase
    {

      public:

        typedef Octree2BufBase<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 BufferedBranchNode<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;

        Octree2BufBase ();

        virtual
        ~Octree2BufBase ();

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

        inline Octree2BufBase&
        operator = (const Octree2BufBase& source)
        {
          leafCount_ = source.leafCount_;
          branchCount_ = source.branchCount_;
          objectCount_ = source.objectCount_;
          rootNode_ = new (BranchNode) (* (source.rootNode_));
          depthMask_ = source.depthMask_;
          maxKey_ = source.maxKey_;
          bufferSelector_ = source.bufferSelector_;
          treeDirtyFlag_ = source.treeDirtyFlag_;
          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_;
        }

        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 unsigned int getLeafCount () const
        {
          return (static_cast<unsigned int> (leafCount_));
        }

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

        void
        deleteTree (bool freeMemory_arg = false);

        inline void deletePreviousBuffer ()
        {
          treeCleanUpRecursive (rootNode_);
        }

        inline void deleteCurrentBuffer ()
        {
          bufferSelector_ = !bufferSelector_;
          treeCleanUpRecursive (rootNode_);
          leafCount_ = 0;
        }

        void
        switchBuffers ();

        void
        serializeTree (std::vector<char>& binaryTreeOut_arg,
            bool doXOREncoding_arg = false);

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

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

        void
        serializeNewLeafs (std::vector<DataT>& dataVector_arg,
            const int minPointsPerLeaf_arg = 0);

        void
        deserializeTree (std::vector<char>& binaryTreeIn_arg,
            bool doXORDecoding_arg = false);

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

      protected:

        // Protected octree methods based on octree keys

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

        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)
        {
          // request a (new) leaf from tree
          LeafT* leaf = createLeaf (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*
        createLeaf (const OctreeKey& key_arg)
        {
          LeafT* result;

          result = createLeafRecursive (key_arg, depthMask_, rootNode_, false);

          // getLeafRecursive has changed the octree -> clean-up/tree-reset might be required
          treeDirtyFlag_ = true;

          return result;
        }

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

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

            // we changed the octree structure -> dirty
            treeDirtyFlag_ = true;
          }
        }

        // Branch node accessor inline functions

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

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

        inline void setBranchChildPtr (BranchNode& branch_arg,
            unsigned char childIdx_arg, OctreeNode* newChild_arg)
        {
          branch_arg.setChildPtr(bufferSelector_, 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++)
          {
            nodeBits |= static_cast<char> ( (!!branch_arg.getChildPtr (
                bufferSelector_, i)) << i);
          }

          return (nodeBits);
        }

        inline char getBranchBitPattern (const BranchNode& branch_arg,
            unsigned char bufferSelector_arg) const
        {
          unsigned char i;
          char nodeBits;

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

          return (nodeBits);
        }

        inline char getBranchXORBitPattern (
            const BranchNode& branch_arg) const
        {
          unsigned char i;
          char nodeBits[2];

          // create bit pattern for both buffers
          nodeBits[0] = nodeBits[1] = 0;

          for (i = 0; i < 8; i++)
          {
            nodeBits[0] |= static_cast<char> ( (!!branch_arg.getChildPtr (0, i))
                << i);
            nodeBits[1] |= static_cast<char> ( (!!branch_arg.getChildPtr (1, i))
                << i);
          }

          return nodeBits[0] ^ nodeBits[1];
        }

        inline bool hasBranchChanges (const BranchNode& branch_arg) const
        {
          return (getBranchXORBitPattern (branch_arg) > 0);
        }

        inline void deleteBranchChild (BranchNode& branch_arg,
            unsigned char bufferSelector_arg,
            unsigned char childIdx_arg)
        {
          if (branch_arg.hasChild(bufferSelector_arg, childIdx_arg))
          {
            OctreeNode* branchChild = branch_arg.getChildPtr(bufferSelector_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.setChildPtr(bufferSelector_arg, childIdx_arg, 0);
          }
        }

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

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

            if (branch_arg.getChildPtr(0, i) == branch_arg.getChildPtr(1, i))
            {
              // reference was copied - there is only one child instance to be deleted
              deleteBranchChild (branch_arg, 0, i);

              // remove pointers from both buffers
              branch_arg.setChildPtr(0, i, 0);
              branch_arg.setChildPtr(1, i, 0);
            }
            else
            {
              deleteBranchChild (branch_arg, 0, i);
              deleteBranchChild (branch_arg, 1, i);
            }
          }
        }

        inline void createBranchChild (BranchNode& branch_arg,
            unsigned char childIdx_arg, BranchNode*& newBranchChild_arg)
        {

          newBranchChild_arg = branchNodePool_.popNode();

          branch_arg.setChildPtr (bufferSelector_, 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.setChildPtr(bufferSelector_, childIdx_arg, newLeafChild_arg);
        }

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

        // Recursive octree methods

        LeafT*
        createLeafRecursive (const OctreeKey& key_arg,
            unsigned int depthMask_arg, BranchNode* branch_arg,
            bool branchReset_arg);

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

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

        void
        serializeTreeRecursive (BranchNode* branch_arg, OctreeKey& key_arg,
            std::vector<char>* binaryTreeOut_arg,
            typename std::vector<DataT>* dataVector_arg, bool doXOREncoding_arg = false,
            bool newLeafsFilter_arg = false, std::size_t minPointsPerLeaf_arg = 0);

        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,
            bool branchReset_arg = false, bool doXORDecoding_arg = false);


        // Serialization callbacks

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

        }

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

        }

        // Helpers

        void
        treeCleanUpRecursive (BranchNode* branch_arg);

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

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

        inline void printBinary (char data_arg)
        {
          unsigned char mask = 1;  // Bit mask

          // Extract the bits
          for (int i = 0; i < 8; i++)
          {
            // Mask each bit in the byte and print it
            std::cout << ((data_arg & (mask << i)) ? "1" : "0");
          }
          std::cout << std::endl;
        }

        // Globals

        std::size_t leafCount_;

        std::size_t branchCount_;

        std::size_t objectCount_;

        BranchNode* rootNode_;

        unsigned int depthMask_;

        OctreeKey maxKey_;

        OctreeNodePool<BranchNode> branchNodePool_;

        OctreeNodePool<LeafNode> leafNodePool_;

        unsigned char bufferSelector_;

        // flags indicating if unused branches and leafs might exist in previous buffer
        bool treeDirtyFlag_;

        unsigned int octreeDepth_;
    };
  }
}

//#include "impl/octree2buf_base.hpp"

#endif