AbstractOccupancyOcTree.cpp
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 
00035 #include <octomap/AbstractOccupancyOcTree.h>
00036 #include <octomap/octomap_types.h>
00037 
00038 
00039 namespace octomap {
00040   AbstractOccupancyOcTree::AbstractOccupancyOcTree(){
00041     // some sane default values:
00042     setOccupancyThres(0.5);   // = 0.0 in logodds
00043     setProbHit(0.7);          // = 0.85 in logodds
00044     setProbMiss(0.4);         // = -0.4 in logodds
00045 
00046     setClampingThresMin(0.1192); // = -2 in log odds
00047     setClampingThresMax(0.971); // = 3.5 in log odds
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     // convert to max likelihood first, this makes efficient pruning on binary data possible
00074     this->toMaxLikelihood();
00075     this->prune();
00076     return writeBinaryConst(s);
00077   }
00078 
00079   bool AbstractOccupancyOcTree::writeBinaryConst(std::ostream &s) const{
00080     // write new header first:
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     // check if first line valid:
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{ // try to read old binary format:
00154       s.clear(); // clear eofbit of istream
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     // otherwise: values are valid, stream is now at binary data!
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 }


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:14