AbstractOccupancyOcTree.cpp
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 
36 #include <octomap/octomap_types.h>
37 
38 
39 namespace octomap {
41  // some sane default values:
42  setOccupancyThres(0.5); // = 0.0 in logodds
43  setProbHit(0.7); // = 0.85 in logodds
44  setProbMiss(0.4); // = -0.4 in logodds
45 
46  setClampingThresMin(0.1192); // = -2 in log odds
47  setClampingThresMax(0.971); // = 3.5 in log odds
48  }
49 
50  bool AbstractOccupancyOcTree::writeBinary(const std::string& filename){
51  std::ofstream binary_outfile( filename.c_str(), std::ios_base::binary);
52 
53  if (!binary_outfile.is_open()){
54  OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing written.");
55  return false;
56  }
57  return writeBinary(binary_outfile);
58  }
59 
60  bool AbstractOccupancyOcTree::writeBinaryConst(const std::string& filename) const{
61  std::ofstream binary_outfile( filename.c_str(), std::ios_base::binary);
62 
63  if (!binary_outfile.is_open()){
64  OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing written.");
65  return false;
66  }
67  writeBinaryConst(binary_outfile);
68  binary_outfile.close();
69  return true;
70  }
71 
72  bool AbstractOccupancyOcTree::writeBinary(std::ostream &s){
73  // convert to max likelihood first, this makes efficient pruning on binary data possible
74  this->toMaxLikelihood();
75  this->prune();
76  return writeBinaryConst(s);
77  }
78 
79  bool AbstractOccupancyOcTree::writeBinaryConst(std::ostream &s) const{
80  // write new header first:
81  s << binaryFileHeader <<"\n# (feel free to add / change comments, but leave the first line as it is!)\n#\n";
82  s << "id " << this->getTreeType() << std::endl;
83  s << "size "<< this->size() << std::endl;
84  s << "res " << this->getResolution() << std::endl;
85  s << "data" << std::endl;
86 
87  writeBinaryData(s);
88 
89  if (s.good()){
90  OCTOMAP_DEBUG(" done.\n");
91  return true;
92  } else {
93  OCTOMAP_WARNING_STR("Output stream not \"good\" after writing tree");
94  return false;
95  }
96  }
97 
98  bool AbstractOccupancyOcTree::readBinaryLegacyHeader(std::istream &s, unsigned int& size, double& res) {
99 
100  if (!s.good()){
101  OCTOMAP_WARNING_STR("Input filestream not \"good\" in OcTree::readBinary");
102  }
103 
104  int tree_type = -1;
105  s.read((char*)&tree_type, sizeof(tree_type));
106  if (tree_type == 3){
107 
108  this->clear();
109 
110  s.read((char*)&res, sizeof(res));
111  if (res <= 0.0){
112  OCTOMAP_ERROR("Invalid tree resolution: %f", res);
113  return false;
114  }
115 
116  s.read((char*)&size, sizeof(size));
117 
118  return true;
119  }
120  else {
121  OCTOMAP_ERROR_STR("Binary file does not contain an OcTree!");
122  return false;
123  }
124  }
125 
126  bool AbstractOccupancyOcTree::readBinary(const std::string& filename){
127  std::ifstream binary_infile( filename.c_str(), std::ios_base::binary);
128  if (!binary_infile.is_open()){
129  OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing read.");
130  return false;
131  }
132  return readBinary(binary_infile);
133  }
134 
135  bool AbstractOccupancyOcTree::readBinary(std::istream &s) {
136 
137  if (!s.good()){
138  OCTOMAP_WARNING_STR("Input filestream not \"good\" in OcTree::readBinary");
139  }
140 
141  // check if first line valid:
142  std::string line;
143  std::istream::pos_type streampos = s.tellg();
144  std::getline(s, line);
145  unsigned size;
146  double res;
148  std::string id;
149  if (!AbstractOcTree::readHeader(s, id, size, res))
150  return false;
151 
152  OCTOMAP_DEBUG_STR("Reading binary octree type "<< id);
153  } else{ // try to read old binary format:
154  s.clear(); // clear eofbit of istream
155  s.seekg(streampos);
156  if (readBinaryLegacyHeader(s, size, res)){
157  OCTOMAP_WARNING_STR("You are using an outdated binary tree file format.");
158  OCTOMAP_WARNING_STR("Please convert your .bt files with convert_octree.");
159  }
160  else {
161  OCTOMAP_ERROR_STR("First line of OcTree file header does not start with \""<< AbstractOccupancyOcTree::binaryFileHeader<<"\"");
162  return false;
163  }
164  }
165  // otherwise: values are valid, stream is now at binary data!
166  this->clear();
167  this->setResolution(res);
168 
169  if (size > 0)
170  this->readBinaryData(s);
171 
172  if (size != this->size()){
173  OCTOMAP_ERROR("Tree size mismatch: # read nodes (%zu) != # expected nodes (%d)\n",this->size(), size);
174  return false;
175  }
176 
177  return true;
178  }
179 
180  const std::string AbstractOccupancyOcTree::binaryFileHeader = "# Octomap OcTree binary file";
181 }
virtual std::string getTreeType() const =0
returns actual class name as string for identification
void setClampingThresMax(double thresProb)
sets the maximum threshold for occupancy clamping (sensor model)
#define OCTOMAP_DEBUG(...)
Definition: octomap_types.h:72
void setProbHit(double prob)
sets the probability for a "hit" (will be converted to logodds) - sensor model
static bool readHeader(std::istream &s, std::string &id, unsigned &size, double &res)
virtual size_t size() const =0
void setProbMiss(double prob)
sets the probability for a "miss" (will be converted to logodds) - sensor model
virtual double getResolution() const =0
virtual std::ostream & writeBinaryData(std::ostream &s) const =0
Writes the actual data, implemented in OccupancyOcTreeBase::writeBinaryData()
bool writeBinary(const std::string &filename)
bool writeBinaryConst(const std::string &filename) const
virtual void prune()=0
virtual void clear()=0
#define OCTOMAP_WARNING_STR(args)
Definition: octomap_types.h:77
static const std::string binaryFileHeader
void setOccupancyThres(double prob)
sets the threshold for occupancy (sensor model)
#define OCTOMAP_ERROR_STR(args)
Definition: octomap_types.h:79
#define OCTOMAP_DEBUG_STR(args)
Definition: octomap_types.h:73
virtual void setResolution(double res)=0
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.
#define OCTOMAP_ERROR(...)
Definition: octomap_types.h:78
virtual std::istream & readBinaryData(std::istream &s)=0
Reads the actual data, implemented in OccupancyOcTreeBase::readBinaryData()
void setClampingThresMin(double thresProb)
sets the minimum threshold for occupancy clamping (sensor model)


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Feb 28 2022 22:58:06