AbstractOcTree.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/AbstractOcTree.h>
00036 #include <octomap/OcTree.h>
00037 #include <octomap/CountingOcTree.h>
00038 
00039 
00040 namespace octomap {
00041   AbstractOcTree::AbstractOcTree(){
00042 
00043   }
00044 
00045   bool AbstractOcTree::write(const std::string& filename) const{
00046      std::ofstream file(filename.c_str(), std::ios_base::out | std::ios_base::binary);
00047 
00048      if (!file.is_open()){
00049        OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing written.");
00050        return false;
00051      } else {
00052        // TODO: check is_good of finished stream, return
00053        write(file);
00054        file.close();
00055      }
00056 
00057      return true;
00058    }
00059 
00060 
00061   bool AbstractOcTree::write(std::ostream &s) const{
00062     s << fileHeader <<"\n# (feel free to add / change comments, but leave the first line as it is!)\n#\n";
00063     s << "id " << getTreeType() << std::endl;
00064     s << "size "<< size() << std::endl;
00065     s << "res " << getResolution() << std::endl;
00066     s << "data" << std::endl;
00067 
00068     // write the actual data:
00069     writeData(s);
00070     // TODO: ret.val, checks stream?
00071     return true;
00072   }
00073 
00074   AbstractOcTree* AbstractOcTree::read(const std::string& filename){
00075     std::ifstream file(filename.c_str(), std::ios_base::in |std::ios_base::binary);
00076 
00077     if (!file.is_open()){
00078       OCTOMAP_ERROR_STR("Filestream to "<< filename << " not open, nothing read.");
00079       return NULL;
00080     } else {
00081       // TODO: check is_good of finished stream, warn?
00082       return read(file);
00083     }
00084   }
00085 
00086 
00087   AbstractOcTree* AbstractOcTree::read(std::istream &s){
00088 
00089     // check if first line valid:
00090     std::string line;
00091     std::getline(s, line);
00092     if (line.compare(0,fileHeader.length(), fileHeader) !=0){
00093       OCTOMAP_ERROR_STR("First line of OcTree file header does not start with \""<< fileHeader);
00094       return NULL;
00095     }
00096 
00097     std::string id;
00098     unsigned size;
00099     double res;
00100     if (!AbstractOcTree::readHeader(s, id, size, res))
00101       return NULL;
00102 
00103 
00104     // otherwise: values are valid, stream is now at binary data!
00105     OCTOMAP_DEBUG_STR("Reading octree type "<< id);
00106 
00107     AbstractOcTree* tree = createTree(id, res);
00108 
00109     if (tree){
00110       if (size > 0)
00111         tree->readData(s);
00112 
00113       OCTOMAP_DEBUG_STR("Done ("<< tree->size() << " nodes)");
00114     }
00115 
00116     return tree;
00117   }
00118 
00119   bool AbstractOcTree::readHeader(std::istream& s, std::string& id, unsigned& size, double& res){
00120     id = "";
00121     size = 0;
00122     res = 0.0;
00123 
00124     std::string token;
00125     bool headerRead = false;
00126     while(s.good() && !headerRead) {
00127       s >> token;
00128       if (token == "data"){
00129         headerRead = true;
00130         // skip forward until end of line:
00131         char c;
00132         do {
00133           c = s.get();
00134         } while(s.good() && (c != '\n'));
00135       }
00136       else if (token.compare(0,1,"#") == 0){
00137         // comment line, skip forward until end of line:
00138         char c;
00139         do {
00140           c = s.get();
00141         } while(s.good() && (c != '\n'));
00142       }
00143       else if (token == "id")
00144         s >> id;
00145       else if (token == "res")
00146         s >> res;
00147       else if (token == "size")
00148         s >> size;
00149       else{
00150         OCTOMAP_WARNING_STR("Unknown keyword in OcTree header, skipping: "<<token);
00151         char c;
00152         do {
00153           c = s.get();
00154         } while(s.good() && (c != '\n'));
00155 
00156       }
00157 
00158     }
00159 
00160     if (!headerRead) {
00161       OCTOMAP_ERROR_STR("Error reading OcTree header");
00162       return false;
00163     }
00164 
00165     if (id == "") {
00166       OCTOMAP_ERROR_STR("Error reading OcTree header, ID not set");
00167       return false;
00168     }
00169 
00170     if (res <= 0.0) {
00171       OCTOMAP_ERROR_STR("Error reading OcTree header, res <= 0.0");
00172       return false;
00173     }
00174     // fix deprecated id value:
00175     if (id == "1"){
00176       OCTOMAP_WARNING("You are using a deprecated id \"%s\", changing to \"OcTree\" (you should update your file header)\n", id.c_str());
00177       id = "OcTree";
00178     }
00179 
00180     return true;
00181 
00182   }
00183 
00184   AbstractOcTree* AbstractOcTree::createTree(const std::string class_name, double res){
00185     std::map<std::string, AbstractOcTree*>::iterator it = classIDMapping().find(class_name);
00186     if (it == classIDMapping().end()){
00187       OCTOMAP_ERROR("Could not create octree of type %s, not in store in classIDMapping\n", class_name.c_str());
00188       return NULL;
00189     } else {
00190       AbstractOcTree* tree = it->second->create();
00191 
00192       tree->setResolution(res);
00193       return tree;
00194     }
00195   }
00196 
00197   std::map<std::string, AbstractOcTree*>& AbstractOcTree::classIDMapping(){
00198     // we will "leak" the memory of the map and all trees until program exits,
00199     // but this ensures all static objects are there as long as needed
00200     // http://www.parashift.com/c++-faq-lite/ctors.html#faq-10.15
00201     static std::map<std::string, AbstractOcTree*>* map = new std::map<std::string, AbstractOcTree*>();
00202     return *map;
00203   }
00204 
00205   void AbstractOcTree::registerTreeType(AbstractOcTree* tree){
00206     classIDMapping()[tree->getTreeType()] = tree;
00207   }
00208 
00209 
00210   const std::string AbstractOcTree::fileHeader = "# Octomap OcTree file";
00211 }


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Feb 11 2016 23:50:59