00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
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] == '#')
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
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) {
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;
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";
00298 os.write((char*)&curTime, sizeof(double));
00299 os << "F" << flush;
00300 }
00301
00302 }