graph_viewer.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 // This file is part of HOG-Man.
00005 // 
00006 // HOG-Man is free software: you can redistribute it and/or modify
00007 // it under the terms of the GNU General Public License as published by
00008 // the Free Software Foundation, either version 3 of the License, or
00009 // (at your option) any later version.
00010 // 
00011 // HOG-Man is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU General Public License for more details.
00015 // 
00016 // You should have received a copy of the GNU General Public License
00017 // along with HOG-Man.  If not, see <http://www.gnu.org/licenses/>.
00018 
00019 #include <iostream>
00020 #include <string>
00021 #include <cassert>
00022 #include <sstream>
00023 
00024 #include "graph/posegraph3d.h"
00025 #include "pose_graph_vis3d.h"
00026 #include "qgl_graph_viewer.h"
00027 #include "main_widget.h"
00028 
00029 #include <qapplication.h>
00030 using namespace std;
00031 using namespace AISNavigation;
00032 
00033 struct HGraph
00034 {
00035   PoseGraph3D graph;
00036   std::vector<PoseGraph3DVis::HEdgeVector> hirarchy;
00037   void clear()
00038   {
00039     graph.clear();
00040     hirarchy.clear();
00041   }
00042 };
00043 
00044 static HGraph graphs[2];
00045 static bool s_drawNeeded = false;
00046 static pthread_mutex_t graphMutex = PTHREAD_MUTEX_INITIALIZER;
00047 static bool overrideCovariances = false;
00048 static bool keepRootAtZero = true;
00049 
00050 void updateDisplayedGraph(PoseGraph3DVis* poseGraph, int graphIdx, int nextIdx)
00051 {
00052   pthread_mutex_lock( &graphMutex );
00053   s_drawNeeded = true;
00054   graphs[graphIdx].clear(); // clear the old graph
00055   PoseGraph3D* graph = &graphs[nextIdx].graph;
00056   if (keepRootAtZero) {
00057     PoseGraph3D::Vertex* rootVertex = static_cast<PoseGraph3D::Vertex*>(graph->vertices().begin()->second);
00058     Transformation3 rootToZero = rootVertex->transformation.inverse();
00059     for (PoseGraph3D::VertexIDMap::iterator it = graph->vertices().begin(); it != graph->vertices().end(); ++it) {
00060       PoseGraph3D::Vertex* v = static_cast<PoseGraph3D::Vertex*>(it->second);
00061       v->transformation = rootToZero * v->transformation;
00062     }
00063   }
00064   poseGraph->setGraph(graph);
00065   poseGraph->setHirarchy(&graphs[nextIdx].hirarchy);
00066   pthread_mutex_unlock( &graphMutex );
00067 }
00068 
00072 void* readStdinThread(void* arg)
00073 {
00074   bool switchedGraph = false;
00075   PoseGraph3DVis* poseGraph = static_cast<PoseGraph3DVis*>(arg);
00076   string token, line;
00077   stringstream auxStream;
00078   int graphIdx = 0;
00079   double timestamp = 0.0;
00080 
00081   // read stdin data
00082   char c = 0;
00083   while (cin.get(c)) { // if cin is not valid, we are done
00084     int nextIdx = (graphIdx + 1) & 1;
00085     PoseGraph3D& nextGraph = graphs[nextIdx].graph;
00086     std::vector<PoseGraph3DVis::HEdgeVector>& nextHirarchy = graphs[nextIdx].hirarchy;
00087 
00088     if (c == 'V') {
00089       switchedGraph = false;
00090       // read vertex
00091       int id;
00092       Transformation3 t;
00093       static Matrix6 identity = Matrix6::eye(1.);
00094       cin.read((char*)&id, sizeof(int));
00095       cin.read((char*)&t.translation()[0], sizeof(double));
00096       cin.read((char*)&t.translation()[1], sizeof(double));
00097       cin.read((char*)&t.translation()[2], sizeof(double));
00098       cin.read((char*)&t.rotation().w(), sizeof(double));
00099       cin.read((char*)&t.rotation().x(), sizeof(double));
00100       cin.read((char*)&t.rotation().y(), sizeof(double));
00101       cin.read((char*)&t.rotation().z(), sizeof(double));
00102       PoseGraph3D::Vertex* v = nextGraph.addVertex(id, t, identity);
00103       if (! v) {
00104         cerr << "vertex " << id << " is already in the graph, reassigning "<<  endl;
00105         v = nextGraph.vertex(id);
00106         assert(v);
00107       } 
00108       v->transformation = t;
00109       v->localTransformation = t;
00110     }
00111     else if (c == 'E') {
00112       switchedGraph = false;
00113       // read edge
00114       int id1, id2;
00115       Transformation3 t;
00116       Matrix6 m = Matrix6::eye(1.);
00117       cin.read((char*)&id1, sizeof(int));
00118       cin.read((char*)&id2, sizeof(int));
00119       cin.read((char*)&t.translation()[0], sizeof(double));
00120       cin.read((char*)&t.translation()[1], sizeof(double));
00121       cin.read((char*)&t.translation()[2], sizeof(double));
00122       cin.read((char*)&t.rotation().w(), sizeof(double));
00123       cin.read((char*)&t.rotation().x(), sizeof(double));
00124       cin.read((char*)&t.rotation().y(), sizeof(double));
00125       cin.read((char*)&t.rotation().z(), sizeof(double));
00126       if (overrideCovariances) {
00127         double dummy; // just read over the information matrix
00128         for (int i=0; i<6; i++)
00129           for (int j=i; j<6; j++)
00130             cin.read((char*)&dummy, sizeof(double));
00131       } else {
00132         for (int i=0; i<6; i++)
00133           for (int j=i; j<6; j++) {
00134             cin.read((char*)&m[i][j], sizeof(double));
00135             if (i != j)
00136               m[j][i] = m[i][j];
00137           }
00138       }
00139 
00140       PoseGraph3D::Vertex* v1 = nextGraph.vertex(id1);
00141       PoseGraph3D::Vertex* v2 = nextGraph.vertex(id2);
00142       if (! v1 ) {
00143         cerr << "vertex " << id1 << " is not existing, cannot add edge (" << id1 << "," << id2 << ")" << endl; 
00144         continue;
00145       }
00146       if (! v2 ) {
00147         cerr << "vertex " << id2 << " is not existing, cannot add edge (" << id1 << "," << id2 << ")" << endl; 
00148         continue;
00149       }
00150       PoseGraph3D::Edge* e = nextGraph.addEdge(v1, v2, t, m);
00151       if (! e){
00152         cerr << "error in adding edge " << id1 << "," << id2 << endl;
00153       } 
00154     }
00155     else if (c == 'H') { // read hirarchy
00156       PoseGraph3DVis::HEdge e;
00157       int l;
00158       cin.read((char*) &l, sizeof(int));
00159       cin.read((char*) &e.id1, sizeof(int));
00160       cin.read((char*) &e.id2, sizeof(int));
00161       if (l + 1 > (int)nextHirarchy.size())
00162         nextHirarchy.resize(l+1);
00163       nextHirarchy[l].push_back(e);
00164     }
00165     else if (c == 'T') {
00166       cin.read((char*) &timestamp, sizeof(double));
00167     }
00168     else if (c == 'F') { // finished reading... switch the graph display
00169       switchedGraph = true;
00170       updateDisplayedGraph(poseGraph, graphIdx, nextIdx);
00171       graphIdx = nextIdx;
00172     }
00173   }
00174 
00175   // done reading from stdin, check wether there was an END read
00176   if (!switchedGraph) {
00177     int nextIdx = (graphIdx + 1) & 1;
00178     updateDisplayedGraph(poseGraph, graphIdx, nextIdx);
00179     graphIdx = nextIdx;
00180   }
00181 
00182   return 0;
00183 }
00184 
00185 void printUsage(const char* progName)
00186 {
00187   cout << "Usage: " << progName << " [options]" << endl << endl;
00188   cout << "Options:" << endl;
00189   cout << "-------------------------------------------" << endl;
00190   cout << "-l <filename>  load a graph from the hard disk" << endl;
00191   cout << "-b             file from disk is binary" << endl;
00192   cout << "-o             overwrite covariances" << endl;
00193   cout << "-h             this help" << endl;
00194 }
00195 
00196 int main(int argc, char** argv)
00197 {
00198   string localFilename = "";
00199   bool binary = false;
00200 
00201   for (char c; (c = getopt(argc, argv, "bol:h")) != -1; ) {
00202     switch (c) {
00203       case 'b':
00204         binary = true;
00205         break;
00206       case 'o':
00207         overrideCovariances = true;
00208         break;
00209       case 'l':
00210         localFilename = optarg;
00211         break;
00212       case 'h':
00213         printUsage(argv[0]);
00214         return 0;
00215       default:
00216         printUsage(argv[0]);
00217         return 1;
00218     }
00219   }
00220   if (optind != argc) {
00221     printUsage(argv[0]);
00222     return 1;
00223   }
00224 
00225   // starting up the QApplication
00226   QApplication qapp(argc, argv);
00227   MainWidget mw;
00228   QGLGraphViewer& viewer = *mw.viewer;
00229   viewer.graph.setUseDrawList(true);
00230   viewer.graph.setGraph(&graphs[0].graph);
00231   viewer.graph.setHirarchy(&graphs[0].hirarchy);
00232   mw.show();
00233 
00234   // start the thread that reads stdin
00235   pthread_t thread1;
00236   int thread_status = pthread_create( &thread1, NULL, readStdinThread, static_cast<void*>(&viewer.graph));
00237   if (thread_status != 0) {
00238     cerr << "unable to create stdin thread" << endl;
00239   }
00240 
00241   // just load a graph and display this graph
00242   if (localFilename.size()) {
00243     cout << "Loading file " << localFilename << " ... " << flush;
00244     ifstream ifs(localFilename.c_str());
00245     if (!ifs) {
00246       cerr << "Unable to open " << localFilename << endl;
00247       return 1;
00248     }
00249     if (!binary)
00250       graphs[0].graph.load(ifs);
00251     else
00252       graphs[0].graph.loadBinary(ifs);
00253     cout << "done." << endl;
00254   }
00255 
00256   while (mw.isVisible()) {
00257     pthread_mutex_lock( &graphMutex );
00258     if (s_drawNeeded) {
00259       s_drawNeeded = false;
00260       viewer.updateGL();
00261     }
00262     qapp.processEvents();
00263     pthread_mutex_unlock( &graphMutex );
00264     usleep(10000);
00265   }
00266 
00267   return 0;
00268 }


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