graph_optimizer2d_aux.cpp
Go to the documentation of this file.
00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. K├╝mmerle, C. Stachniss
00003 // 
00004 // HOG-Man is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 // 
00009 // HOG-Man is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 // 
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #include <sstream>
00018 #include "graph_optimizer2d_aux.h"
00019 
00020 namespace AISNavigation{
00021   using namespace std;
00022 
00023 
00024 #define LINESIZE 4096
00025 
00026   void loadEdges(LoadedEdgeSet& edges, istream& is, bool overrideCovariances){
00027     edges.clear();
00028     if (!is)
00029       return;
00030     
00031     while(is){
00032       char buf[LINESIZE];
00033       is.getline(buf,LINESIZE);
00034       istringstream ls(buf);
00035       string tag;
00036       ls >> tag;
00037       
00038       if (tag=="EDGE" || tag=="EDGE2"){
00039         int id1, id2;
00040         Vector3 p;
00041         Matrix3 m;
00042         ls >> id1 >> id2 >> p.x() >> p.y() >> p.z();
00043         if (id1>id2) { //hack
00044           int a=id1; id1=id2; id2=a;
00045           p=(Transformation2::fromVector(p).inverse()).toVector();
00046         }
00047         
00048         if (overrideCovariances){
00049           m=Matrix3::eye(1.);
00050         } else {
00051           ls >> m[0][0] >> m[0][1] >> m [1][1]
00052              >> m[2][2] >> m[0][2] >> m [1][2];
00053           m[1][0]=m[0][1];
00054           m[2][0]=m[0][2];
00055           m[2][1]=m[1][2];
00056         }
00057         LoadedEdge e;
00058         e.id1=id1;
00059         e.id2=id2;
00060         e.mean=Transformation2::fromVector(p);
00061         e.informationMatrix=m;
00062         edges.insert(e);
00063       }
00064     }
00065   }
00066 
00067 }


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:58