Program Listing for File OcTreeIterator.hxx

↰ Return to documentation for file (include/octomap/OcTreeIterator.hxx)

/*
 * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
 * https://octomap.github.io/
 *
 * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
 * All rights reserved.
 * License: New BSD
 *
 * 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_

    template<class NodeType>
    class iterator_base{
    public:
      using iterator_category = std::forward_iterator_tag;
      using value_type = NodeType;
      using difference_type = NodeType;
      using pointer = NodeType*;
      using reference = NodeType&;

      struct StackElement;
      iterator_base() : tree(NULL), maxDepth(0){}

      iterator_base(OcTreeBaseImpl<NodeType,INTERFACE> const* ptree, uint8_t depth=0)
        : tree((ptree && ptree->root) ? ptree : NULL), maxDepth(depth)
      {
        if (ptree && maxDepth == 0)
          maxDepth = ptree->getTreeDepth();

        if (tree && tree->root){ // tree is not empty
          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);
        } else{ // construct the same as "end"
          tree = NULL;
          this->maxDepth = 0;
        }
      }

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


    protected:
      OcTreeBaseImpl<NodeType,INTERFACE> const* tree;
      uint8_t maxDepth;

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

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

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

        key_type center_offset_key = tree->tree_max_val >> s.depth;
        // push on stack in reverse order
        for (int i=7; i>=0; --i) {
          if (tree->nodeChildExists(top.node,i)) {
            computeChildKey(i, center_offset_key, top.key, s.key);
            s.node = tree->getNodeChild(top.node, 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);
          }
        }
      }

    };

    class tree_iterator : public iterator_base<NodeType> {
    public:
      tree_iterator() : iterator_base<NodeType>(){}
      tree_iterator(OcTreeBaseImpl<NodeType,INTERFACE> const* ptree, uint8_t depth=0) : iterator_base<NodeType>(ptree, 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->tree->nodeHasChildren(this->stack.top().node) || this->stack.top().depth == this->maxDepth);
      }
    };

    class leaf_iterator : public iterator_base<NodeType> {
      public:
          leaf_iterator() : iterator_base<NodeType>(){}

          leaf_iterator(OcTreeBaseImpl<NodeType, INTERFACE> const* ptree, uint8_t depth=0) : iterator_base<NodeType>(ptree, depth) {
            // tree could be empty (= no stack)
            if (this->stack.size() > 0){
              // 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<NodeType>(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->tree->nodeHasChildren(this->stack.top().node))
              {
                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<NodeType> {
    public:
      leaf_bbx_iterator() : iterator_base<NodeType>() {}
      leaf_bbx_iterator(OcTreeBaseImpl<NodeType,INTERFACE> const* ptree, const point3d& min, const point3d& max, uint8_t depth=0)
        : iterator_base<NodeType>(ptree, depth)
      {
        if (this->stack.size() > 0){
          assert(ptree);
          if (!this->tree->coordToKeyChecked(min, minKey) || !this->tree->coordToKeyChecked(max, maxKey)){
            // coordinates invalid, set to end iterator
            this->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* ptree, const OcTreeKey& min, const OcTreeKey& max, uint8_t depth=0)
        : iterator_base<NodeType>(ptree, depth), minKey(min), maxKey(max)
      {
        // tree could be empty (= no stack)
        if (this->stack.size() > 0){
          // 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<NodeType>(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->tree->nodeHasChildren(this->stack.top().node))
          {
            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<NodeType>::StackElement top = this->stack.top();
        this->stack.pop();

        typename iterator_base<NodeType>::StackElement s;
        s.depth = top.depth +1;
        key_type center_offset_key = this->tree->tree_max_val >> s.depth;
        // push on stack in reverse order
        for (int i=7; i>=0; --i) {
          if (this->tree->nodeChildExists(top.node, 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 = this->tree->getNodeChild(top.node, i);
              this->stack.push(s);
              assert(s.depth <= this->maxDepth);
            }
          }
        }
      }


      OcTreeKey minKey;
      OcTreeKey maxKey;
    };


#endif /* OCTREEITERATOR_HXX_ */