Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
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) {
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 }