loadEdges3d.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 "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] == '#') // skip comment lines
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     // also load the 2D edges for evaluation purposes
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 } // end namespace


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