AbstractOccupancyOcTree.h
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License: New BSD
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
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     //-- IO
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     // -- occupancy queries
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     // - update functions
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     //-- parameters for occupancy and sensor model:
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     // occupancy parameters of tree, stored in logodds:
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 }; // end namespace
00239 
00240 
00241 #endif


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Feb 11 2016 23:50:59