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
00035 #include <octomap/AbstractOccupancyOcTree.h>
00036 #include <octomap/octomap_types.h>
00037
00038
00039 namespace octomap {
00040 AbstractOccupancyOcTree::AbstractOccupancyOcTree(){
00041
00042 setOccupancyThres(0.5);
00043 setProbHit(0.7);
00044 setProbMiss(0.4);
00045
00046 setClampingThresMin(0.1192);
00047 setClampingThresMax(0.971);
00048 }
00049
00050 bool AbstractOccupancyOcTree::writeBinary(const std::string& filename){
00051 std::ofstream binary_outfile( filename.c_str(), std::ios_base::binary);
00052
00053 if (!binary_outfile.is_open()){
00054 OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing written.");
00055 return false;
00056 }
00057 return writeBinary(binary_outfile);
00058 }
00059
00060 bool AbstractOccupancyOcTree::writeBinaryConst(const std::string& filename) const{
00061 std::ofstream binary_outfile( filename.c_str(), std::ios_base::binary);
00062
00063 if (!binary_outfile.is_open()){
00064 OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing written.");
00065 return false;
00066 }
00067 writeBinaryConst(binary_outfile);
00068 binary_outfile.close();
00069 return true;
00070 }
00071
00072 bool AbstractOccupancyOcTree::writeBinary(std::ostream &s){
00073
00074 this->toMaxLikelihood();
00075 this->prune();
00076 return writeBinaryConst(s);
00077 }
00078
00079 bool AbstractOccupancyOcTree::writeBinaryConst(std::ostream &s) const{
00080
00081 s << binaryFileHeader <<"\n# (feel free to add / change comments, but leave the first line as it is!)\n#\n";
00082 s << "id " << this->getTreeType() << std::endl;
00083 s << "size "<< this->size() << std::endl;
00084 s << "res " << this->getResolution() << std::endl;
00085 s << "data" << std::endl;
00086
00087 writeBinaryData(s);
00088
00089 if (s.good()){
00090 OCTOMAP_DEBUG(" done.\n");
00091 return true;
00092 } else {
00093 OCTOMAP_WARNING_STR("Output stream not \"good\" after writing tree");
00094 return false;
00095 }
00096 }
00097
00098 bool AbstractOccupancyOcTree::readBinaryLegacyHeader(std::istream &s, unsigned int& size, double& res) {
00099
00100 if (!s.good()){
00101 OCTOMAP_WARNING_STR("Input filestream not \"good\" in OcTree::readBinary");
00102 }
00103
00104 int tree_type = -1;
00105 s.read((char*)&tree_type, sizeof(tree_type));
00106 if (tree_type == 3){
00107
00108 this->clear();
00109
00110 s.read((char*)&res, sizeof(res));
00111 if (res <= 0.0){
00112 OCTOMAP_ERROR("Invalid tree resolution: %f", res);
00113 return false;
00114 }
00115
00116 s.read((char*)&size, sizeof(size));
00117
00118 return true;
00119 }
00120 else {
00121 OCTOMAP_ERROR_STR("Binary file does not contain an OcTree!");
00122 return false;
00123 }
00124 }
00125
00126 bool AbstractOccupancyOcTree::readBinary(const std::string& filename){
00127 std::ifstream binary_infile( filename.c_str(), std::ios_base::binary);
00128 if (!binary_infile.is_open()){
00129 OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing read.");
00130 return false;
00131 }
00132 return readBinary(binary_infile);
00133 }
00134
00135 bool AbstractOccupancyOcTree::readBinary(std::istream &s) {
00136
00137 if (!s.good()){
00138 OCTOMAP_WARNING_STR("Input filestream not \"good\" in OcTree::readBinary");
00139 }
00140
00141
00142 std::string line;
00143 std::istream::pos_type streampos = s.tellg();
00144 std::getline(s, line);
00145 unsigned size;
00146 double res;
00147 if (line.compare(0,AbstractOccupancyOcTree::binaryFileHeader.length(), AbstractOccupancyOcTree::binaryFileHeader) ==0){
00148 std::string id;
00149 if (!AbstractOcTree::readHeader(s, id, size, res))
00150 return false;
00151
00152 OCTOMAP_DEBUG_STR("Reading binary octree type "<< id);
00153 } else{
00154 s.clear();
00155 s.seekg(streampos);
00156 if (readBinaryLegacyHeader(s, size, res)){
00157 OCTOMAP_WARNING_STR("You are using an outdated binary tree file format.");
00158 OCTOMAP_WARNING_STR("Please convert your .bt files with convert_octree.");
00159 }
00160 else {
00161 OCTOMAP_ERROR_STR("First line of OcTree file header does not start with \""<< AbstractOccupancyOcTree::binaryFileHeader<<"\"");
00162 return false;
00163 }
00164 }
00165
00166 this->clear();
00167 this->setResolution(res);
00168
00169 if (size > 0)
00170 this->readBinaryData(s);
00171
00172 if (size != this->size()){
00173 OCTOMAP_ERROR("Tree size mismatch: # read nodes (%zu) != # expected nodes (%d)\n",this->size(), size);
00174 return false;
00175 }
00176
00177 return true;
00178 }
00179
00180 const std::string AbstractOccupancyOcTree::binaryFileHeader = "# Octomap OcTree binary file";
00181 }