include/octomap/OcTreeIterator.hxx

Go to the documentation of this file.

// $Id: OcTreeIterator.hxx 420 2012-08-27 14:10:25Z ahornung $

/*
 * OctoMap:
 * A probabilistic, flexible, and compact 3D mapping library for robotic systems.
 * @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009.
 * @see http://octomap.sourceforge.net/
 * License: New BSD License
 */

/*
 * Copyright (c) 2009, K. M. Wurm, A. Hornung, University of Freiburg
 * 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 the University of Freiburg 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.
 */

#ifndef OCTOMAP_OCTREEITERATOR_HXX_
#define OCTOMAP_OCTREEITERATOR_HXX_

    class iterator_base : public std::iterator<std::forward_iterator_tag, NodeType>{
    public:
      struct StackElement;
      iterator_base() : tree(NULL), maxDepth(0){}

      iterator_base(OcTreeBaseImpl<NodeType,INTERFACE> const* tree, unsigned char depth=0)
        : tree(tree), maxDepth(depth)
      {
        if (maxDepth == 0)
          maxDepth = tree->getTreeDepth();

        StackElement s;
        s.node = tree->root;
        s.depth = 0;
        s.key[0] = s.key[1] = s.key[2] = tree->tree_max_val;
        stack.push(s);
      }

      iterator_base(const iterator_base& other)
      : tree(other.tree), maxDepth(other.maxDepth), stack(other.stack) {}

      bool operator==(const iterator_base& other) const {
        return (tree ==other.tree && stack.size() == other.stack.size()
            && (stack.size()==0 || (stack.size() > 0 && (stack.top().node == other.stack.top().node
                && stack.top().depth == other.stack.top().depth
                && stack.top().key == other.stack.top().key ))));
      }

      bool operator!=(const iterator_base& other) const {
        return (tree !=other.tree || stack.size() != other.stack.size()
            || (stack.size() > 0 && ((stack.top().node != other.stack.top().node
                || stack.top().depth != other.stack.top().depth
                || stack.top().key != other.stack.top().key ))));
      }

      iterator_base& operator=(const iterator_base& other){
        tree = other.tree;
        maxDepth = other.maxDepth;
        stack = other.stack;
        return *this;
      };

      NodeType const* operator->() const { return stack.top().node;}

      NodeType* operator->() { return stack.top().node;}

      const NodeType& operator*() const { return *(stack.top().node);}

      NodeType& operator*() { return *(stack.top().node);}

      point3d getCoordinate() const {
        return tree->keyToCoord(stack.top().key, stack.top().depth);
      }

      double getX() const{
        return tree->keyToCoord(stack.top().key[0], stack.top().depth);
      }
      double getY() const{
        return tree->keyToCoord(stack.top().key[1], stack.top().depth);
      }
      double getZ() const{
        return tree->keyToCoord(stack.top().key[2], stack.top().depth);
      }

      double getSize() const {return  tree->getNodeSize(stack.top().depth); }

      unsigned getDepth() const {return unsigned(stack.top().depth); }

      const OcTreeKey& getKey() const {return stack.top().key;}

      OcTreeKey getIndexKey() const {
        return computeIndexKey(tree->getTreeDepth() - stack.top().depth, stack.top().key);
      }


      struct StackElement{
        NodeType* node;
        OcTreeKey key;
        unsigned char depth;
      };


    protected:
      void singleIncrement(){
        StackElement top = stack.top();
        stack.pop();
        if (top.depth == maxDepth)
          return;

        StackElement s;
        s.depth = top.depth +1;

        unsigned short int center_offset_key = tree->tree_max_val >> (top.depth +1);
        // push on stack in reverse order
        for (int i=7; i>=0; --i) {
          if (top.node->childExists(i)) {
            computeChildKey(i, center_offset_key, top.key, s.key);
            s.node = top.node->getChild(i);
            //OCTOMAP_DEBUG_STR("Current depth: " << int(top.depth) << " new: "<< int(s.depth) << " child#" << i <<" ptr: "<<s.node);
            stack.push(s);
            assert(s.depth <= maxDepth);
          }
        }
      }

      OcTreeBaseImpl<NodeType,INTERFACE> const* tree; 
      unsigned char maxDepth; 

      std::stack<StackElement,std::vector<StackElement> > stack;

    };

    class tree_iterator : public iterator_base {
    public:
      tree_iterator() : iterator_base(){}
      tree_iterator(OcTreeBaseImpl<NodeType,INTERFACE> const* tree, unsigned char depth=0) : iterator_base(tree, depth) {};

      tree_iterator operator++(int){
        tree_iterator result = *this;
        ++(*this);
        return result;
      }

      tree_iterator& operator++(){

        if (!this->stack.empty()){
          this->singleIncrement();
        }

        if (this->stack.empty()){
          this->tree = NULL;
        }

        return *this;
      }

      bool isLeaf() const{ return (!this->stack.top().node->hasChildren() || this->stack.top().depth == this->maxDepth); }
    };

    class leaf_iterator : public iterator_base {
      public:
          leaf_iterator() : iterator_base(){}

          leaf_iterator(OcTreeBaseImpl<NodeType, INTERFACE> const* tree, unsigned char depth=0) : iterator_base(tree, depth) {
            // skip forward to next valid leaf node:
            // add root another time (one will be removed) and ++
            this->stack.push(this->stack.top());
            operator ++();
          }

          leaf_iterator(const leaf_iterator& other) : iterator_base(other) {};

          leaf_iterator operator++(int){
            leaf_iterator result = *this;
            ++(*this);
            return result;
          }

          leaf_iterator& operator++(){
            if (this->stack.empty()){
              this->tree = NULL; // TODO check?

            } else {
              this->stack.pop();

              // skip forward to next leaf
              while(!this->stack.empty()  && this->stack.top().depth < this->maxDepth
                  && this->stack.top().node->hasChildren())
              {
                this->singleIncrement();
              }
              // done: either stack is empty (== end iterator) or a next leaf node is reached!
              if (this->stack.empty())
                this->tree = NULL;
            }


            return *this;
          }

    };

    class leaf_bbx_iterator : public iterator_base {
    public:
      leaf_bbx_iterator() : iterator_base() {};
      leaf_bbx_iterator(OcTreeBaseImpl<NodeType,INTERFACE> const* tree, const point3d& min, const point3d& max, unsigned char depth=0)
        : iterator_base(tree, depth)
      {

        if (!this->tree->coordToKeyChecked(min, minKey) || !this->tree->coordToKeyChecked(max, maxKey)){
          // coordinates invalid, set to end iterator
          tree = NULL;
          this->maxDepth = 0;
        } else{  // else: keys are generated and stored

          // advance from root to next valid leaf in bbx:
          this->stack.push(this->stack.top());
          this->operator ++();
        }

      }

      leaf_bbx_iterator(OcTreeBaseImpl<NodeType,INTERFACE> const* tree, const OcTreeKey& min, const OcTreeKey& max, unsigned char depth=0)
        : iterator_base(tree, depth), minKey(min), maxKey(max)
      {
          // advance from root to next valid leaf in bbx:
          this->stack.push(this->stack.top());
          this->operator ++();
      }

      leaf_bbx_iterator(const leaf_bbx_iterator& other) : iterator_base(other) {
        minKey = other.minKey;
        maxKey = other.maxKey;
      }



      leaf_bbx_iterator operator++(int){
        leaf_bbx_iterator result = *this;
        ++(*this);
        return result;
      }

      leaf_bbx_iterator& operator++(){
        if (this->stack.empty()){
          this->tree = NULL; // TODO check?

        } else {
          this->stack.pop();

          // skip forward to next leaf
          while(!this->stack.empty()  && this->stack.top().depth < this->maxDepth
              && this->stack.top().node->hasChildren())
          {
            this->singleIncrement();
          }
          // done: either stack is empty (== end iterator) or a next leaf node is reached!
          if (this->stack.empty())
            this->tree = NULL;
        }


        return *this;
      };

    protected:

      void singleIncrement(){
        typename iterator_base::StackElement top = this->stack.top();
        this->stack.pop();

        typename iterator_base::StackElement s;
        s.depth = top.depth +1;
        unsigned short int center_offset_key = this->tree->tree_max_val >> (top.depth +1);
        // push on stack in reverse order
        for (int i=7; i>=0; --i) {
          if (top.node->childExists(i)) {
            computeChildKey(i, center_offset_key, top.key, s.key);

            // overlap of query bbx and child bbx?
            if ((minKey[0] <= (s.key[0] + center_offset_key)) && (maxKey[0] >= (s.key[0] - center_offset_key))
                && (minKey[1] <= (s.key[1] + center_offset_key)) && (maxKey[1] >= (s.key[1] - center_offset_key))
                && (minKey[2] <= (s.key[2] + center_offset_key)) && (maxKey[2] >= (s.key[2] - center_offset_key)))
            {
              s.node = top.node->getChild(i);
              this->stack.push(s);
              assert(s.depth <= this->maxDepth);
            }
          }
        }
      }


      OcTreeKey minKey;
      OcTreeKey maxKey;
    };


#endif /* OCTREEITERATOR_HXX_ */