.. _program_listing_file__tmp_ws_src_octomap_octomap_include_octomap_MapCollection.hxx:

Program Listing for File MapCollection.hxx
==========================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_octomap_octomap_include_octomap_MapCollection.hxx>` (``/tmp/ws/src/octomap/octomap/include/octomap/MapCollection.hxx``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
    * https://octomap.github.io/
    *
    * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
    * All rights reserved.
    * License: New BSD
    *
    * Redistribution and use in source and binary forms, with or without
    * modification, are permitted provided that the following conditions are met:
    *
    *     * Redistributions of source code must retain the above copyright
    *       notice, this list of conditions and the following disclaimer.
    *     * Redistributions in binary form must reproduce the above copyright
    *       notice, this list of conditions and the following disclaimer in the
    *       documentation and/or other materials provided with the distribution.
    *     * Neither the name of the University of Freiburg nor the names of its
    *       contributors may be used to endorse or promote products derived from
    *       this software without specific prior written permission.
    *
    * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
    * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
    * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
    * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
    * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
    * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
    * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
    * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
    * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
    * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    * POSSIBILITY OF SUCH DAMAGE.
    */
   
   #include <stdio.h>
   #include <sstream>
   #include <fstream>
   
   namespace octomap {
     
     template <class MAPNODE>
     MapCollection<MAPNODE>::MapCollection() {
     }
   
     template <class MAPNODE>
     MapCollection<MAPNODE>::MapCollection(std::string filename) {
       this->read(filename);
     }
   
     template <class MAPNODE>
     MapCollection<MAPNODE>::~MapCollection() {
       this->clear();
     }
   
     template <class MAPNODE>
     void MapCollection<MAPNODE>::clear() {
       // FIXME: memory leak, else we run into double frees in, e.g., the viewer...
   
       // for(typename std::vector<MAPNODE*>::iterator it= nodes.begin(); it != nodes.end(); ++it)
       //   delete *it;
       nodes.clear();
     }
   
     template <class MAPNODE>
     bool MapCollection<MAPNODE>::read(std::string filenamefullpath) {
   
       std::string path;
       std::string filename;
       splitPathAndFilename(filenamefullpath, &path, &filename);
   
       std::ifstream infile;
       infile.open(filenamefullpath.c_str(), std::ifstream::in);
       if(!infile.is_open()){
         OCTOMAP_ERROR_STR("Could not open "<< filenamefullpath << ". MapCollection not loaded.");
         return false;
       }
   
       bool ok = true;
       while(ok){
         std::string nodeID;
         ok = readTagValue("MAPNODEID", infile, &nodeID);
         if(!ok){
           //do not throw error, you could be at the end of the file
           break;
         }
   
         std::string mapNodeFilename;
         ok = readTagValue("MAPNODEFILENAME", infile, &mapNodeFilename);
         if(!ok){
           OCTOMAP_ERROR_STR("Could not read MAPNODEFILENAME.");
           break;
         }
   
         std::string poseStr;
         ok = readTagValue("MAPNODEPOSE", infile, &poseStr);
         std::istringstream poseStream(poseStr);
         float x,y,z;
         poseStream >> x >> y >> z;
         double roll,pitch,yaw;
         poseStream >> roll >> pitch >> yaw;
         ok = ok && !poseStream.fail();
         if(!ok){
           OCTOMAP_ERROR_STR("Could not read MAPNODEPOSE.");
           break;
         }
         octomap::pose6d origin(x, y, z, roll, pitch, yaw);
   
         MAPNODE* node = new MAPNODE(combinePathAndFilename(path,mapNodeFilename), origin);
         node->setId(nodeID);
   
         if(!ok){
           for(unsigned int i=0; i<nodes.size(); i++){
             delete nodes[i];
           }
           infile.close();
           return false;
         } else {
           nodes.push_back(node);
         }
       }
       infile.close();
       return true;
     }
   
     template <class MAPNODE>
     void MapCollection<MAPNODE>::addNode( MAPNODE* node){
       nodes.push_back(node);
     }
   
     template <class MAPNODE>
     MAPNODE* MapCollection<MAPNODE>::addNode(const Pointcloud& cloud, point3d sensor_origin) {
       // TODO...
       return 0;
     }
   
     template <class MAPNODE>
     bool MapCollection<MAPNODE>::removeNode(const MAPNODE* n) {
       // TODO...
       return false;
     }
   
     template <class MAPNODE>
     MAPNODE* MapCollection<MAPNODE>::queryNode(const point3d& p) {
       for (const_iterator it = this->begin(); it != this->end(); ++it) {
         point3d ptrans = (*it)->getOrigin().inv().transform(p);
         typename MAPNODE::TreeType::NodeType* n = (*it)->getMap()->search(ptrans);
         if (!n) continue;
         if ((*it)->getMap()->isNodeOccupied(n)) return (*it);
       }
       return 0;
     }
   
     template <class MAPNODE>
     bool MapCollection<MAPNODE>::isOccupied(const point3d& p) const {
       for (const_iterator it = this->begin(); it != this->end(); ++it) {
         point3d ptrans = (*it)->getOrigin().inv().transform(p);
         typename MAPNODE::TreeType::NodeType* n = (*it)->getMap()->search(ptrans);
         if (!n) continue;
         if ((*it)->getMap()->isNodeOccupied(n)) return true;
       }
       return false;
     }
   
     template <class MAPNODE>
     bool MapCollection<MAPNODE>::isOccupied(float x, float y, float z) const {
       point3d q(x,y,z);
       return this->isOccupied(q);
     }
   
   
     template <class MAPNODE>
     double MapCollection<MAPNODE>::getOccupancy(const point3d& p) {
       double max_occ_val = 0;
       bool is_unknown = true;
       for (const_iterator it = this->begin(); it != this->end(); ++it) {
         point3d ptrans = (*it)->getOrigin().inv().transform(p);
         typename MAPNODE::TreeType::NodeType* n = (*it)->getMap()->search(ptrans);
         if (n) {
           double occ = n->getOccupancy();
           if (occ > max_occ_val) max_occ_val = occ;
           is_unknown = false;
         }
       }
       if (is_unknown) return 0.5;
       return max_occ_val;
     }
   
   
     template <class MAPNODE>
     bool MapCollection<MAPNODE>::castRay(const point3d& origin, const point3d& direction, point3d& end,
                                          bool ignoreUnknownCells, double maxRange) const {
       bool hit_obstacle = false;
       double min_dist = 1e6;
       // SPEEDUP: use openMP to do raycasting in parallel
       // SPEEDUP: use bounding boxes to determine submaps 
       for (const_iterator it = this->begin(); it != this->end(); ++it) {
         point3d origin_trans = (*it)->getOrigin().inv().transform(origin);
         point3d direction_trans = (*it)->getOrigin().inv().rot().rotate(direction);
         printf("ray from %.2f,%.2f,%.2f in dir %.2f,%.2f,%.2f in node %s\n",
                origin_trans.x(), origin_trans.y(), origin_trans.z(),
                direction_trans.x(), direction_trans.y(), direction_trans.z(),
                (*it)->getId().c_str());
         point3d temp_endpoint;
         if ((*it)->getMap()->castRay(origin_trans, direction_trans, temp_endpoint, ignoreUnknownCells, maxRange)) {
           printf("hit obstacle in node %s\n", (*it)->getId().c_str());
           double current_dist =  origin_trans.distance(temp_endpoint);
           if (current_dist < min_dist) {
             min_dist = current_dist;
             end = (*it)->getOrigin().transform(temp_endpoint);
           }
           hit_obstacle = true;
         } // end if hit obst
       } // end for
       return hit_obstacle;
     }
   
     
     template <class MAPNODE>
     bool MapCollection<MAPNODE>::writePointcloud(std::string filename) {
       Pointcloud pc;
       for(typename std::vector<MAPNODE* >::iterator it = nodes.begin(); it != nodes.end(); ++it){
         Pointcloud tmp = (*it)->generatePointcloud();
         pc.push_back(tmp);
       }
       pc.writeVrml(filename);
       return true;
     }
   
   
     template <class MAPNODE>
     bool MapCollection<MAPNODE>::write(std::string filename) {
       bool ok = true;
   
       std::ofstream outfile(filename.c_str());
       outfile << "#This file was generated by the write-method of MapCollection\n";
   
       for(typename std::vector<MAPNODE* >::iterator it = nodes.begin(); it != nodes.end(); ++it){
         std::string id = (*it)->getId();
         pose6d origin = (*it)->getOrigin();
         std::string nodemapFilename = "nodemap_";
         nodemapFilename.append(id);
         nodemapFilename.append(".bt");
   
         outfile << "MAPNODEID " << id << "\n";
         outfile << "MAPNODEFILENAME "<< nodemapFilename << "\n";
         outfile << "MAPNODEPOSE " << origin.x() << " " << origin.y() << " " << origin.z() << " "
                 << origin.roll() << " " << origin.pitch() << " " << origin.yaw() << std::endl;
         ok = ok && (*it)->writeMap(nodemapFilename);
       }
       outfile.close();
       return ok;
     }
   
     // TODO
     template <class MAPNODE>
     void MapCollection<MAPNODE>::insertScan(const Pointcloud& scan, const octomap::point3d& sensor_origin,
                                             double maxrange, bool pruning, bool lazy_eval) {
       fprintf(stderr, "ERROR: MapCollection::insertScan is not implemented yet.\n");
     }
   
     template <class MAPNODE>
     MAPNODE* MapCollection<MAPNODE>::queryNode(std::string id) {
       for (const_iterator it = this->begin(); it != this->end(); ++it) {
         if ((*it)->getId() == id) return *(it);
       }
       return 0;
     }
           
     // TODO
     template <class MAPNODE>
     std::vector<Pointcloud*> MapCollection<MAPNODE>::segment(const Pointcloud& scan) const {
       std::vector<Pointcloud*> result;
       fprintf(stderr, "ERROR: MapCollection::segment is not implemented yet.\n");
       return result;
     }
   
     // TODO
     template <class MAPNODE>
     MAPNODE* MapCollection<MAPNODE>::associate(const Pointcloud& scan) {
       fprintf(stderr, "ERROR: MapCollection::associate is not implemented yet.\n");
       return 0;
     }
   
     template <class MAPNODE>
     void MapCollection<MAPNODE>::splitPathAndFilename(std::string &filenamefullpath, 
                                                       std::string* path, std::string *filename) {
   #ifdef WIN32
       std::string::size_type lastSlash = filenamefullpath.find_last_of('\\');
   #else
       std::string::size_type lastSlash = filenamefullpath.find_last_of('/');
   #endif
       if (lastSlash != std::string::npos){
         *filename =  filenamefullpath.substr(lastSlash + 1);
         *path = filenamefullpath.substr(0, lastSlash);
       } else {
         *filename = filenamefullpath;
         *path = "";
       }
     }
   
     template <class MAPNODE>
     std::string MapCollection<MAPNODE>::combinePathAndFilename(std::string path, std::string filename) {
       std::string result = path;
       if(path != ""){
   #ifdef WIN32
         result.append("\\");
   #else
         result.append("/");
   #endif
       }
       result.append(filename);
       return result;
     }
   
     template <class MAPNODE>
     bool MapCollection<MAPNODE>::readTagValue(std::string /*tag*/, std::ifstream& infile, std::string* value) {
       std::string line;
       while( getline(infile, line) ){
         if(line.length() != 0 && line[0] != '#')
           break;
       }
       *value = "";
       std::string::size_type firstSpace = line.find(' ');
       if(firstSpace != std::string::npos && firstSpace != line.size()-1){
         *value = line.substr(firstSpace + 1);
         return true;
       } 
       else return false;
     }
   
   } // namespace