1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
    AbstractOcTree();
    virtual ~AbstractOcTree() {};

    /// returns actual class name as string for identification
    virtual std::string getTreeType() const = 0;



    virtual double getResolution() const = 0;
    virtual void setResolution(double res) = 0;
    virtual size_t size() const = 0;
    virtual size_t memoryUsage() const = 0;
    virtual size_t memoryUsageNode() const = 0;
    virtual void getMetricMin(double& x, double& y, double& z) = 0;
    virtual void getMetricMin(double& x, double& y, double& z) const = 0;
    virtual void getMetricMax(double& x, double& y, double& z) = 0;
    virtual void getMetricMax(double& x, double& y, double& z) const = 0;
    virtual void getMetricSize(double& x, double& y, double& z) = 0;

    virtual void prune() = 0;
    virtual void expand() = 0;
    virtual void clear() = 0;

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
    /// Write file header and complete tree to file (serialization)
    bool write(const std::string& filename) const;   

    /**
     * Read the file header, create the appropriate class and deserialize.
     * This creates a new octree which you need to delete yourself. If you
     * expect or requre a specific kind of octree, use dynamic_cast afterwards:
     * @code
     * AbstractOcTree* tree = AbstractOcTree::read("filename.ot");
     * OcTree* octree = dynamic_cast<OcTree*>(tree);
     *
     * @endcode
     */
    static AbstractOcTree* read(const std::string& filename);

    /**
     * Read all nodes from the input stream (without file header),
     * for this the tree needs to be already created.
     * For general file IO, you
     * should probably use AbstractOcTree::read() instead.
     */
    virtual std::istream& readData(std::istream &s) = 0;

    /// Write complete state of tree to stream (without file header) unmodified.
    /// Pruning the tree first produces smaller files (lossless compression)
    virtual std::ostream& writeData(std::ostream &s) const = 0;

1
2
3
4
5
6
7
8
9
10
11
  bool AbstractOcTree::write(std::ostream &s) const{
    s << fileHeader <<"\n# (feel free to add / change comments, but leave the first line as it is!)\n#\n";
    s << "id " << getTreeType() << std::endl;
    s << "size "<< size() << std::endl;
    s << "res " << getResolution() << std::endl;
    s << "data" << std::endl;

    // write the actual data:
    writeData(s);
    return true;
  }

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
  AbstractOcTree* AbstractOcTree::read(std::istream &s){

    // check if first line valid:
    std::string line;
    std::getline(s, line);
    if (line.compare(0,fileHeader.length(), fileHeader) !=0){
      OCTOMAP_ERROR_STR("First line of OcTree file header does not start with \""<< fileHeader);
      return NULL;
    }

    std::string id;
    unsigned size;
    double res;
    if (!AbstractOcTree::readHeader(s, id, size, res))
      return NULL;


    // otherwise: values are valid, stream is now at binary data!
    OCTOMAP_DEBUG_STR("Reading octree type "<< id);

    AbstractOcTree* tree = createTree(id, res);

    if (tree){
      if (size > 0)
        tree->readData(s);

      OCTOMAP_DEBUG_STR("Done ("<< tree->size() << " nodes)");
    }

    return tree;
  }

1
2
3
4
5
6
7
8
9
10
11
12
  AbstractOcTree* AbstractOcTree::createTree(const std::string class_name, double res){
    std::map<std::string, AbstractOcTree*>::iterator it = classIDMapping().find(class_name);
    if (it == classIDMapping().end()){
      OCTOMAP_ERROR("Could not create octree of type %s, not in store in classIDMapping\n", class_name.c_str());
      return NULL;
    } else {
      AbstractOcTree* tree = it->second->create();

      tree->setResolution(res);
      return tree;
    }
  }

1
2
3
4
5
6
7
  private:
    /// create private store, Construct on first use
    static std::map<std::string, AbstractOcTree*>& classIDMapping();

  protected:
    static void registerTreeType(AbstractOcTree* tree);

1
2
3
4
5
6
7
8
9
10
11
  std::map<std::string, AbstractOcTree*>& AbstractOcTree::classIDMapping(){
    // we will "leak" the memory of the map and all trees until program exits,
    // but this ensures all static objects are there as long as needed
    // http://www.parashift.com/c++-faq-lite/ctors.html#faq-10.15
    static std::map<std::string, AbstractOcTree*>* map = new std::map<std::string, AbstractOcTree*>();
    return *map;
  }

  void AbstractOcTree::registerTreeType(AbstractOcTree* tree){
    classIDMapping()[tree->getTreeType()] = tree;
  }

1
2
3
4
  /* This tree implementation currently has a maximum depth of 16
   * nodes. For this reason, coordinates values have to be, e.g.,
   * below +/- 327.68 meters (2^15) at a maximum resolution of 0.01m.
   */

1
2
3
    NODE * octomap::OcTreeBaseImpl< NODE, I >::search	(	const point3d & 	value,
    unsigned int 	depth = 0 
    )		const

1
2
3
4
5
6
7
8
9
10
11
12
13
14
    // file IO

    /**
     * Read all nodes from the input stream (without file header),
     * for this the tree needs to be already created.
     * For general file IO, you
     * should probably use AbstractOcTree::read() instead.
     */
    std::istream& readData(std::istream &s);

    /// Write complete state of tree to stream (without file header) unmodified.
    /// Pruning the tree first produces smaller files (lossless compression)
    std::ostream& writeData(std::ostream &s) const;

1
2
3
4
5
6
    /// recursive call of readData()
    std::istream& readNodesRecurs(NODE*, std::istream &s);
    
    /// recursive call of writeData()
    std::ostream& writeNodesRecurs(const NODE*, std::ostream &s) const;
    

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
  
  template <class NODE,class I>
  std::ostream& OcTreeBaseImpl<NODE,I>::writeData(std::ostream &s) const{
    if (root)
      writeNodesRecurs(root, s);

    return s;
  }
    
  template <class NODE,class I>
  std::ostream& OcTreeBaseImpl<NODE,I>::writeNodesRecurs(const NODE* node, std::ostream &s) const{
    node->writeData(s);
    
    // 1 bit for each children; 0: empty, 1: allocated
    std::bitset<8> children;
    for (unsigned int i=0; i<8; i++) {
      if (nodeChildExists(node, i))
        children[i] = 1;
      else
        children[i] = 0;
    }

    char children_char = (char) children.to_ulong();
    s.write((char*)&children_char, sizeof(char));

//     std::cout << "wrote: " << value << " "
//               << children.to_string<char,std::char_traits<char>,std::allocator<char> >() 
//               << std::endl;

    // recursively write children
    for (unsigned int i=0; i<8; i++) {
      if (children[i] == 1) {
        this->writeNodesRecurs(getNodeChild(node, i), s);
      }
    }
    
    return s;
  }


  template <class NODE,class I>
  std::istream& OcTreeBaseImpl<NODE,I>::readData(std::istream &s) {

    if (!s.good()){
      OCTOMAP_WARNING_STR(__FILE__ << ":" << __LINE__ << "Warning: Input filestream not \"good\"");
    }

    this->tree_size = 0;
    size_changed = true;

    // tree needs to be newly created or cleared externally
    if (root) {
      OCTOMAP_ERROR_STR("Trying to read into an existing tree.");
      return s;
    }

    root = new NODE();
    readNodesRecurs(root, s);
    
    tree_size = calcNumNodes();  // compute number of nodes
    return s;
  }
  

  template <class NODE,class I>
  std::istream& OcTreeBaseImpl<NODE,I>::readNodesRecurs(NODE* node, std::istream &s) {
    
    node->readData(s);
    
    char children_char;
    s.read((char*)&children_char, sizeof(char));
    std::bitset<8> children ((unsigned long long) children_char);

    //std::cout << "read: " << node->getValue() << " "
    //            << children.to_string<char,std::char_traits<char>,std::allocator<char> >()
    //            << std::endl;

    for (unsigned int i=0; i<8; i++) {
      if (children[i] == 1){
        NODE* newNode = createNodeChild(node, i);
        readNodesRecurs(newNode, s);
      }
    }
    
    return s;
  }

1
2
3
4
5
6
7
8
9
10
11
12
13
   /**
    * Traces a ray from origin to end (excluding), returning an
    * OcTreeKey of all nodes traversed by the beam. You still need to check
    * if a node at that coordinate exists (e.g. with search()).
    *
    * @param origin start coordinate of ray
    * @param end end coordinate of ray
    * @param ray KeyRay structure that holds the keys of all nodes traversed by the ray, excluding "end"
    * @return Success of operation. Returning false usually means that one of the coordinates is out of the OcTree's range
    */
    bool computeRayKeys(const point3d& origin, const point3d& end, KeyRay& ray) const;

1
2
3
4
5
6
7
8
9
10
11
12
   /**
    * Traces a ray from origin to end (excluding), returning the
    * coordinates of all nodes traversed by the beam. You still need to check
    * if a node at that coordinate exists (e.g. with search()).
    * @note: use the faster computeRayKeys method if possible.
    * 
    * @param origin start coordinate of ray
    * @param end end coordinate of ray
    * @param ray KeyRay structure that holds the keys of all nodes traversed by the ray, excluding "end"
    * @return Success of operation. Returning false usually means that one of the coordinates is out of the OcTree's range
    */
    bool computeRay(const point3d& origin, const point3d& end, std::vector<point3d>& ray);

1
2
3
4
/// queries whether a node is occupied according to the tree's parameter for "occupancy"
inline bool isNodeOccupied(const OcTreeNode* occupancyNode) const{
  return (occupancyNode->getLogOdds() >= this->occ_prob_thres_log);
}

1
2
3
4
5
    virtual OcTreeNode* octomap::AbstractOccupancyOcTree::updateNode	(	const point3d & 	value,
    bool 	occupied,
    bool 	lazy_eval = false 
    )		
    pure virtual

1
2
3
4
5
6
7
template <class NODE>
NODE* OccupancyOcTreeBase<NODE>::updateNode(const point3d& value, bool occupied, bool lazy_eval) {
	OcTreeKey key;
    if (!this->coordToKeyChecked(value, key))
	    return NULL;
	return updateNode(key, occupied, lazy_eval);
}

1
2
3
4
5
6
setOccupancyThres(0.5);   // = 0.0 in logodds  
setProbHit(0.7);          // = 0.85 in logodds  
setProbMiss(0.4);         // = -0.4 in logodds  
  
setClampingThresMin(0.1192); // = -2 in log odds  
setClampingThresMax(0.971); // = 3.5 in log odds

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
     //-- parameters for occupancy and sensor model:
     
    /// sets the threshold for occupancy (sensor model)
    void setOccupancyThres(double prob){occ_prob_thres_log = logodds(prob); }
    /// sets the probability for a "hit" (will be converted to logodds) - sensor model
    void setProbHit(double prob){prob_hit_log = logodds(prob); assert(prob_hit_log >= 0.0);}
    /// sets the probability for a "miss" (will be converted to logodds) - sensor model
    void setProbMiss(double prob){prob_miss_log = logodds(prob); assert(prob_miss_log <= 0.0);}
    /// sets the minimum threshold for occupancy clamping (sensor model)
    void setClampingThresMin(double thresProb){clamping_thres_min = logodds(thresProb); }
    /// sets the maximum threshold for occupancy clamping (sensor model)
    void setClampingThresMax(double thresProb){clamping_thres_max = logodds(thresProb); }

    /// @return threshold (probability) for occupancy - sensor model
    double getOccupancyThres() const {return probability(occ_prob_thres_log); }
    /// @return threshold (logodds) for occupancy - sensor model
    float getOccupancyThresLog() const {return occ_prob_thres_log; }

    /// @return probability for a "hit" in the sensor model (probability)
    double getProbHit() const {return probability(prob_hit_log); }
    /// @return probability for a "hit" in the sensor model (logodds)
    float getProbHitLog() const {return prob_hit_log; }
    /// @return probability for a "miss"  in the sensor model (probability)
    double getProbMiss() const {return probability(prob_miss_log); }
    /// @return probability for a "miss"  in the sensor model (logodds)
    float getProbMissLog() const {return prob_miss_log; }

    /// @return minimum threshold for occupancy clamping in the sensor model (probability)
    double getClampingThresMin() const {return probability(clamping_thres_min); }
    /// @return minimum threshold for occupancy clamping in the sensor model (logodds)
    float getClampingThresMinLog() const {return clamping_thres_min; }
    /// @return maximum threshold for occupancy clamping in the sensor model (probability)
    double getClampingThresMax() const {return probability(clamping_thres_max); }
    /// @return maximum threshold for occupancy clamping in the sensor model (logodds)
    float getClampingThresMaxLog() const {return clamping_thres_max; }

1
2
3
4
5
6
    virtual bool octomap::OccupancyOcTreeBase< OcTreeNode >::castRay(const point3d & origin,
                                                                   const point3d & direction,
                                                                   point3d & end,
                                                                   bool ignoreUnknownCells = false,
                                                                   double maxRange = -1.0 
                                                                   ) const

1
2
3
4
5
6
    bool octomap::OccupancyOcTreeBase< NODE >::getRayIntersection	(	const point3d & 	origin,
    const point3d & 	direction,
    const point3d & 	center,
    point3d & 	intersection,
    double 	delta = 0.0 
    )		const

1
2
3
4
5
6
7
8
9
10
11
    /// integrate a "hit" measurement according to the tree's sensor model
    virtual void integrateHit(NODE* occupancyNode) const;
    /// integrate a "miss" measurement according to the tree's sensor model
    virtual void integrateMiss(NODE* occupancyNode) const;
    /// update logodds value of node by adding to the current value.
    virtual void updateNodeLogOdds(NODE* occupancyNode, const float& update) const;

    /// converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"
    virtual void nodeToMaxLikelihood(NODE* occupancyNode) const;
    /// converts the node to the maximum likelihood value according to the tree's parameter for "occupancy"
    virtual void nodeToMaxLikelihood(NODE& occupancyNode) const;

1
2
3
4
5
6
7
8
  /**
   * octomap main map data structure, stores 3D occupancy grid map in an OcTree.
   * Basic functionality is implemented in OcTreeBase.
   *
   */
  class OcTree : public OccupancyOcTreeBase <OcTreeNode> {
      
  };

1
    tree.integrateNodeColor(x, y, z, r, g, b);

1
2
3
  class AbstractOcTreeNode {

  };

1
2
3
4
5
6
7
8
9
  octomap::OcTree* tree = new octomap::OcTree(1.0);  

  unsigned int maxDepth = tree->getTreeDepth();
  std::vector<octomap::OcTreeNode*> collapsed_occ_nodes;
  for (octomap::OcTree::iterator it = tree->begin(); it != tree->end(); ++it) {
    if (tree->isNodeOccupied(*it) && it.getDepth() < maxDepth) {
      collapsed_occ_nodes.push_back(&(*it));
    }
  }

1
2
3
4
5
6
      std::cerr << "The log file needs to be in the format of:\n"
          << "NODE x y z roll pitch yaw\n"
          << "x y z\nx y z\n...\n"
          << "NODE x y z roll pitch yaw\n"
          << "x y z\n...\n\n"
          << "Lines starting with '#' or empty lines are ignored.\n\n";