Main MRPT website > C++ reference for MRPT 1.4.0
AbstractOccupancyOcTree.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef OCTOMAP_ABSTRACT_OCCUPANCY_OCTREE_H
10 #define OCTOMAP_ABSTRACT_OCCUPANCY_OCTREE_H
11 
12 // $Id: AbstractOccupancyOcTree.h 420 2012-08-27 14:10:25Z ahornung $
13 
14 /**
15 * OctoMap:
16 * A probabilistic, flexible, and compact 3D mapping library for robotic systems.
17 * @author K. M. Wurm, A. Hornung, University of Freiburg, Copyright (C) 2009-2011.
18 * @see http://octomap.sourceforge.net/
19 * License: New BSD License
20 */
21 
22 /*
23  * Copyright (c) 2009-2011, K. M. Wurm, A. Hornung, University of Freiburg
24  * All rights reserved.
25  *
26  * Redistribution and use in source and binary forms, with or without
27  * modification, are permitted provided that the following conditions are met:
28  *
29  * * Redistributions of source code must retain the above copyright
30  * notice, this list of conditions and the following disclaimer.
31  * * Redistributions in binary form must reproduce the above copyright
32  * notice, this list of conditions and the following disclaimer in the
33  * documentation and/or other materials provided with the distribution.
34  * * Neither the name of the University of Freiburg nor the names of its
35  * contributors may be used to endorse or promote products derived from
36  * this software without specific prior written permission.
37  *
38  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
39  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
40  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
41  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
42  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
43  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
44  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
45  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
46  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
47  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
48  * POSSIBILITY OF SUCH DAMAGE.
49  */
50 
51 #include "AbstractOcTree.h"
52 #include "octomap_utils.h"
53 #include "OcTreeNode.h"
54 #include "OcTreeKey.h"
55 #include <cassert>
56 #include <fstream>
57 
58 
59 namespace octomap {
60 
61  /**
62  * Interface class for all octree types that store occupancy. This serves
63  * as a common base class e.g. for polymorphism and contains common code
64  * for reading and writing binary trees.
65  */
67  public:
70 
71  //-- IO
72 
73  /**
74  * Writes OcTree to a binary file using writeBinary().
75  * The OcTree is first converted to the maximum likelihood estimate and pruned.
76  * @return success of operation
77  */
78  bool writeBinary(const std::string& filename);
79 
80  /**
81  * Writes compressed maximum likelihood OcTree to a binary stream.
82  * The OcTree is first converted to the maximum likelihood estimate and pruned
83  * for maximum compression.
84  * @return success of operation
85  */
86  bool writeBinary(std::ostream &s);
87 
88  /**
89  * Writes OcTree to a binary file using writeBinaryConst().
90  * The OcTree is not changed, in particular not pruned first.
91  * Files will be smaller when the tree is pruned first or by using
92  * writeBinary() instead.
93  * @return success of operation
94  */
95  bool writeBinaryConst(const std::string& filename) const;
96 
97  /**
98  * Writes the maximum likelihood OcTree to a binary stream (const variant).
99  * Files will be smaller when the tree is pruned first or by using
100  * writeBinary() instead.
101  * @return success of operation
102  */
103  bool writeBinaryConst(std::ostream &s) const;
104 
105  /// Writes the actual data, implemented in OccupancyOcTreeBase::writeBinaryData()
106  virtual std::ostream& writeBinaryData(std::ostream &s) const = 0;
107 
108  /**
109  * Reads an OcTree from an input stream.
110  * Existing nodes of the tree are deleted before the tree is read.
111  * @return success of operation
112  */
113  bool readBinary(std::istream &s);
114 
115  /**
116  * Reads OcTree from a binary file.
117  * Existing nodes of the tree are deleted before the tree is read.
118  * @return success of operation
119  */
120  bool readBinary(const std::string& filename);
121 
122  /// Reads the actual data, implemented in OccupancyOcTreeBase::readBinaryData()
123  virtual std::istream& readBinaryData(std::istream &s) = 0;
124 
125  // -- occupancy queries
126 
127  /// queries whether a node is occupied according to the tree's parameter for "occupancy"
128  inline bool isNodeOccupied(const OcTreeNode* occupancyNode) const{
129  return (occupancyNode->getLogOdds() >= this->occ_prob_thres_log);
130  }
131 
132  /// queries whether a node is occupied according to the tree's parameter for "occupancy"
133  inline bool isNodeOccupied(const OcTreeNode& occupancyNode) const{
134  return (occupancyNode.getLogOdds() >= this->occ_prob_thres_log);
135  }
136 
137  /// queries whether a node is at the clamping threshold according to the tree's parameter
138  inline bool isNodeAtThreshold(const OcTreeNode* occupancyNode) const{
139  return (occupancyNode->getLogOdds() >= this->clamping_thres_max
140  || occupancyNode->getLogOdds() <= this->clamping_thres_min);
141  }
142 
143  /// queries whether a node is at the clamping threshold according to the tree's parameter
144  inline bool isNodeAtThreshold(const OcTreeNode& occupancyNode) const{
145  return (occupancyNode.getLogOdds() >= this->clamping_thres_max
146  || occupancyNode.getLogOdds() <= this->clamping_thres_min);
147  }
148 
149  // - update functions
150 
151  /**
152  * Manipulate log_odds value of voxel directly
153  *
154  * @param key of the NODE that is to be updated
155  * @param log_odds_update value to be added (+) to log_odds value of node
156  * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
157  * This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
158  * @return pointer to the updated NODE
159  */
160  virtual OcTreeNode* updateNode(const OcTreeKey& key, float log_odds_update, bool lazy_eval = false) = 0;
161 
162  /**
163  * Manipulate log_odds value of voxel directly.
164  * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.
165  *
166  * @param value 3d coordinate of the NODE that is to be updated
167  * @param log_odds_update value to be added (+) to log_odds value of node
168  * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
169  * This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
170  * @return pointer to the updated NODE
171  */
172  virtual OcTreeNode* updateNode(const point3d& value, float log_odds_update, bool lazy_eval = false) = 0;
173 
174  /**
175  * Integrate occupancy measurement.
176  *
177  * @param key of the NODE that is to be updated
178  * @param occupied true if the node was measured occupied, else false
179  * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
180  * This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
181  * @return pointer to the updated NODE
182  */
183  virtual OcTreeNode* updateNode(const OcTreeKey& key, bool occupied, bool lazy_eval = false) = 0;
184 
185  /**
186  * Integrate occupancy measurement.
187  * Looks up the OcTreeKey corresponding to the coordinate and then calls udpateNode() with it.
188  *
189  * @param value 3d coordinate of the NODE that is to be updated
190  * @param occupied true if the node was measured occupied, else false
191  * @param lazy_eval whether update of inner nodes is omitted after the update (default: false).
192  * This speeds up the insertion, but you need to call updateInnerOccupancy() when done.
193  * @return pointer to the updated NODE
194  */
195  virtual OcTreeNode* updateNode(const point3d& value, bool occupied, bool lazy_eval = false) = 0;
196 
197  virtual void toMaxLikelihood() = 0;
198 
199  //-- parameters for occupancy and sensor model:
200 
201  /// sets the threshold for occupancy (sensor model)
202  void setOccupancyThres(double prob){occ_prob_thres_log = logodds(prob); }
203  /// sets the probablility for a "hit" (will be converted to logodds) - sensor model
204  void setProbHit(double prob){prob_hit_log = logodds(prob); assert(prob_hit_log >= 0.0);}
205  /// sets the probablility for a "miss" (will be converted to logodds) - sensor model
206  void setProbMiss(double prob){prob_miss_log = logodds(prob); assert(prob_miss_log <= 0.0);}
207  /// sets the minimum threshold for occupancy clamping (sensor model)
208  void setClampingThresMin(double thresProb){clamping_thres_min = logodds(thresProb); }
209  /// sets the maximum threshold for occupancy clamping (sensor model)
210  void setClampingThresMax(double thresProb){clamping_thres_max = logodds(thresProb); }
211 
212  /// @return threshold (probability) for occupancy - sensor model
214  /// @return threshold (logodds) for occupancy - sensor model
215  float getOccupancyThresLog() const {return occ_prob_thres_log; }
216 
217  /// @return probablility for a "hit" in the sensor model (probability)
218  double getProbHit() const {return probability(prob_hit_log); }
219  /// @return probablility for a "hit" in the sensor model (logodds)
220  float getProbHitLog() const {return prob_hit_log; }
221  /// @return probablility for a "miss" in the sensor model (probability)
222  double getProbMiss() const {return probability(prob_miss_log); }
223  /// @return probablility for a "miss" in the sensor model (logodds)
224  float getProbMissLog() const {return prob_miss_log; }
225 
226  /// @return minimum threshold for occupancy clamping in the sensor model (probability)
228  /// @return minimum threshold for occupancy clamping in the sensor model (logodds)
230  /// @return maximum threshold for occupancy clamping in the sensor model (probability)
232  /// @return maximum threshold for occupancy clamping in the sensor model (logodds)
234 
235 
236 
237 
238  protected:
239  /// Try to read the old binary format for conversion, will be removed in the future
240  bool readBinaryLegacyHeader(std::istream &s, unsigned int& size, double& res);
241 
242  // occupancy parameters of tree, stored in logodds:
248 
249  static const std::string binaryFileHeader;
250  };
251 
252 } // end namespace
253 
254 
255 #endif
bool writeBinary(const std::string &filename)
Writes OcTree to a binary file using writeBinary().
bool isNodeAtThreshold(const OcTreeNode &occupancyNode) const
queries whether a node is at the clamping threshold according to the tree&#39;s parameter ...
OctoMap: A probabilistic, flexible, and compact 3D mapping library for robotic systems.
This abstract class is an interface to all octrees and provides a factory design pattern for readin a...
bool writeBinaryConst(const std::string &filename) const
Writes OcTree to a binary file using writeBinaryConst().
virtual std::istream & readBinaryData(std::istream &s)=0
Reads the actual data, implemented in OccupancyOcTreeBase::readBinaryData()
double probability(double logodds)
compute probability from logodds:
Definition: octomap_utils.h:61
static const std::string binaryFileHeader
bool isNodeOccupied(const OcTreeNode &occupancyNode) const
queries whether a node is occupied according to the tree&#39;s parameter for "occupancy" ...
void setProbHit(double prob)
sets the probablility for a "hit" (will be converted to logodds) - sensor model
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
queries whether a node is occupied according to the tree&#39;s parameter for "occupancy" ...
float getLogOdds() const
Definition: OcTreeNode.h:89
void setClampingThresMin(double thresProb)
sets the minimum threshold for occupancy clamping (sensor model)
bool isNodeAtThreshold(const OcTreeNode *occupancyNode) const
queries whether a node is at the clamping threshold according to the tree&#39;s parameter ...
Interface class for all octree types that store occupancy.
virtual OcTreeNode * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)=0
Manipulate log_odds value of voxel directly.
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.
void setOccupancyThres(double prob)
sets the threshold for occupancy (sensor model)
virtual std::ostream & writeBinaryData(std::ostream &s) const =0
Writes the actual data, implemented in OccupancyOcTreeBase::writeBinaryData()
OcTreeKey is a container class for internal key addressing.
Definition: OcTreeKey.h:68
bool readBinary(std::istream &s)
Reads an OcTree from an input stream.
Nodes to be used in OcTree.
Definition: OcTreeNode.h:67
float logodds(double probability)
compute log-odds from probability:
Definition: octomap_utils.h:56
virtual size_t size() const =0
void setProbMiss(double prob)
sets the probablility for a "miss" (will be converted to logodds) - sensor model
void setClampingThresMax(double thresProb)
sets the maximum threshold for occupancy clamping (sensor model)
This class represents a three-dimensional vector.
Definition: Vector3.h:65



Page generated by Doxygen 1.8.11 for MRPT 1.4.0 SVN:Unversioned directory at Mon Jul 4 10:31:07 UTC 2016