Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #ifndef OCTOMAP_ABSTRACT_OCCUPANCY_OCTREE_H
00035 #define OCTOMAP_ABSTRACT_OCCUPANCY_OCTREE_H
00036
00037 #include "AbstractOcTree.h"
00038 #include "octomap_utils.h"
00039 #include "OcTreeNode.h"
00040 #include "OcTreeKey.h"
00041 #include <cassert>
00042 #include <fstream>
00043
00044
00045 namespace octomap {
00046
00052 class AbstractOccupancyOcTree : public AbstractOcTree {
00053 public:
00054 AbstractOccupancyOcTree();
00055 virtual ~AbstractOccupancyOcTree() {};
00056
00057
00058
00064 bool writeBinary(const std::string& filename);
00065
00072 bool writeBinary(std::ostream &s);
00073
00081 bool writeBinaryConst(const std::string& filename) const;
00082
00089 bool writeBinaryConst(std::ostream &s) const;
00090
00092 virtual std::ostream& writeBinaryData(std::ostream &s) const = 0;
00093
00099 bool readBinary(std::istream &s);
00100
00106 bool readBinary(const std::string& filename);
00107
00109 virtual std::istream& readBinaryData(std::istream &s) = 0;
00110
00111
00112
00114 inline bool isNodeOccupied(const OcTreeNode* occupancyNode) const{
00115 return (occupancyNode->getLogOdds() >= this->occ_prob_thres_log);
00116 }
00117
00119 inline bool isNodeOccupied(const OcTreeNode& occupancyNode) const{
00120 return (occupancyNode.getLogOdds() >= this->occ_prob_thres_log);
00121 }
00122
00124 inline bool isNodeAtThreshold(const OcTreeNode* occupancyNode) const{
00125 return (occupancyNode->getLogOdds() >= this->clamping_thres_max
00126 || occupancyNode->getLogOdds() <= this->clamping_thres_min);
00127 }
00128
00130 inline bool isNodeAtThreshold(const OcTreeNode& occupancyNode) const{
00131 return (occupancyNode.getLogOdds() >= this->clamping_thres_max
00132 || occupancyNode.getLogOdds() <= this->clamping_thres_min);
00133 }
00134
00135
00136
00146 virtual OcTreeNode* updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval = false) = 0;
00147
00158 virtual OcTreeNode* updateNode(const point3d& value, float log_odds_update, bool lazy_eval = false) = 0;
00159
00169 virtual OcTreeNode* updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval = false) = 0;
00170
00181 virtual OcTreeNode* updateNode(const point3d& value, bool occupied, bool lazy_eval = false) = 0;
00182
00183 virtual void toMaxLikelihood() = 0;
00184
00185
00186
00188 void setOccupancyThres(double prob){occ_prob_thres_log = logodds(prob); }
00190 void setProbHit(double prob){prob_hit_log = logodds(prob); assert(prob_hit_log >= 0.0);}
00192 void setProbMiss(double prob){prob_miss_log = logodds(prob); assert(prob_miss_log <= 0.0);}
00194 void setClampingThresMin(double thresProb){clamping_thres_min = logodds(thresProb); }
00196 void setClampingThresMax(double thresProb){clamping_thres_max = logodds(thresProb); }
00197
00199 double getOccupancyThres() const {return probability(occ_prob_thres_log); }
00201 float getOccupancyThresLog() const {return occ_prob_thres_log; }
00202
00204 double getProbHit() const {return probability(prob_hit_log); }
00206 float getProbHitLog() const {return prob_hit_log; }
00208 double getProbMiss() const {return probability(prob_miss_log); }
00210 float getProbMissLog() const {return prob_miss_log; }
00211
00213 double getClampingThresMin() const {return probability(clamping_thres_min); }
00215 float getClampingThresMinLog() const {return clamping_thres_min; }
00217 double getClampingThresMax() const {return probability(clamping_thres_max); }
00219 float getClampingThresMaxLog() const {return clamping_thres_max; }
00220
00221
00222
00223
00224 protected:
00226 bool readBinaryLegacyHeader(std::istream &s, unsigned int& size, double& res);
00227
00228
00229 float clamping_thres_min;
00230 float clamping_thres_max;
00231 float prob_hit_log;
00232 float prob_miss_log;
00233 float occ_prob_thres_log;
00234
00235 static const std::string binaryFileHeader;
00236 };
00237
00238 };
00239
00240
00241 #endif