00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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();
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
00082 char c = 0;
00083 while (cin.get(c)) {
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
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
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;
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') {
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*) ×tamp, sizeof(double));
00167 }
00168 else if (c == 'F') {
00169 switchedGraph = true;
00170 updateDisplayedGraph(poseGraph, graphIdx, nextIdx);
00171 graphIdx = nextIdx;
00172 }
00173 }
00174
00175
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
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
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
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 }