posegraph3.cpp
Go to the documentation of this file.
00001 /**********************************************************************
00002  *
00003  * This source code is part of the Tree-based Network Optimizer (TORO)
00004  *
00005  * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
00006  *                         Slawomir Grzonka, and Wolfram Burgard
00007  *
00008  * TORO is licences under the Common Creative License,
00009  * Attribution-NonCommercial-ShareAlike 3.0
00010  *
00011  * You are free:
00012  *   - to Share - to copy, distribute and transmit the work
00013  *   - to Remix - to adapt the work
00014  *
00015  * Under the following conditions:
00016  *
00017  *   - Attribution. You must attribute the work in the manner specified
00018  *     by the author or licensor (but not in any way that suggests that
00019  *     they endorse you or your use of the work).
00020  *  
00021  *   - Noncommercial. You may not use this work for commercial purposes.
00022  *  
00023  *   - Share Alike. If you alter, transform, or build upon this work,
00024  *     you may distribute the resulting work only under the same or
00025  *     similar license to this one.
00026  * * Any of the above conditions can be waived if you get permission
00027  * from the copyright holder.  Nothing in this license impairs or
00028  * restricts the author's moral rights.
00029  *
00030  * TORO is distributed in the hope that it will be useful,
00031  * but WITHOUT ANY WARRANTY; without even the implied 
00032  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
00033  * PURPOSE.  
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(); /* clears the end-of-file and error flags */
00094   is.seekg(0, ios::beg);
00095 
00096   //bool edgesOk=true;
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           //edgesOk=false;
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           //edgesOk=false;
00144         }
00145       }
00146     }
00147   }
00148   return true;
00149   //return edgesOk;
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   // here we assume uniform covariances, and we neglect the transofrmation
00294   // induced by the Jacobian when reverting the link
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   // all the edges of v2 become outgoing
00311   for (EdgeList::iterator it=v2->edges.begin(); it!=v2->edges.end(); it++){
00312     if ( (*it)->v1!=v2 )
00313       revertEdge(*it);
00314   }
00315 
00316  // all the edges of v1 become outgoing
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   //Transformation iT12=T12.inv();
00330 
00331   //compute the marginal information of the nodes in the path v1-v2-v*
00332   for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
00333     Edge* e2=*it2;
00334     if (e2->v1==v2){ //edge leaving v2
00335       Transformation T2x=e2->transformation;
00336       Pose           p2x=T2x.toPoseType();
00337       InformationMatrix I2x=e2->informationMatrix;
00338       CovarianceMatrix  C2x=I2x.inv();
00339       
00340       //compute the estimate of the vertex based on the path v1-v2-vx
00341       
00342       //Transformation tr=iT12*T2x;
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   //all the edges leaving v1 and leaving v2 and leading to the same point are merged
00357   std::list<Transformation> tList;
00358   std::list<InformationMatrix> iList;
00359   std::list<Vertex*> vList;
00360 
00361   //others are transformed and added to v1
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     // FIXME
00374     // edges leading to the same node are ignored
00375     // should be merged
00376     if (e1x && e2x){
00377       // here goes something for mergin the constraints, according to the information matrices.
00378       // in 3D it is a nightmare, so i postpone this, and i simply ignore the redundant constraints.
00379       // the resultng system is overconfident
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 }; //namespace AISNavigation


rtabmap
Author(s): Mathieu Labbe
autogenerated on Fri Aug 28 2015 12:51:32