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$
 */

#ifndef PCL_OCTREE_TREE_2BUF_BASE_H
#define PCL_OCTREE_TREE_2BUF_BASE_H

#include <vector>

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

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

namespace pcl
{
  namespace octree
  {

    template<typename ContainerT>
    class BufferedBranchNode : public OctreeNode
    {

      public:
        BufferedBranchNode () : OctreeNode()
        {
          reset ();
        }

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

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

          unsigned char i, b;

          memset (child_node_array_, 0, sizeof(child_node_array_));

          for (b = 0; b < 2; ++b)
            for (i = 0; i < 8; ++i)
              if (source_arg.child_node_array_[b][i])
                child_node_array_[b][i] = source_arg.child_node_array_[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 child_node_array_[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));
          child_node_array_[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 (child_node_array_[buffer_arg][index_arg] != 0);
        }

        virtual node_type_t getNodeType () const
        {
          return BRANCH_NODE;
        }

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

        const ContainerT*
        operator->() const
        {
          return &container_;
        }

        ContainerT*
        operator-> ()
        {
          return &container_;
        }

        const ContainerT&
        operator* () const
        {
          return container_;
        }

        ContainerT&
        operator* ()
        {
          return container_;
        }

        const ContainerT&
        getContainer () const
        {
          return container_;
        }

        ContainerT&
        getContainer ()
        {
          return container_;
        }

        const ContainerT*
        getContainerPtr() const
        {
          return &container_;
        }

        ContainerT*
        getContainerPtr ()
        {
          return &container_;
        }

      protected:
        ContainerT container_;

        OctreeNode* child_node_array_[2][8];
    };

    template<typename LeafContainerT = int,
             typename BranchContainerT = OctreeContainerEmpty >
    class Octree2BufBase
    {

      public:

        typedef Octree2BufBase<LeafContainerT, BranchContainerT> OctreeT;

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

        typedef BufferedBranchNode<BranchContainerT> BranchNode;
        typedef OctreeLeafNode<LeafContainerT> LeafNode;

        typedef BranchContainerT BranchContainer;
        typedef LeafContainerT LeafContainer;

        // Octree default iterators
        typedef OctreeDepthFirstIterator<OctreeT> Iterator;
        typedef const OctreeDepthFirstIterator<OctreeT> ConstIterator;
        Iterator begin(unsigned int max_depth_arg = 0) {return Iterator(this, max_depth_arg);};
        const Iterator end() {return Iterator();};

        // Octree leaf node iterators
        typedef OctreeLeafNodeIterator<OctreeT> LeafNodeIterator;
        typedef const OctreeLeafNodeIterator<OctreeT> ConstLeafNodeIterator;
        LeafNodeIterator leaf_begin(unsigned int max_depth_arg = 0) {return LeafNodeIterator(this, max_depth_arg);};
        const LeafNodeIterator leaf_end() {return LeafNodeIterator();};

        // Octree depth-first iterators
        typedef OctreeDepthFirstIterator<OctreeT> DepthFirstIterator;
        typedef const OctreeDepthFirstIterator<OctreeT> ConstDepthFirstIterator;
        DepthFirstIterator depth_begin(unsigned int maxDepth_arg = 0) {return DepthFirstIterator(this, maxDepth_arg);};
        const DepthFirstIterator depth_end() {return DepthFirstIterator();};

        // Octree breadth-first iterators
        typedef OctreeBreadthFirstIterator<OctreeT> BreadthFirstIterator;
        typedef const OctreeBreadthFirstIterator<OctreeT> ConstBreadthFirstIterator;
        BreadthFirstIterator breadth_begin(unsigned int max_depth_arg = 0) {return BreadthFirstIterator(this, max_depth_arg);};
        const BreadthFirstIterator breadth_end() {return BreadthFirstIterator();};

        Octree2BufBase ();

        virtual
        ~Octree2BufBase ();

        Octree2BufBase (const Octree2BufBase& source) :
            leaf_count_ (source.leaf_count_),
            branch_count_ (source.branch_count_),
            root_node_ (new (BranchNode) (*(source.root_node_))),
            depth_mask_ (source.depth_mask_),
            max_key_ (source.max_key_),
            buffer_selector_ (source.buffer_selector_),
            tree_dirty_flag_ (source.tree_dirty_flag_),
            octree_depth_ (source.octree_depth_),
            dynamic_depth_enabled_(source.dynamic_depth_enabled_)
        {
        }

        inline Octree2BufBase&
        operator = (const Octree2BufBase& source)
        {
          leaf_count_ = source.leaf_count_;
          branch_count_ = source.branch_count_;
          root_node_ = new (BranchNode) (* (source.root_node_));
          depth_mask_ = source.depth_mask_;
          max_key_ = source.max_key_;
          buffer_selector_ = source.buffer_selector_;
          tree_dirty_flag_ = source.tree_dirty_flag_;
          octree_depth_ = source.octree_depth_;
          dynamic_depth_enabled_ = source.dynamic_depth_enabled_;
          return (*this);
        }

        void
        setMaxVoxelIndex (unsigned int max_voxel_index_arg);

        void
        setTreeDepth (unsigned int depth_arg);

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

        LeafContainerT*
        createLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);

        LeafContainerT*
        findLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);

        bool
        existLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg) const;

        void
        removeLeaf (unsigned int idx_x_arg, unsigned int idx_y_arg, unsigned int idx_z_arg);

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

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

        void
        deleteTree ();

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

        inline void deleteCurrentBuffer ()
        {
          buffer_selector_ = !buffer_selector_;
          treeCleanUpRecursive (root_node_);
          leaf_count_ = 0;
        }

        void
        switchBuffers ();

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

        void
        serializeTree (std::vector<char>& binary_tree_out_arg,
                       std::vector<LeafContainerT*>& leaf_container_vector_arg,
                       bool do_XOR_encoding_arg = false);

        void
        serializeLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg);

        void
        serializeNewLeafs (std::vector<LeafContainerT*>& leaf_container_vector_arg);

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

        void
        deserializeTree (std::vector<char>& binary_tree_in_arg,
                         std::vector<LeafContainerT*>& leaf_container_vector_arg,
                         bool do_XOR_decoding_arg = false);

      protected:

        // Protected octree methods based on octree keys

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

        inline LeafContainerT*
        findLeaf (const OctreeKey& key_arg) const
        {
          LeafContainerT* result = 0;
          findLeafRecursive (key_arg, depth_mask_, root_node_, result);
          return result;
        }

        inline LeafContainerT*
        createLeaf (const OctreeKey& key_arg)
        {
          LeafNode* leaf_node;
          BranchNode* leaf_node_parent;

          createLeafRecursive (key_arg, depth_mask_ ,root_node_, leaf_node, leaf_node_parent, false);

          LeafContainerT* ret = leaf_node->getContainerPtr();

          return ret;
        }

        inline bool existLeaf (const OctreeKey& key_arg) const
        {
          return (findLeaf(key_arg) != 0);
        }

        inline void removeLeaf (const OctreeKey& key_arg)
        {
          if (key_arg <= max_key_)
          {
            deleteLeafRecursive (key_arg, depth_mask_, root_node_);

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

        // Branch node accessor inline functions

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

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

        inline void
        setBranchChildPtr (BranchNode& branch_arg, unsigned char child_idx_arg, OctreeNode* new_child_arg)
        {
          branch_arg.setChildPtr (buffer_selector_, child_idx_arg, new_child_arg);
        }

        inline char getBranchBitPattern (const BranchNode& branch_arg) const
        {
          unsigned char i;
          char node_bits;

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

          return (node_bits);
        }

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

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

          return (node_bits);
        }

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

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

          for (i = 0; i < 8; i++)
          {
            const OctreeNode* childA = branch_arg.getChildPtr(0, i);
            const OctreeNode* childB = branch_arg.getChildPtr(1, i);

            node_bits[0] |= static_cast<char> ( (!!childA) << i);
            node_bits[1] |= static_cast<char> ( (!!childB) << i);
          }

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

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

        inline void deleteBranchChild (BranchNode& branch_arg,
            unsigned char buffer_selector_arg,
            unsigned char child_idx_arg)
        {
          if (branch_arg.hasChild(buffer_selector_arg, child_idx_arg))
          {
            OctreeNode* branchChild = branch_arg.getChildPtr(buffer_selector_arg, child_idx_arg);

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

                // delete unused branch
                delete (branchChild);
                break;
              }

              case LEAF_NODE:
              {
                // push unused leaf to branch pool
                delete (branchChild);
                break;
              }
              default:
                break;
            }

            // set branch child pointer to 0
            branch_arg.setChildPtr(buffer_selector_arg, child_idx_arg, 0);
          }
        }

        inline void deleteBranchChild (BranchNode& branch_arg,  unsigned char child_idx_arg)
        {
          deleteBranchChild(branch_arg, buffer_selector_, child_idx_arg);
        }

        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  BranchNode* createBranchChild (BranchNode& branch_arg,
            unsigned char child_idx_arg)
        {
          BranchNode* new_branch_child = new BranchNode();

          branch_arg.setChildPtr (buffer_selector_, child_idx_arg,
              static_cast<OctreeNode*> (new_branch_child));

          return new_branch_child;
        }

        inline LeafNode*
        createLeafChild (BranchNode& branch_arg, unsigned char child_idx_arg)
        {
          LeafNode* new_leaf_child = new LeafNode();

          branch_arg.setChildPtr(buffer_selector_, child_idx_arg, new_leaf_child);

          return new_leaf_child;
        }

        // Recursive octree methods

        unsigned int
        createLeafRecursive (const OctreeKey& key_arg,
                             unsigned int depth_mask_arg,
                             BranchNode* branch_arg,
                             LeafNode*& return_leaf_arg,
                             BranchNode*& parent_of_leaf_arg,
                             bool branch_reset_arg = false);


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


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

        void
        serializeTreeRecursive (BranchNode* branch_arg,
                                OctreeKey& key_arg,
                                std::vector<char>* binary_tree_out_arg,
                                typename std::vector<LeafContainerT*>* leaf_container_vector_arg,
                                bool do_XOR_encoding_arg = false,
                                bool new_leafs_filter_arg = false);

        void
        deserializeTreeRecursive (BranchNode* branch_arg,
                                  unsigned int depth_mask_arg,
                                  OctreeKey& key_arg,
                                  typename std::vector<char>::const_iterator& binary_tree_in_it_arg,
                                  typename std::vector<char>::const_iterator& binary_tree_in_it_end_arg,
                                  typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_arg,
                                  typename std::vector<LeafContainerT*>::const_iterator* leaf_container_vector_it_end_arg,
                                  bool branch_reset_arg = false,
                                  bool do_XOR_decoding_arg = false);


        // Serialization callbacks

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

        }

        virtual void deserializeTreeCallback (LeafContainerT&, 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 leaf_count_;

        std::size_t branch_count_;

        BranchNode* root_node_;

        unsigned int depth_mask_;

        OctreeKey max_key_;

        unsigned char buffer_selector_;

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

        unsigned int octree_depth_;

        bool dynamic_depth_enabled_;

    };
  }
}

//#include "impl/octree2buf_base.hpp"

#endif