include/octomap/OcTreeBaseImpl.h

Go to the documentation of this file.

#ifndef OCTOMAP_OCTREE_BASE_IMPL_H
#define OCTOMAP_OCTREE_BASE_IMPL_H

// $Id: OcTreeBaseImpl.h 420 2012-08-27 14:10:25Z ahornung $

/*
 * Copyright (c) 2009-2011, 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.
 */

#include <list>
#include <limits>
#include <iterator>
#include <stack>


#include "octomap_types.h"
#include "OcTreeKey.h"
#include "ScanGraph.h"


namespace octomap {


  template <class NODE,class INTERFACE>
  class OcTreeBaseImpl : public INTERFACE {

  public:
    typedef NODE NodeType;
    
    OcTreeBaseImpl(double resolution);
    virtual ~OcTreeBaseImpl();

    OcTreeBaseImpl(const OcTreeBaseImpl<NODE,INTERFACE>& rhs);

    bool operator== (const OcTreeBaseImpl<NODE,INTERFACE>& rhs) const;

    std::string getTreeType() const {return "OcTreeBaseImpl";}

    void setResolution(double r);
    inline double getResolution() const { return resolution; }

    inline unsigned int getTreeDepth () const { return tree_depth; }

    inline double getNodeSize(unsigned depth) const {assert(depth <= tree_depth); return sizeLookupTable[depth];}

    inline NODE* getRoot() const { return root; }

    NODE* search(float x, float y, float z, unsigned int depth = 0) const;

    NODE* search(const point3d& value, unsigned int depth = 0) const;

    NODE* search(const OcTreeKey& key, unsigned int depth = 0) const;

    bool deleteNode(float x, float y, float z, unsigned int depth = 0);

    bool deleteNode(const point3d& value, unsigned int depth = 0);

    bool deleteNode(const OcTreeKey& key, unsigned int depth = 0);

    void clear();

    OcTreeBaseImpl deepCopy() const;


    virtual void prune();

    virtual void expand();

    // -- statistics  ----------------------

    virtual inline size_t size() const { return tree_size; }

    virtual size_t memoryUsage() const;

    virtual inline size_t memoryUsageNode() const {return sizeof(NODE); };

    size_t memoryFullGrid();

    double volume();

    virtual void getMetricSize(double& x, double& y, double& z);
    virtual void getMetricMin(double& x, double& y, double& z);
    void getMetricMin(double& x, double& y, double& z) const;
    virtual void getMetricMax(double& x, double& y, double& z);
    void getMetricMax(double& x, double& y, double& z) const;

    size_t calcNumNodes() const;

    size_t getNumLeafNodes() const;


    // -- access tree nodes  ------------------

    void getUnknownLeafCenters(point3d_list& node_centers, point3d pmin, point3d pmax) const;


    // -- raytracing  -----------------------

    bool computeRayKeys(const point3d& origin, const point3d& end, KeyRay& ray) const;


    bool computeRay(const point3d& origin, const point3d& end, std::vector<point3d>& ray);


    // file IO

    std::istream& readData(std::istream &s);

    std::ostream& writeData(std::ostream &s) const;

    class leaf_iterator;
    class tree_iterator;
    class leaf_bbx_iterator;
    typedef leaf_iterator iterator;

    iterator begin(unsigned char maxDepth=0) const {return iterator(this, maxDepth);};
    const iterator end() const {return leaf_iterator_end;}; // TODO: RVE?

    leaf_iterator begin_leafs(unsigned char maxDepth=0) const {return leaf_iterator(this, maxDepth);};
    const leaf_iterator end_leafs() const {return leaf_iterator_end;}

    leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey& min, const OcTreeKey& max, unsigned char maxDepth=0) const {
      return leaf_bbx_iterator(this, min, max, maxDepth);
    }
    leaf_bbx_iterator begin_leafs_bbx(const point3d& min, const point3d& max, unsigned char maxDepth=0) const {
      return leaf_bbx_iterator(this, min, max, maxDepth);
    }
    const leaf_bbx_iterator end_leafs_bbx() const {return leaf_iterator_bbx_end;}

    tree_iterator begin_tree(unsigned char maxDepth=0) const {return tree_iterator(this, maxDepth);}
    const tree_iterator end_tree() const {return tree_iterator_end;}

    //
    // Key / coordinate conversion functions
    //

    inline unsigned short int coordToKey(double coordinate) const{
      return ((int) floor(resolution_factor * coordinate)) + tree_max_val;
    }

    unsigned short int coordToKey(double coordinate, unsigned depth) const;


    inline OcTreeKey coordToKey(const point3d& coord) const{
      return OcTreeKey(coordToKey(coord(0)), coordToKey(coord(1)), coordToKey(coord(2)));
    }

    inline OcTreeKey coordToKey(const point3d& coord, unsigned depth) const{
      if (depth == tree_depth)
        return coordToKey(coord);
      else
        return OcTreeKey(coordToKey(coord(0), depth), coordToKey(coord(1), depth), coordToKey(coord(2), depth));
    }

    inline OcTreeKey adjustKeyAtDepth(const OcTreeKey& key, unsigned int depth) const{
      if (depth == tree_depth)
        return key;

      assert(depth <= tree_depth);
      return OcTreeKey(adjustKeyAtDepth(key[0], depth), adjustKeyAtDepth(key[1], depth), adjustKeyAtDepth(key[2], depth));
    }

    unsigned short int adjustKeyAtDepth(unsigned short int key, unsigned int depth) const;

    bool coordToKeyChecked(const point3d& coord, OcTreeKey& key) const;

    bool coordToKeyChecked(const point3d& coord, unsigned depth, OcTreeKey& key) const;

    bool coordToKeyChecked(double coordinate, unsigned short int& key) const;

    bool coordToKeyChecked(double coordinate, unsigned depth, unsigned short int& key) const;

    double keyToCoord(unsigned short int key, unsigned depth) const;

    inline double keyToCoord(unsigned short int key) const{
      return (double( (int) key - (int) this->tree_max_val ) +0.5) * this->resolution;
    }

    inline point3d keyToCoord(const OcTreeKey& key) const{
      return point3d(float(keyToCoord(key[0])), float(keyToCoord(key[1])), float(keyToCoord(key[2])));
    }

    inline point3d keyToCoord(const OcTreeKey& key, unsigned depth) const{
      return point3d(float(keyToCoord(key[0], depth)), float(keyToCoord(key[1], depth)), float(keyToCoord(key[2], depth)));
    }

    DEPRECATED( bool genKeyValue(double coordinate, unsigned short int& keyval) const) {
      return coordToKeyChecked(coordinate, keyval);
    }

    DEPRECATED( bool genKey(const point3d& point, OcTreeKey& key) const ) {
      return coordToKeyChecked(point, key);
    }

    DEPRECATED( bool genKeyValueAtDepth(const unsigned short int keyval, unsigned int depth, unsigned short int &out_keyval) const );

    DEPRECATED( bool genKeyAtDepth(const OcTreeKey& key, unsigned int depth, OcTreeKey& out_key) const );

    DEPRECATED( bool genCoordFromKey(const unsigned short int& key, unsigned depth, float& coord) const ){
      coord = float(keyToCoord(key, depth));
      return true;
    }

    DEPRECATED( inline bool genCoordFromKey(const unsigned short int& key, float& coord, unsigned depth) const) {
      coord = float(keyToCoord(key, depth));
      return true;
    }

    DEPRECATED( inline bool genCoordFromKey(const unsigned short int& key, float& coord) const) {
      coord = float(keyToCoord(key));
      return true;
    }

    DEPRECATED( double genCoordFromKey(const unsigned short int& key, unsigned depth) const) {
      return keyToCoord(key, depth);
    }

    DEPRECATED( inline double genCoordFromKey(const unsigned short int& key) const) {
      return keyToCoord(key);
    }

    DEPRECATED( inline bool genCoords(const OcTreeKey& key, unsigned int depth, point3d& point) const){
      point = keyToCoord(key, depth);
      return true;
    }

    DEPRECATED( inline void genPos(const OcTreeKey& key, int depth, unsigned int& pos) const) {
      pos = computeChildIdx(key, depth);
    }

 protected:
    OcTreeBaseImpl(double resolution, unsigned int tree_depth, unsigned int tree_max_val);

    void calcMinMax();

    void calcNumNodesRecurs(NODE* node, size_t& num_nodes) const;

    bool deleteNodeRecurs(NODE* node, unsigned int depth, unsigned int max_depth, const OcTreeKey& key);

    void pruneRecurs(NODE* node, unsigned int depth, unsigned int max_depth, unsigned int& num_pruned);

    void expandRecurs(NODE* node, unsigned int depth, unsigned int max_depth);
    
    size_t getNumLeafNodesRecurs(const NODE* parent) const;

 private:
    OcTreeBaseImpl<NODE,INTERFACE>& operator=(const OcTreeBaseImpl<NODE,INTERFACE>&);

  protected:

    NODE* root;

    // constants of the tree
    const unsigned int tree_depth; 
    const unsigned int tree_max_val;
    double resolution;  
    double resolution_factor; 
  
    size_t tree_size; 

    bool size_changed;

    point3d tree_center;  // coordinate offset of tree

    double max_value[3]; 
    double min_value[3]; 

    std::vector<double> sizeLookupTable;

    KeyRay keyray;  // data structure for ray casting

    const leaf_iterator leaf_iterator_end;
    const leaf_bbx_iterator leaf_iterator_bbx_end;
    const tree_iterator tree_iterator_end;

  public:
    // the actual iterator implementation within this class:
    #include <octomap/OcTreeIterator.hxx>
  };

}

#include <octomap/OcTreeBaseImpl.hxx>

#endif