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 "loadEdges3d.h"
00018
00019 #include <string>
00020 #include <sstream>
00021 using namespace std;
00022
00023 namespace AISNavigation {
00024
00025 void loadEdges3D(LoadedEdgeSet3D& edges, std::istream& is, bool overrideCovariances)
00026 {
00027 if (! is)
00028 return;
00029 string line;
00030 while (getline(is, line)) {
00031 if (line.size() > 0 && line[0] == '#')
00032 continue;
00033 istringstream ls(line);
00034 string tag;
00035 ls >> tag;
00036 if (tag == "EDGE3"){
00037 int id1, id2;
00038 Vector6 p;
00039 Matrix6 m;
00040 ls >> id1 >> id2 >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw();
00041 if (overrideCovariances) {
00042 m = Matrix6::eye(1.0);
00043 } else {
00044 for (int i=0; i<6; i++)
00045 for (int j=i; j<6; j++) {
00046 ls >> m[i][j];
00047 if (i != j)
00048 m[j][i] = m[i][j];
00049 }
00050 }
00051
00052 LoadedEdge3D e;
00053 e.id1 = id1;
00054 e.id2 = id2;
00055 e.mean = Transformation3::fromVector(p);
00056 e.informationMatrix = m;
00057 edges.insert(e);
00058 }
00059
00060 else if (tag == "EDGE2") {
00061 int id1, id2;
00062 Vector6 p;
00063 Matrix6 m = Matrix6::eye(1.0) * 1e9;
00064 ls >> id1 >> id2 >> p.x() >> p.y() >> p.yaw();
00065 ls >> m[0][0] >> m[0][1] >> m[1][1] >> m[5][5] >> m[0][5] >> m[1][5];
00066 m[1][0] = m[0][1];
00067 m[5][0] = m[0][5];
00068 m[5][1] = m[1][5];
00069 LoadedEdge3D e;
00070 e.id1 = id1;
00071 e.id2 = id2;
00072 e.mean = Transformation3::fromVector(p);
00073 e.informationMatrix = m;
00074 edges.insert(e);
00075 }
00076 }
00077
00078 }
00079
00080 }