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) if (verboseLevel>i) cerr
00055 
00056 
00057 bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool twoDimensions){
00058   clear();
00059   ifstream is(filename);
00060   if (!is)
00061     return false;
00062 
00063   while(is){
00064     char buf[LINESIZE];
00065     is.getline(buf,LINESIZE);
00066     istringstream ls(buf);
00067     string tag;
00068     ls >> tag;
00069     
00070     if (twoDimensions){
00071       if (tag=="VERTEX"){
00072         int id;
00073         Pose p(0.,0.,0.,0.,0.,0.);
00074         ls >> id >> p.x() >> p.y() >> p.yaw(); 
00075         TreePoseGraph3::Vertex* v=addVertex(id,p);
00076         if (v){
00077           v->transformation=Transformation(p);
00078         }
00079       }
00080     } else {
00081       if (tag=="VERTEX3"){
00082         int id;
00083         Pose p;
00084         ls >> id >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw(); 
00085         TreePoseGraph3::Vertex* v=addVertex(id,p);
00086         if (v){
00087           v->transformation=Transformation(p);
00088         }
00089       }
00090     }      
00091   }
00092   is.clear(); /* clears the end-of-file and error flags */
00093   is.seekg(0, ios::beg);
00094 
00095   //bool edgesOk=true;
00096   while(is){
00097     char buf[LINESIZE];
00098     is.getline(buf,LINESIZE);
00099     istringstream ls(buf);
00100     string tag;
00101     ls >> tag;
00102     
00103     if (twoDimensions){
00104       if (tag=="EDGE"){
00105         int id1, id2;
00106         Pose p(0.,0.,0.,0.,0.,0.);
00107         InformationMatrix m;
00108         ls >> id1 >> id2 >> p.x() >> p.y() >> p.yaw();
00109         m=DMatrix<double>::I(6);
00110         if (! overrideCovariances){
00111           ls >> m[0][0] >> m[0][1] >> m[1][1] >> m[2][2] >> m[0][2] >> m[1][2];
00112           m[2][0]=m[0][2]; m[2][1]=m[1][2]; m[1][0]=m[0][1];
00113         }
00114       
00115         TreePoseGraph3::Vertex* v1=vertex(id1);
00116         TreePoseGraph3::Vertex* v2=vertex(id2);
00117         Transformation t(p);
00118         if (!addEdge(v1, v2,t ,m)){
00119           cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
00120           cerr << "edge=" << id1 <<" -> " << id2 << endl;
00121           //edgesOk=false;
00122         } 
00123       }
00124     } else {
00125       if (tag=="EDGE3"){
00126         int id1, id2;
00127         Pose p;
00128         InformationMatrix m;
00129         ls >> id1 >> id2 >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw();
00130         m=DMatrix<double>::I(6);
00131         if (! overrideCovariances){
00132           for (int i=0; i<6; i++)
00133             for (int j=i; j<6; j++)
00134               ls >> m[i][j];
00135         }
00136         TreePoseGraph3::Vertex* v1=vertex(id1);
00137         TreePoseGraph3::Vertex* v2=vertex(id2);
00138         Transformation t(p);
00139         if (!addEdge(v1, v2,t ,m)){
00140           cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
00141           cerr << "edge=" << id1 <<" -> " << id2 << endl;
00142           //edgesOk=false;
00143         }
00144       }
00145     }
00146   }
00147   return true;
00148   //return edgesOk;
00149 }
00150 
00151 bool TreePoseGraph3::loadEquivalences(const char* filename){
00152   ifstream is(filename);
00153   if (!is)
00154     return false;
00155   EdgeList suppressed;
00156   uint equivCount=0;
00157   while (is){
00158     char buf[LINESIZE];
00159     is.getline(buf, LINESIZE);
00160     istringstream ls(buf);
00161     string tag;
00162     ls >> tag;
00163     if (tag=="EQUIV"){
00164       int id1, id2;
00165       ls >> id1 >> id2;
00166       Edge* e=edge(id1,id2);
00167       if (!e)
00168         e=edge(id2,id1);
00169       if (e){
00170         suppressed.push_back(e);
00171         equivCount++;
00172       }
00173     }
00174   }
00175   for (EdgeList::iterator it=suppressed.begin(); it!=suppressed.end(); it++){
00176     Edge* e=*it;
00177     if (e->v1->id > e->v2->id)
00178       revertEdge(e);
00179     collapseEdge(e);
00180   }
00181   return true;
00182 }
00183 
00184 bool TreePoseGraph3::saveGnuplot(const char* filename){
00185   ofstream os(filename);
00186   if (!os)
00187     return false;
00188   for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
00189     TreePoseGraph3::Vertex* v=it->second;
00190     v->pose=v->transformation.toPoseType();
00191   }
00192   for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
00193     const TreePoseGraph3::Edge * e=it->second;
00194     const Vertex* v1=e->v1;
00195     const Vertex* v2=e->v2;
00196     
00197     os << v1->pose.x() << " " << v1->pose.y() << " " << v1->pose.z() << " "
00198        << v1->pose.roll() << " " << v1->pose.pitch() << " " << v1->pose.yaw() <<endl;
00199     os << v2->pose.x() << " " << v2->pose.y() << " " << v2->pose.z() << " "
00200        << v2->pose.roll() << " " << v2->pose.pitch() << " " << v2->pose.yaw() <<endl;
00201     os << endl << endl;
00202   }
00203   return true;
00204 
00205 }
00206 
00207 bool TreePoseGraph3::save(const char* filename){
00208   ofstream os(filename);
00209   if (!os)
00210     return false;
00211   
00212   for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
00213     TreePoseGraph3::Vertex* v=it->second;
00214     v->pose=v->transformation.toPoseType();
00215 
00216     os << "VERTEX3 " 
00217        << v->id << " " 
00218        << v->pose.x() << " " 
00219        << v->pose.y() << " " 
00220        << v->pose.z() << " "
00221        << v->pose.roll() << " " 
00222        << v->pose.pitch() << " " 
00223        << v->pose.yaw() << endl; 
00224   }
00225   for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
00226     const TreePoseGraph3::Edge * e=it->second;
00227     os << "EDGE3 " << e->v1->id << " " << e->v2->id << " ";
00228     Pose p=e->transformation.toPoseType();
00229     os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << " ";
00230     for (int i=0; i<6; i++)
00231       for (int j=i; j<6; j++)
00232         os << e->informationMatrix[i][j] << " ";
00233     os << endl;
00234   }
00235   return true;
00236 }
00237 
00240 struct IdPrinter{
00241   IdPrinter(std::ostream& _os):os(_os){}
00242   std::ostream& os;
00243   void perform(TreePoseGraph3::Vertex* v){
00244     std::cout << "(" << v->id << "," << v->level << ")" << endl;
00245   }
00246 };
00247 
00248 void TreePoseGraph3::printDepth( std::ostream& os ){
00249   IdPrinter ip(os);
00250   treeDepthVisit(ip, root);
00251 }
00252 
00253 void TreePoseGraph3::printWidth( std::ostream& os ){
00254   IdPrinter ip(os);
00255   treeBreadthVisit(ip);
00256 }
00257 
00262 struct PosePropagator{
00263   void perform(TreePoseGraph3::Vertex* v){
00264     if (!v->parent)
00265       return;
00266     TreePoseGraph3::Transformation tParent(v->parent->transformation);
00267     TreePoseGraph3::Transformation tNode=tParent*v->parentEdge->transformation;
00268 
00269     assert(v->parentEdge->v1==v->parent);
00270     assert(v->parentEdge->v2==v);
00271     v->transformation=tNode;
00272   }
00273 };
00274 
00275 void TreePoseGraph3::initializeOnTree(){
00276   PosePropagator pp;
00277   treeDepthVisit(pp, root);
00278 }
00279 
00280 
00281 void TreePoseGraph3::printEdgesStat(std::ostream& os){
00282   for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
00283     const TreePoseGraph3::Edge * e=it->second;
00284     os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
00285     Pose p=e->transformation.toPoseType();
00286     os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << endl;
00287     os << "   top=" << e->top->id << " length=" << e->length << endl;
00288   }
00289 }
00290 
00291 void TreePoseGraph3::revertEdgeInfo(Edge* e){
00292   // here we assume uniform covariances, and we neglect the transofrmation
00293   // induced by the Jacobian when reverting the link
00294   e->transformation=e->transformation.inv();
00295 };
00296 
00297 void TreePoseGraph3::initializeFromParentEdge(Vertex* v){
00298   Transformation tp=Transformation(v->parent->pose)*v->parentEdge->transformation;
00299   v->transformation=tp;
00300   v->pose=tp.toPoseType();
00301   v->parameters=v->parentEdge->transformation;
00302 }
00303 
00304 
00305 void TreePoseGraph3::collapseEdge(Edge* e){
00306   Vertex* v1=e->v1;
00307   Vertex* v2=e->v2;
00308   
00309   // all the edges of v2 become outgoing
00310   for (EdgeList::iterator it=v2->edges.begin(); it!=v2->edges.end(); it++){
00311     if ( (*it)->v1!=v2 )
00312       revertEdge(*it);
00313   }
00314 
00315  // all the edges of v1 become outgoing
00316   for (EdgeList::iterator it=v1->edges.begin(); it!=v1->edges.end(); it++){
00317     if ( (*it)->v1!=v1 )
00318       revertEdge(*it);
00319   }
00320 
00321   assert(e->v1==v1);
00322 
00323   InformationMatrix I12=e->informationMatrix;
00324   CovarianceMatrix  C12=I12.inv();
00325   Transformation    T12=e->transformation;
00326   Pose              p12=T12.toPoseType();
00327 
00328   //Transformation iT12=T12.inv();
00329 
00330   //compute the marginal information of the nodes in the path v1-v2-v*
00331   for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
00332     Edge* e2=*it2;
00333     if (e2->v1==v2){ //edge leaving v2
00334       Transformation T2x=e2->transformation;
00335       Pose           p2x=T2x.toPoseType();
00336       InformationMatrix I2x=e2->informationMatrix;
00337       CovarianceMatrix  C2x=I2x.inv();
00338       
00339       //compute the estimate of the vertex based on the path v1-v2-vx
00340       
00341       //Transformation tr=iT12*T2x;
00342       
00343       CovarianceMatrix CM=C2x;
00344       
00345       
00346       Transformation T1x_pred=T12*e2->transformation;
00347       Covariance C1x_pred=C12+C2x;
00348       InformationMatrix I1x_pred=C1x_pred.inv();
00349 
00350       e2->transformation=T1x_pred;
00351       e2->informationMatrix=I1x_pred;
00352     }
00353   }
00354 
00355   //all the edges leaving v1 and leaving v2 and leading to the same point are merged
00356   std::list<Transformation> tList;
00357   std::list<InformationMatrix> iList;
00358   std::list<Vertex*> vList;
00359 
00360   //others are transformed and added to v1
00361   for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
00362     Edge* e1x=0;
00363     Edge* e2x=0;
00364     if ( ((*it2)->v1!=v1)){
00365       e2x=*it2;
00366       for (EdgeList::iterator it1=v1->edges.begin(); it1!=v1->edges.end(); it1++){
00367         if ((*it1)->v2==(*it2)->v2)
00368           e1x=*it1;
00369       }
00370 
00371     }
00372     // FIXME
00373     // edges leading to the same node are ignored
00374     // should be merged
00375     if (e1x && e2x){
00376       // here goes something for mergin the constraints, according to the information matrices.
00377       // in 3D it is a nightmare, so i postpone this, and i simply ignore the redundant constraints.
00378       // the resultng system is overconfident
00379     }
00380     if (!e1x && e2x){
00381       tList.push_back(e2x->transformation);
00382       iList.push_back(e2x->informationMatrix);
00383       vList.push_back(e2x->v2);
00384     }
00385   }
00386   removeVertex(v2->id);
00387 
00388   std::list<Transformation>::iterator t=tList.begin();
00389   std::list<InformationMatrix>::iterator i=iList.begin();
00390   std::list<Vertex*>::iterator v=vList.begin();
00391   while (i!=iList.end()){
00392     addEdge(v1,*v,*t,*i);
00393     i++;
00394     t++;
00395     v++;
00396   }
00397 }
00398 
00399 void TreePoseGraph3::recomputeAllTransformations(){
00400   TransformationPropagator tp;
00401   treeDepthVisit(tp,root);
00402 }
00403 
00404 }; //namespace AISNavigation


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:21