Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00042 #include "posegraph3.hh"
00043 #include <fstream>
00044 #include <sstream>
00045 #include <string>
00046
00047 using namespace std;
00048
00049 namespace AISNavigation {
00050
00051 #define LINESIZE 81920
00052
00053
00054 #define DEBUG(i) \
00055 if (verboseLevel>i) cerr
00056
00057
00058 bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool twoDimensions){
00059 clear();
00060 ifstream is(filename);
00061 if (!is)
00062 return false;
00063
00064 while(is){
00065 char buf[LINESIZE];
00066 is.getline(buf,LINESIZE);
00067 istringstream ls(buf);
00068 string tag;
00069 ls >> tag;
00070
00071 if (twoDimensions){
00072 if (tag=="VERTEX"){
00073 int id;
00074 Pose p(0.,0.,0.,0.,0.,0.);
00075 ls >> id >> p.x() >> p.y() >> p.yaw();
00076 TreePoseGraph3::Vertex* v=addVertex(id,p);
00077 if (v){
00078 v->transformation=Transformation(p);
00079 }
00080 }
00081 } else {
00082 if (tag=="VERTEX3"){
00083 int id;
00084 Pose p;
00085 ls >> id >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw();
00086 TreePoseGraph3::Vertex* v=addVertex(id,p);
00087 if (v){
00088 v->transformation=Transformation(p);
00089 }
00090 }
00091 }
00092 }
00093 is.clear();
00094 is.seekg(0, ios::beg);
00095
00096
00097 while(is){
00098 char buf[LINESIZE];
00099 is.getline(buf,LINESIZE);
00100 istringstream ls(buf);
00101 string tag;
00102 ls >> tag;
00103
00104 if (twoDimensions){
00105 if (tag=="EDGE"){
00106 int id1, id2;
00107 Pose p(0.,0.,0.,0.,0.,0.);
00108 InformationMatrix m;
00109 ls >> id1 >> id2 >> p.x() >> p.y() >> p.yaw();
00110 m=DMatrix<double>::I(6);
00111 if (! overrideCovariances){
00112 ls >> m[0][0] >> m[0][1] >> m[1][1] >> m[2][2] >> m[0][2] >> m[1][2];
00113 m[2][0]=m[0][2]; m[2][1]=m[1][2]; m[1][0]=m[0][1];
00114 }
00115
00116 TreePoseGraph3::Vertex* v1=vertex(id1);
00117 TreePoseGraph3::Vertex* v2=vertex(id2);
00118 Transformation t(p);
00119 if (!addEdge(v1, v2,t ,m)){
00120 cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
00121 cerr << "edge=" << id1 <<" -> " << id2 << endl;
00122
00123 }
00124 }
00125 } else {
00126 if (tag=="EDGE3"){
00127 int id1, id2;
00128 Pose p;
00129 InformationMatrix m;
00130 ls >> id1 >> id2 >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw();
00131 m=DMatrix<double>::I(6);
00132 if (! overrideCovariances){
00133 for (int i=0; i<6; i++)
00134 for (int j=i; j<6; j++)
00135 ls >> m[i][j];
00136 }
00137 TreePoseGraph3::Vertex* v1=vertex(id1);
00138 TreePoseGraph3::Vertex* v2=vertex(id2);
00139 Transformation t(p);
00140 if (!addEdge(v1, v2,t ,m)){
00141 cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
00142 cerr << "edge=" << id1 <<" -> " << id2 << endl;
00143
00144 }
00145 }
00146 }
00147 }
00148 return true;
00149
00150 }
00151
00152 bool TreePoseGraph3::loadEquivalences(const char* filename){
00153 ifstream is(filename);
00154 if (!is)
00155 return false;
00156 EdgeList suppressed;
00157 uint equivCount=0;
00158 while (is){
00159 char buf[LINESIZE];
00160 is.getline(buf, LINESIZE);
00161 istringstream ls(buf);
00162 string tag;
00163 ls >> tag;
00164 if (tag=="EQUIV"){
00165 int id1, id2;
00166 ls >> id1 >> id2;
00167 Edge* e=edge(id1,id2);
00168 if (!e)
00169 e=edge(id2,id1);
00170 if (e){
00171 suppressed.push_back(e);
00172 equivCount++;
00173 }
00174 }
00175 }
00176 for (EdgeList::iterator it=suppressed.begin(); it!=suppressed.end(); it++){
00177 Edge* e=*it;
00178 if (e->v1->id > e->v2->id)
00179 revertEdge(e);
00180 collapseEdge(e);
00181 }
00182 return true;
00183 }
00184
00185 bool TreePoseGraph3::saveGnuplot(const char* filename){
00186 ofstream os(filename);
00187 if (!os)
00188 return false;
00189 for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
00190 TreePoseGraph3::Vertex* v=it->second;
00191 v->pose=v->transformation.toPoseType();
00192 }
00193 for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
00194 const TreePoseGraph3::Edge * e=it->second;
00195 const Vertex* v1=e->v1;
00196 const Vertex* v2=e->v2;
00197
00198 os << v1->pose.x() << " " << v1->pose.y() << " " << v1->pose.z() << " "
00199 << v1->pose.roll() << " " << v1->pose.pitch() << " " << v1->pose.yaw() <<endl;
00200 os << v2->pose.x() << " " << v2->pose.y() << " " << v2->pose.z() << " "
00201 << v2->pose.roll() << " " << v2->pose.pitch() << " " << v2->pose.yaw() <<endl;
00202 os << endl << endl;
00203 }
00204 return true;
00205
00206 }
00207
00208 bool TreePoseGraph3::save(const char* filename){
00209 ofstream os(filename);
00210 if (!os)
00211 return false;
00212
00213 for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
00214 TreePoseGraph3::Vertex* v=it->second;
00215 v->pose=v->transformation.toPoseType();
00216
00217 os << "VERTEX3 "
00218 << v->id << " "
00219 << v->pose.x() << " "
00220 << v->pose.y() << " "
00221 << v->pose.z() << " "
00222 << v->pose.roll() << " "
00223 << v->pose.pitch() << " "
00224 << v->pose.yaw() << endl;
00225 }
00226 for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
00227 const TreePoseGraph3::Edge * e=it->second;
00228 os << "EDGE3 " << e->v1->id << " " << e->v2->id << " ";
00229 Pose p=e->transformation.toPoseType();
00230 os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << " ";
00231 for (int i=0; i<6; i++)
00232 for (int j=i; j<6; j++)
00233 os << e->informationMatrix[i][j] << " ";
00234 os << endl;
00235 }
00236 return true;
00237 }
00238
00241 struct IdPrinter{
00242 IdPrinter(std::ostream& _os):os(_os){}
00243 std::ostream& os;
00244 void perform(TreePoseGraph3::Vertex* v){
00245 std::cout << "(" << v->id << "," << v->level << ")" << endl;
00246 }
00247 };
00248
00249 void TreePoseGraph3::printDepth( std::ostream& os ){
00250 IdPrinter ip(os);
00251 treeDepthVisit(ip, root);
00252 }
00253
00254 void TreePoseGraph3::printWidth( std::ostream& os ){
00255 IdPrinter ip(os);
00256 treeBreadthVisit(ip);
00257 }
00258
00263 struct PosePropagator{
00264 void perform(TreePoseGraph3::Vertex* v){
00265 if (!v->parent)
00266 return;
00267 TreePoseGraph3::Transformation tParent(v->parent->transformation);
00268 TreePoseGraph3::Transformation tNode=tParent*v->parentEdge->transformation;
00269
00270 assert(v->parentEdge->v1==v->parent);
00271 assert(v->parentEdge->v2==v);
00272 v->transformation=tNode;
00273 }
00274 };
00275
00276 void TreePoseGraph3::initializeOnTree(){
00277 PosePropagator pp;
00278 treeDepthVisit(pp, root);
00279 }
00280
00281
00282 void TreePoseGraph3::printEdgesStat(std::ostream& os){
00283 for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
00284 const TreePoseGraph3::Edge * e=it->second;
00285 os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
00286 Pose p=e->transformation.toPoseType();
00287 os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << endl;
00288 os << " top=" << e->top->id << " length=" << e->length << endl;
00289 }
00290 }
00291
00292 void TreePoseGraph3::revertEdgeInfo(Edge* e){
00293
00294
00295 e->transformation=e->transformation.inv();
00296 };
00297
00298 void TreePoseGraph3::initializeFromParentEdge(Vertex* v){
00299 Transformation tp=Transformation(v->parent->pose)*v->parentEdge->transformation;
00300 v->transformation=tp;
00301 v->pose=tp.toPoseType();
00302 v->parameters=v->parentEdge->transformation;
00303 }
00304
00305
00306 void TreePoseGraph3::collapseEdge(Edge* e){
00307 Vertex* v1=e->v1;
00308 Vertex* v2=e->v2;
00309
00310
00311 for (EdgeList::iterator it=v2->edges.begin(); it!=v2->edges.end(); it++){
00312 if ( (*it)->v1!=v2 )
00313 revertEdge(*it);
00314 }
00315
00316
00317 for (EdgeList::iterator it=v1->edges.begin(); it!=v1->edges.end(); it++){
00318 if ( (*it)->v1!=v1 )
00319 revertEdge(*it);
00320 }
00321
00322 assert(e->v1==v1);
00323
00324 InformationMatrix I12=e->informationMatrix;
00325 CovarianceMatrix C12=I12.inv();
00326 Transformation T12=e->transformation;
00327 Pose p12=T12.toPoseType();
00328
00329
00330
00331
00332 for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
00333 Edge* e2=*it2;
00334 if (e2->v1==v2){
00335 Transformation T2x=e2->transformation;
00336 Pose p2x=T2x.toPoseType();
00337 InformationMatrix I2x=e2->informationMatrix;
00338 CovarianceMatrix C2x=I2x.inv();
00339
00340
00341
00342
00343
00344 CovarianceMatrix CM=C2x;
00345
00346
00347 Transformation T1x_pred=T12*e2->transformation;
00348 Covariance C1x_pred=C12+C2x;
00349 InformationMatrix I1x_pred=C1x_pred.inv();
00350
00351 e2->transformation=T1x_pred;
00352 e2->informationMatrix=I1x_pred;
00353 }
00354 }
00355
00356
00357 std::list<Transformation> tList;
00358 std::list<InformationMatrix> iList;
00359 std::list<Vertex*> vList;
00360
00361
00362 for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
00363 Edge* e1x=0;
00364 Edge* e2x=0;
00365 if ( ((*it2)->v1!=v1)){
00366 e2x=*it2;
00367 for (EdgeList::iterator it1=v1->edges.begin(); it1!=v1->edges.end(); it1++){
00368 if ((*it1)->v2==(*it2)->v2)
00369 e1x=*it1;
00370 }
00371
00372 }
00373
00374
00375
00376 if (e1x && e2x){
00377
00378
00379
00380 }
00381 if (!e1x && e2x){
00382 tList.push_back(e2x->transformation);
00383 iList.push_back(e2x->informationMatrix);
00384 vList.push_back(e2x->v2);
00385 }
00386 }
00387 removeVertex(v2->id);
00388
00389 std::list<Transformation>::iterator t=tList.begin();
00390 std::list<InformationMatrix>::iterator i=iList.begin();
00391 std::list<Vertex*>::iterator v=vList.begin();
00392 while (i!=iList.end()){
00393 addEdge(v1,*v,*t,*i);
00394 i++;
00395 t++;
00396 v++;
00397 }
00398 }
00399
00400 void TreePoseGraph3::recomputeAllTransformations(){
00401 TransformationPropagator tp;
00402 treeDepthVisit(tp,root);
00403 }
00404
00405 };