posegraph2d.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 "posegraph2d.h"
00018 
00019 #include <sstream>
00020 using namespace std;
00021 
00022 namespace AISNavigation {
00023 
00024   void PoseGraph2D::load(istream& is, bool overrideCovariances, std::vector <PoseGraph2D::Edge*>* orderedEdges){
00025     clear();
00026     if (! is)
00027       return;
00028     Vertex* previousVertex=0;
00029     if (orderedEdges)
00030       orderedEdges->clear();
00031     while(is){
00032       char buf[LINESIZE];
00033       is.getline(buf,LINESIZE);
00034       istringstream ls(buf);
00035       string tag;
00036       ls >> tag;
00037       if (tag=="VERTEX" || tag=="VERTEX2"){
00038         int id;
00039         PoseGraph2D::TransformationVectorType p;
00040         ls >> id >> p.x() >> p.y() >> p.z();
00041         PoseGraph2D::TransformationType t=PoseGraph2D::TransformationType::fromVector(p);
00042         PoseGraph2D::InformationType identity=PoseGraph2D::InformationType::eye(1.);
00043         PoseGraph2D::Vertex* v=addVertex(id,t,identity);
00044         if (! v) {
00045           cerr << "vertex " << id << " is already in the graph, reassigning "<<  endl;
00046           v=vertex(id);
00047           assert(v);
00048         } 
00049         v->transformation=t;
00050         v->localTransformation=t;
00051         previousVertex=v;
00052       } else if (tag=="EDGE" || tag=="EDGE2"){
00053         int id1, id2;
00054         PoseGraph2D::TransformationVectorType p;
00055         PoseGraph2D::InformationType m;
00056         ls >> id1 >> id2 >> p.x() >> p.y() >> p.z();
00057         if (overrideCovariances){
00058           m=PoseGraph2D::InformationType::eye(1.);
00059         } else {
00060           ls >> m[0][0] >> m[0][1] >> m [1][1]
00061             >> m[2][2] >> m[0][2] >> m [1][2];
00062           m[1][0]=m[0][1];
00063           m[2][0]=m[0][2];
00064           m[2][1]=m[1][2];
00065         }
00066         previousVertex=0;
00067         PoseGraph2D::Vertex* v1=vertex(id1);
00068         PoseGraph2D::Vertex* v2=vertex(id2);
00069         if (! v1 ) {
00070           cerr << "vertex " << id1 << " is not existing, cannot add edge (" << id1 << "," << id2 << ")" << endl; 
00071           continue;
00072         }
00073         if (! v2 ) {
00074           cerr << "vertex " << id2 << " is not existing, cannot add edge (" << id1 << "," << id2 << ")" << endl; 
00075           continue;
00076         }
00077         PoseGraph2D::TransformationType t=PoseGraph2D::TransformationType::fromVector(p);
00078         PoseGraph2D::Edge* e=addEdge(v1, v2,t ,m);
00079         if (! e){
00080           cerr << "error in adding edge " << id1 << "," << id2 << endl;
00081         } else {
00082           if (orderedEdges)
00083             orderedEdges->push_back(e);
00084         }
00085       }
00086     }
00087   }
00088 
00089   void PoseGraph2D::save(ostream& os, const PoseGraph2D::TransformationType& offset, int type, bool onlyMarked) const{
00090     for (VertexIDMap::const_iterator it=_vertices.begin(); it!=_vertices.end(); it++){
00091       const PoseGraph2D::Vertex* v=dynamic_cast<const PoseGraph2D::Vertex*>(it->second);
00092       PoseGraph2D::TransformationType t=v->transformation;
00093       switch(type){
00094         case 1: t=v->localTransformation; break;
00095       }
00096 
00097       t=offset*t;
00098       os << "VERTEX2 " 
00099         << v->id() << " " 
00100         << t.translation().x() << " "
00101         << t.translation().y() << " "
00102         << t.rotation() << endl;
00103 
00104     }
00105 
00106     for (int l = 0; l <= 1; ++l) { // separate writing of loop edges and sequential edges
00107       bool writeLoopEdges = l == 1;
00108       if (writeLoopEdges)
00109         os << "#LOOP EDGES" << endl;
00110       else
00111         os << "#SEQUENTIAL EDGES" << endl;
00112 
00113       for (EdgeSet::const_iterator it=_edges.begin(); it!=_edges.end(); it++){
00114         const PoseGraph2D::Edge* e=dynamic_cast<const PoseGraph2D::Edge*>(*it);
00115         if (onlyMarked && !e->_mark)
00116           continue;
00117         const PoseGraph2D::Vertex* v1=dynamic_cast<const PoseGraph2D::Vertex*>(e->from());
00118         const PoseGraph2D::Vertex* v2=dynamic_cast<const PoseGraph2D::Vertex*>(e->to());
00119         bool isLoop = abs(v1->id()-v2->id()) != 1;
00120         if (isLoop == writeLoopEdges) {
00121           os << "EDGE2 " << v1->id() << " " << v2->id() << " ";
00122           PoseGraph2D::TransformationVectorType p=e->mean().toVector();
00123           os << p.x() << " " << p.y() << " " << p.z() << " ";
00124           os << e->information()[0][0] << " "
00125             << e->information()[0][1] << " "
00126             << e->information()[1][1] << " "
00127             << e->information()[2][2] << " "
00128             << e->information()[0][2] << " "
00129             << e->information()[1][2] << endl;
00130         }
00131       }
00132     }
00133 
00134   }
00135 
00136   void PoseGraph2D::visualizeToStream(std::ostream& os) const
00137   {
00138     os << "set terminal x11 noraise" << endl;
00139     os << "set size ratio -1" << endl;
00140     os << "plot '-' w l "<< endl;
00141     saveAsGnuplot(os);
00142     os  << "e" << endl;
00143     os << flush;
00144   }
00145 
00146   void PoseGraph2D::saveAsGnuplot(ostream& os, bool onlyMarked) const
00147   {
00148     for (EdgeSet::const_iterator it=_edges.begin(); it!=_edges.end(); it++){
00149       const PoseGraph2D::Edge* e = reinterpret_cast<const PoseGraph2D::Edge*>(*it);
00150       if (onlyMarked && ! e->_mark)
00151         continue;
00152       const PoseGraph2D::Vertex* v1 = reinterpret_cast<const PoseGraph2D::Vertex*>(e->from());
00153       const PoseGraph2D::Vertex* v2 = reinterpret_cast<const PoseGraph2D::Vertex*>(e->to());
00154       os << v1->transformation.translation().x() << " " << v1->transformation.translation().y() << endl;
00155       os << v2->transformation.translation().x() << " " << v2->transformation.translation().y() << endl << endl;
00156     }
00157   }
00158 
00159 } // end namespace


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