AbstractOccupancyOcTree.h
Go to the documentation of this file.
1 /*
2  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3  * http://octomap.github.com/
4  *
5  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
6  * All rights reserved.
7  * License: New BSD
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above copyright
15  * notice, this list of conditions and the following disclaimer in the
16  * documentation and/or other materials provided with the distribution.
17  * * Neither the name of the University of Freiburg nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 #ifndef OCTOMAP_ABSTRACT_OCCUPANCY_OCTREE_H
35 #define OCTOMAP_ABSTRACT_OCCUPANCY_OCTREE_H
36 
37 #include "AbstractOcTree.h"
38 #include "octomap_utils.h"
39 #include "OcTreeNode.h"
40 #include "OcTreeKey.h"
41 #include <cassert>
42 #include <fstream>
43 
44 
45 namespace octomap {
46 
53  public:
56 
57  //-- IO
58 
64  bool writeBinary(const std::string& filename);
65 
72  bool writeBinary(std::ostream &s);
73 
81  bool writeBinaryConst(const std::string& filename) const;
82 
89  bool writeBinaryConst(std::ostream &s) const;
90 
92  virtual std::ostream& writeBinaryData(std::ostream &s) const = 0;
93 
99  bool readBinary(std::istream &s);
100 
106  bool readBinary(const std::string& filename);
107 
109  virtual std::istream& readBinaryData(std::istream &s) = 0;
110 
111  // -- occupancy queries
112 
114  inline bool isNodeOccupied(const OcTreeNode* occupancyNode) const{
115  return (occupancyNode->getLogOdds() >= this->occ_prob_thres_log);
116  }
117 
119  inline bool isNodeOccupied(const OcTreeNode& occupancyNode) const{
120  return (occupancyNode.getLogOdds() >= this->occ_prob_thres_log);
121  }
122 
124  inline bool isNodeAtThreshold(const OcTreeNode* occupancyNode) const{
125  return (occupancyNode->getLogOdds() >= this->clamping_thres_max
126  || occupancyNode->getLogOdds() <= this->clamping_thres_min);
127  }
128 
130  inline bool isNodeAtThreshold(const OcTreeNode& occupancyNode) const{
131  return (occupancyNode.getLogOdds() >= this->clamping_thres_max
132  || occupancyNode.getLogOdds() <= this->clamping_thres_min);
133  }
134 
135  // - update functions
136 
146  virtual OcTreeNode* updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval = false) = 0;
147 
158  virtual OcTreeNode* updateNode(const point3d& value, float log_odds_update, bool lazy_eval = false) = 0;
159 
169  virtual OcTreeNode* updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval = false) = 0;
170 
181  virtual OcTreeNode* updateNode(const point3d& value, bool occupied, bool lazy_eval = false) = 0;
182 
183  virtual void toMaxLikelihood() = 0;
184 
185  //-- parameters for occupancy and sensor model:
186 
188  void setOccupancyThres(double prob){occ_prob_thres_log = logodds(prob); }
190  void setProbHit(double prob){prob_hit_log = logodds(prob); assert(prob_hit_log >= 0.0);}
192  void setProbMiss(double prob){prob_miss_log = logodds(prob); assert(prob_miss_log <= 0.0);}
194  void setClampingThresMin(double thresProb){clamping_thres_min = logodds(thresProb); }
196  void setClampingThresMax(double thresProb){clamping_thres_max = logodds(thresProb); }
197 
201  float getOccupancyThresLog() const {return occ_prob_thres_log; }
202 
204  double getProbHit() const {return probability(prob_hit_log); }
206  float getProbHitLog() const {return prob_hit_log; }
208  double getProbMiss() const {return probability(prob_miss_log); }
210  float getProbMissLog() const {return prob_miss_log; }
211 
220 
221 
222 
223 
224  protected:
226  bool readBinaryLegacyHeader(std::istream &s, unsigned int& size, double& res);
227 
228  // occupancy parameters of tree, stored in logodds:
234 
235  static const std::string binaryFileHeader;
236  };
237 
238 }; // end namespace
239 
240 
241 #endif
bool isNodeAtThreshold(const OcTreeNode &occupancyNode) const
queries whether a node is at the clamping threshold according to the tree&#39;s parameter ...
void setClampingThresMax(double thresProb)
sets the maximum threshold for occupancy clamping (sensor model)
void setProbHit(double prob)
sets the probability for a "hit" (will be converted to logodds) - sensor model
double probability(double logodds)
compute probability from logodds:
Definition: octomap_utils.h:47
virtual size_t size() const =0
void setProbMiss(double prob)
sets the probability for a "miss" (will be converted to logodds) - sensor model
virtual std::ostream & writeBinaryData(std::ostream &s) const =0
Writes the actual data, implemented in OccupancyOcTreeBase::writeBinaryData()
bool isNodeAtThreshold(const OcTreeNode *occupancyNode) const
queries whether a node is at the clamping threshold according to the tree&#39;s parameter ...
virtual OcTreeNode * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)=0
bool writeBinaryConst(const std::string &filename) const
bool writeBinary(const std::string &filename)
float getLogOdds() const
Definition: OcTreeNode.h:68
This class represents a three-dimensional vector.
Definition: Vector3.h:50
static const std::string binaryFileHeader
void setOccupancyThres(double prob)
sets the threshold for occupancy (sensor model)
float logodds(double probability)
compute log-odds from probability:
Definition: octomap_utils.h:42
bool readBinaryLegacyHeader(std::istream &s, unsigned int &size, double &res)
Try to read the old binary format for conversion, will be removed in the future.
bool isNodeOccupied(const OcTreeNode &occupancyNode) const
queries whether a node is occupied according to the tree&#39;s parameter for "occupancy" ...
virtual std::istream & readBinaryData(std::istream &s)=0
Reads the actual data, implemented in OccupancyOcTreeBase::readBinaryData()
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree&#39;s parameter for "occupancy" ...
void setClampingThresMin(double thresProb)
sets the minimum threshold for occupancy clamping (sensor model)


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Wed Jun 5 2019 19:26:27