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

#ifndef PCL_OCTREE_TREE_BASE_H
#define PCL_OCTREE_TREE_BASE_H

#include <cstddef>
#include <vector>

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

namespace pcl
{
  namespace octree
  {
    template<typename LeafContainerT = int,
        typename BranchContainerT = OctreeContainerEmpty >
    class OctreeBase
    {

      public:

        typedef OctreeBase<LeafContainerT, BranchContainerT> OctreeT;

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

        typedef BranchContainerT BranchContainer;
        typedef LeafContainerT LeafContainer;

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

        // 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 max_depth_arg = 0) {return DepthFirstIterator(this, max_depth_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();};


        OctreeBase ();

        virtual
        ~OctreeBase ();

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

        OctreeBase&
        operator = (const OctreeBase &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_;
          octree_depth_ = source.octree_depth_;
          return (*this);
        }

        void
        setMaxVoxelIndex (unsigned int max_voxel_index_arg);

        void
        setTreeDepth (unsigned int max_depth_arg);

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

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

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

        void
        deleteTree ( );

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

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

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

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

        void
        deserializeTree (std::vector<char>& binary_tree_input_arg, std::vector<LeafContainerT*>& leaf_container_vector_arg);

      protected:
        
        // Protected octree methods based on octree keys

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

          LeafContainerT* ret = leaf_node->getContainerPtr();

          return ret;
        }

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

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

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

        // Branch node access functions

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

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

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

        void setBranchChildPtr (BranchNode& branch_arg,
                                unsigned char child_idx_arg,
                                OctreeNode* new_child_arg)
        {
          branch_arg[child_idx_arg] = new_child_arg;
        }

        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(i);
            node_bits |= static_cast<char> ((!!child) << i);
          }

          return (node_bits);
        }

        void
        deleteBranchChild (BranchNode& branch_arg, unsigned char child_idx_arg)
        {
          if (branch_arg.hasChild(child_idx_arg))
          {
            OctreeNode* branch_child = branch_arg[child_idx_arg];
            
            switch (branch_child->getNodeType ())
            {
              case BRANCH_NODE:
              {
                // free child branch recursively
                deleteBranch (*static_cast<BranchNode*> (branch_child));
                // delete branch node
                delete branch_child;
              }
                break;

              case LEAF_NODE:
              {
                // delete leaf node
                delete branch_child;
                break;
              }
              default:
                break;
            }

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

        void
        deleteBranch (BranchNode& branch_arg)
        {
          char i;

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

        BranchNode* createBranchChild (BranchNode& branch_arg,
                                       unsigned char child_idx_arg)
        {
          BranchNode* new_branch_child = new BranchNode();
          branch_arg[child_idx_arg] = static_cast<OctreeNode*> (new_branch_child);

          return new_branch_child;
        }

        LeafNode*
        createLeafChild (BranchNode& branch_arg, unsigned char child_idx_arg)
        {
          LeafNode* new_leaf_child = new LeafNode();
          branch_arg[child_idx_arg] = static_cast<OctreeNode*> (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);

        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 (const BranchNode* branch_arg,
                                OctreeKey& key_arg,
                                std::vector<char>* binary_tree_out_arg,
                                typename std::vector<LeafContainerT*>* leaf_container_vector_arg) const;

        void
        deserializeTreeRecursive (BranchNode* branch_arg, unsigned int depth_mask_arg, OctreeKey& key_arg,
                                  typename std::vector<char>::const_iterator& binary_tree_input_it_arg,
                                  typename std::vector<char>::const_iterator& binary_tree_input_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);


        // Serialization callbacks

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

        }

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

        }

        // Helpers

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

        bool
        octreeCanResize ()
        {
          return (true);
        }

        // Globals

        std::size_t leaf_count_;

        std::size_t branch_count_;

        BranchNode* root_node_;

        unsigned int depth_mask_;

        unsigned int octree_depth_;

        bool dynamic_depth_enabled_;

        OctreeKey max_key_;
    };
  }
}

//#include "impl/octree_base.hpp"

#endif