octomap 1.9.8
AbstractOccupancyOcTree.h
Go to the documentation of this file.
1/*
2 * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
3 * https://octomap.github.io/
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
45namespace 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
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
This abstract class is an interface to all octrees and provides a factory design pattern for readin a...
Definition: AbstractOcTree.h:50
virtual size_t size() const =0
Interface class for all octree types that store occupancy.
Definition: AbstractOccupancyOcTree.h:52
void setOccupancyThres(double prob)
sets the threshold for occupancy (sensor model)
Definition: AbstractOccupancyOcTree.h:188
void setProbHit(double prob)
sets the probability for a "hit" (will be converted to logodds) - sensor model
Definition: AbstractOccupancyOcTree.h:190
float prob_miss_log
Definition: AbstractOccupancyOcTree.h:232
float clamping_thres_min
Definition: AbstractOccupancyOcTree.h:229
double getClampingThresMin() const
Definition: AbstractOccupancyOcTree.h:213
float prob_hit_log
Definition: AbstractOccupancyOcTree.h:231
AbstractOccupancyOcTree()
Definition: AbstractOccupancyOcTree.cpp:40
void setProbMiss(double prob)
sets the probability for a "miss" (will be converted to logodds) - sensor model
Definition: AbstractOccupancyOcTree.h:192
float getProbMissLog() const
Definition: AbstractOccupancyOcTree.h:210
float getProbHitLog() const
Definition: AbstractOccupancyOcTree.h:206
bool writeBinaryConst(const std::string &filename) const
Writes OcTree to a binary file using writeBinaryConst().
Definition: AbstractOccupancyOcTree.cpp:60
float clamping_thres_max
Definition: AbstractOccupancyOcTree.h:230
float getClampingThresMaxLog() const
Definition: AbstractOccupancyOcTree.h:219
double getProbMiss() const
Definition: AbstractOccupancyOcTree.h:208
void setClampingThresMax(double thresProb)
sets the maximum threshold for occupancy clamping (sensor model)
Definition: AbstractOccupancyOcTree.h:196
double getClampingThresMax() const
Definition: AbstractOccupancyOcTree.h:217
bool isNodeAtThreshold(const OcTreeNode &occupancyNode) const
queries whether a node is at the clamping threshold according to the tree's parameter
Definition: AbstractOccupancyOcTree.h:130
float occ_prob_thres_log
Definition: AbstractOccupancyOcTree.h:233
float getOccupancyThresLog() const
Definition: AbstractOccupancyOcTree.h:201
virtual OcTreeNode * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)=0
Manipulate log_odds value of voxel directly.
virtual OcTreeNode * updateNode(const OcTreeKey &key, bool occupied, bool lazy_eval=false)=0
Integrate occupancy measurement.
float getClampingThresMinLog() const
Definition: AbstractOccupancyOcTree.h:215
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree's parameter for "occupancy"
Definition: AbstractOccupancyOcTree.h:114
bool writeBinary(const std::string &filename)
Writes OcTree to a binary file using writeBinary().
Definition: AbstractOccupancyOcTree.cpp:50
bool isNodeAtThreshold(const OcTreeNode *occupancyNode) const
queries whether a node is at the clamping threshold according to the tree's parameter
Definition: AbstractOccupancyOcTree.h:124
double getProbHit() const
Definition: AbstractOccupancyOcTree.h:204
bool isNodeOccupied(const OcTreeNode &occupancyNode) const
queries whether a node is occupied according to the tree's parameter for "occupancy"
Definition: AbstractOccupancyOcTree.h:119
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.
Definition: AbstractOccupancyOcTree.cpp:98
virtual ~AbstractOccupancyOcTree()
Definition: AbstractOccupancyOcTree.h:55
virtual OcTreeNode * updateNode(const point3d &value, float log_odds_update, bool lazy_eval=false)=0
Manipulate log_odds value of voxel directly.
virtual std::istream & readBinaryData(std::istream &s)=0
Reads the actual data, implemented in OccupancyOcTreeBase::readBinaryData()
virtual OcTreeNode * updateNode(const point3d &value, bool occupied, bool lazy_eval=false)=0
Integrate occupancy measurement.
double getOccupancyThres() const
Definition: AbstractOccupancyOcTree.h:199
virtual std::ostream & writeBinaryData(std::ostream &s) const =0
Writes the actual data, implemented in OccupancyOcTreeBase::writeBinaryData()
void setClampingThresMin(double thresProb)
sets the minimum threshold for occupancy clamping (sensor model)
Definition: AbstractOccupancyOcTree.h:194
bool readBinary(std::istream &s)
Reads an OcTree from an input stream.
Definition: AbstractOccupancyOcTree.cpp:135
static const std::string binaryFileHeader
Definition: AbstractOccupancyOcTree.h:235
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:70
Nodes to be used in OcTree.
Definition: OcTreeNode.h:55
float getLogOdds() const
Definition: OcTreeNode.h:68
This class represents a three-dimensional vector.
Definition: Vector3.h:50
Namespace the OctoMap library and visualization tools.
float logodds(double probability)
compute log-odds from probability:
Definition: octomap_utils.h:42
double probability(double logodds)
compute probability from logodds:
Definition: octomap_utils.h:47