posegraph3d.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 "posegraph3d.h"
00018 
00019 #include <cassert>
00020 #include <queue>
00021 #include <iostream>
00022 #include <iomanip>
00023 #include <cstdlib>
00024 #include <sys/time.h>
00025 
00026 #include <sstream>
00027 #include "hogman_minimal/stuff/os_specific.h"
00028 
00029 using namespace std;
00030 
00031 namespace AISNavigation{
00032   void PoseGraph3D::load(istream& is, bool overrideCovariances, std::vector <PoseGraph3D::Edge*>* orderedEdges)
00033   {
00034     clear();
00035     if (! is)
00036       return;
00037     Vertex* previousVertex=0;
00038     if (orderedEdges)
00039       orderedEdges->clear();
00040     string line;
00041     while (getline(is, line)) {
00042       if (line.size() == 0 || line[0] == '#') // skip comment lines
00043         continue;
00044       istringstream ls(line);
00045       string tag;
00046       ls >> tag;
00047       if (tag == "VERTEX3"){
00048         int id;
00049         Vector6 p;
00050         ls >> id;
00051         for (int i = 0; i < 6; ++i)
00052           ls >> p[i];
00053         Transformation3 t = Transformation3::fromVector(p);
00054         Matrix6 identity = Matrix6::eye(1.0);
00055         Vertex* v=addVertex(id, t, identity);
00056         if (! v) {
00057           cerr << "vertex " << id << " is already in the graph, reassigning "<<  endl;
00058           v = vertex(id);
00059           assert(v);
00060         } 
00061         v->transformation = t;
00062         v->localTransformation = t;
00063         previousVertex = v;
00064       } else if (tag == "EDGE3"){
00065         int id1, id2;
00066         Vector6 p;
00067         ls >> id1 >> id2;
00068         for (int i = 0; i < 6; ++i)
00069           ls >> p[i];
00070         Matrix6 m;
00071         if (overrideCovariances) {
00072           m = Matrix6::eye(1.0);
00073         } else {
00074           for (int i=0; i<6; i++)
00075             for (int j=i; j<6; j++) {
00076               ls >> m[i][j];
00077               if (i != j)
00078                 m[j][i] = m[i][j];
00079             }
00080         }
00081         previousVertex=0;
00082         Vertex* v1=vertex(id1);
00083         Vertex* v2=vertex(id2);
00084         if (! v1 ) {
00085           cerr << "vertex " << id1 << " is not existing, cannot add edge (" << id1 << "," << id2 << ")" << endl; 
00086           continue;
00087         }
00088         if (! v2 ) {
00089           cerr << "vertex " << id2 << " is not existing, cannot add edge (" << id1 << "," << id2 << ")" << endl; 
00090           continue;
00091         }
00092         Transformation3 t = Transformation3::fromVector(p);
00093         Edge* e = addEdge(v1, v2, t, m);
00094         if (! e){
00095           cerr << "error in adding edge " << id1 << "," << id2 << endl;
00096         } else {
00097           if (orderedEdges)
00098             orderedEdges->push_back(e);
00099         }
00100       }
00101     }
00102   }
00103   
00104   void PoseGraph3D::save(ostream& os, const Transformation3& offset, int type, bool onlyMarked) const
00105   {
00106     //os << setprecision(3);
00107 
00108     for (VertexIDMap::const_iterator it = _vertices.begin(); it != _vertices.end(); ++it) {
00109       const Vertex* v=dynamic_cast<const PoseGraph3D::Vertex*>(it->second);
00110       Transformation3 t = v->transformation;
00111       switch (type) {
00112         case 1: t = v->localTransformation; break;
00113       }
00114 
00115       t=offset*t;
00116       Vector6 pose =  t.toVector();
00117       os << "VERTEX3 " << v->id() << " " 
00118        << pose.x() << " " << pose.y() << " " << pose.z() << " "
00119        << pose[3] << " " << pose[4] << " " << pose[5] << endl;
00120 
00121     }
00122 
00123     for (int l = 0; l <= 1; ++l) { // separate writing of loop edges and sequential edges
00124       bool writeLoopEdges = l == 1;
00125       if (writeLoopEdges)
00126         os << "#LOOP EDGES" << endl;
00127       else
00128         os << "#SEQUENTIAL EDGES" << endl;
00129       for (EdgeSet::const_iterator it = _edges.begin(); it != _edges.end(); ++it) {
00130         const Edge* e = dynamic_cast<const PoseGraph3D::Edge*>(*it);
00131         if (onlyMarked && !e->_mark)
00132           continue;
00133         const Vertex* v1 = dynamic_cast<const PoseGraph3D::Vertex*>(e->from());
00134         const Vertex* v2 = dynamic_cast<const PoseGraph3D::Vertex*>(e->to());
00135         bool isLoop = abs(v1->id()-v2->id()) != 1;
00136         if (isLoop == writeLoopEdges) {
00137           os << "EDGE3 " << v1->id() << " " << v2->id() << " ";
00138           Vector6 p = e->mean().toVector();
00139           os << p.x() << " " << p.y() << " " << p.z() << " " << p[3] << " " << p[4] << " " << p[5];
00140           for (int i=0; i<6; i++)
00141             for (int j=i; j<6; j++)
00142               os << " " << e->information()[i][j];
00143           os << endl;
00144         }
00145       }
00146     }
00147   }
00148 
00149 void PoseGraph3D::saveGnuplot(std::ostream& os, const Transformation3& offset, bool onlyMarked) const
00150 {
00151   for (EdgeSet::const_iterator it = _edges.begin(); it != _edges.end(); ++it) {
00152     const Edge* e = dynamic_cast<const PoseGraph3D::Edge*>(*it);
00153     if (onlyMarked && !e->_mark)
00154       continue;
00155     const Vertex* v1 = dynamic_cast<const PoseGraph3D::Vertex*>(e->from());
00156     const Vertex* v2 = dynamic_cast<const PoseGraph3D::Vertex*>(e->to());
00157     Vector6 v1p = (offset * v1->transformation).toVector();
00158     Vector6 v2p = (offset * v2->transformation).toVector();
00159     os << v1p.x() << " " << v1p.y() << " " << v1p.z() << " "
00160        << v1p[3] << " " << v1p[4] << " " << v1p[5] <<endl;
00161     os << v2p.x() << " " << v2p.y() << " " << v2p.z() << " "
00162        << v2p[3] << " " << v2p[4] << " " << v2p[5] <<endl;
00163     os << endl << endl;
00164   }
00165 
00166 }
00167 
00168 void PoseGraph3D::loadBinary(std::istream& is, bool overrideCovariances, std::vector <PoseGraph3D::Edge*> *orderedEdges)
00169 {
00170   clear();
00171   if (! is)
00172     return;
00173   if (orderedEdges)
00174     orderedEdges->clear();
00175   char c = 0;
00176   while (is.get(c)) {
00177     if (c == 'V'){
00178       int id;
00179       Transformation3 t;
00180       Matrix6 identity = Matrix6::eye(1.0);
00181       is.read((char*)&id, sizeof(int));
00182       is.read((char*)&t.translation().x(), sizeof(double));
00183       is.read((char*)&t.translation().y(), sizeof(double));
00184       is.read((char*)&t.translation().z(), sizeof(double));
00185       is.read((char*)&t.rotation().w(), sizeof(double));
00186       is.read((char*)&t.rotation().x(), sizeof(double));
00187       is.read((char*)&t.rotation().y(), sizeof(double));
00188       is.read((char*)&t.rotation().z(), sizeof(double));
00189       Vertex* v=addVertex(id, t, identity);
00190       if (! v) {
00191         cerr << "vertex " << id << " is already in the graph, reassigning "<<  endl;
00192         v = vertex(id);
00193         assert(v);
00194       } 
00195       v->transformation = t;
00196       v->localTransformation = t;
00197     } else if (c == 'E'){
00198       int id1, id2;
00199       Transformation3 t;
00200       Matrix6 m(6, 6);
00201       is.read((char*)&id1, sizeof(int));
00202       is.read((char*)&id2, sizeof(int));
00203       is.read((char*)&t.translation().x(), sizeof(double));
00204       is.read((char*)&t.translation().y(), sizeof(double));
00205       is.read((char*)&t.translation().z(), sizeof(double));
00206       is.read((char*)&t.rotation().w(), sizeof(double));
00207       is.read((char*)&t.rotation().x(), sizeof(double));
00208       is.read((char*)&t.rotation().y(), sizeof(double));
00209       is.read((char*)&t.rotation().z(), sizeof(double));
00210       if (overrideCovariances) {
00211         double dummy; // just read over the information matrix
00212         for (int i=0; i<6; i++)
00213           for (int j=i; j<6; j++)
00214             is.read((char*)&dummy, sizeof(double));
00215       } else {
00216         for (int i=0; i<6; i++)
00217           for (int j=i; j<6; j++) {
00218             is.read((char*)&m[i][j], sizeof(double));
00219             if (i != j)
00220               m[j][i] = m[i][j];
00221           }
00222       }
00223 
00224       Vertex* v1=vertex(id1);
00225       Vertex* v2=vertex(id2);
00226       if (! v1 ) {
00227         cerr << "vertex " << id1 << " is not existing, cannot add edge (" << id1 << "," << id2 << ")" << endl; 
00228         continue;
00229       }
00230       if (! v2 ) {
00231         cerr << "vertex " << id2 << " is not existing, cannot add edge (" << id1 << "," << id2 << ")" << endl; 
00232         continue;
00233       }
00234       Edge* e = addEdge(v1, v2, t, m);
00235       if (! e){
00236         cerr << "error in adding edge " << id1 << "," << id2 << endl;
00237       } else {
00238         if (orderedEdges)
00239           orderedEdges->push_back(e);
00240       }
00241     }
00242   }
00243 
00244 }
00245 
00246 void PoseGraph3D::saveBinary(std::ostream& os, int type, bool onlyMarked) const
00247 {
00248   for (VertexIDMap::const_iterator it = _vertices.begin(); it != _vertices.end(); ++it) {
00249     const Vertex* v=dynamic_cast<const PoseGraph3D::Vertex*>(it->second);
00250     Transformation3 t = v->transformation;
00251     switch (type) {
00252       case 1: t = v->localTransformation; break;
00253     }
00254 
00255     os.put('V');
00256     int id = v->id();
00257     os.write((char*)&id, sizeof(int));
00258     os.write((char*)&t.translation().x(), sizeof(double));
00259     os.write((char*)&t.translation().y(), sizeof(double));
00260     os.write((char*)&t.translation().z(), sizeof(double));
00261     os.write((char*)&t.rotation().w(), sizeof(double));
00262     os.write((char*)&t.rotation().x(), sizeof(double));
00263     os.write((char*)&t.rotation().y(), sizeof(double));
00264     os.write((char*)&t.rotation().z(), sizeof(double));
00265   }
00266 
00267   for (EdgeSet::const_iterator it = _edges.begin(); it != _edges.end(); ++it) {
00268     const Edge* e = dynamic_cast<const PoseGraph3D::Edge*>(*it);
00269     if (onlyMarked && !e->_mark)
00270       continue;
00271     const Vertex* v1 = dynamic_cast<const PoseGraph3D::Vertex*>(e->from());
00272     const Vertex* v2 = dynamic_cast<const PoseGraph3D::Vertex*>(e->to());
00273     os.put('E');
00274     int id1 = v1->id();
00275     int id2 = v2->id();
00276     os.write((char*)&id1, sizeof(int));
00277     os.write((char*)&id2, sizeof(int));
00278     os.write((char*)&e->mean().translation().x(), sizeof(double));
00279     os.write((char*)&e->mean().translation().y(), sizeof(double));
00280     os.write((char*)&e->mean().translation().z(), sizeof(double));
00281     os.write((char*)&e->mean().rotation().w(), sizeof(double));
00282     os.write((char*)&e->mean().rotation().x(), sizeof(double));
00283     os.write((char*)&e->mean().rotation().y(), sizeof(double));
00284     os.write((char*)&e->mean().rotation().z(), sizeof(double));
00285     for (int i=0; i<6; i++)
00286       for (int j=i; j<6; j++)
00287         os.write((char*)&e->information()[i][j], sizeof(double));
00288   }
00289 }
00290 
00291 void PoseGraph3D::visualizeToStream(std::ostream& os) const
00292 {
00293   struct timeval now;
00294   gettimeofday(&now, 0);
00295   double curTime = now.tv_sec + now.tv_usec*1e-6;
00296   saveBinary(os);
00297   os << "T"; // write time spend in optimization, used to record video in realtime
00298   os.write((char*)&curTime, sizeof(double));
00299   os << "F" << flush;
00300 }
00301 
00302 } // end namespace


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