posegraph2.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  *
00027  * Any of the above conditions can be waived if you get permission
00028  * from the copyright holder.  Nothing in this license impairs or
00029  * restricts the author's moral rights.
00030  *
00031  * TORO is distributed in the hope that it will be useful,
00032  * but WITHOUT ANY WARRANTY; without even the implied 
00033  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
00034  * PURPOSE.  
00035  **********************************************************************/
00036 
00043 #include "posegraph2.hh"
00044 #include <fstream>
00045 #include <sstream>
00046 #include <string>
00047 
00048 using namespace std;
00049 
00050 namespace AISNavigation {
00051 
00052 
00053 typedef unsigned int uint;
00054 #define LINESIZE 81920
00055 
00056 
00057 //#define DEBUG(i) if (verboseLevel>i) cerr
00058 
00059 
00060 bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
00061   clear();
00062   ifstream is(filename);
00063   if (!is)
00064     return false;
00065 
00066   while(is){
00067     char buf[LINESIZE];
00068     is.getline(buf,LINESIZE);
00069     istringstream ls(buf);
00070     string tag;
00071     ls >> tag;
00072 
00073     if (tag=="VERTEX" || tag=="VERTEX2"){
00074       int id;
00075       Pose p;
00076       ls >> id >> p.x() >> p.y() >> p.theta(); 
00077       addVertex(id,p);
00078         //DEBUG(2) << "V " << id << endl;
00079       
00080     }
00081 
00082     if (tag=="EDGE" || tag=="EDGE2"){
00083       int id1, id2;
00084       Pose p;
00085       InformationMatrix m;
00086       ls >> id1 >> id2 >> p.x() >> p.y() >> p.theta();
00087       if (overrideCovariances){
00088         m.values[0][0]=1;  m.values[1][1]=1; m.values[2][2]=1;
00089         m.values[0][1]=0;  m.values[0][2]=0; m.values[1][2]=0;
00090       } else {
00091         ls >> m.values[0][0] >> m.values[0][1] >> m.values [1][1]
00092            >> m.values[2][2] >> m.values[0][2] >> m.values [1][2];
00093       }
00094       m.values[1][0]=m.values[0][1];
00095       m.values[2][0]=m.values[0][2];
00096       m.values[2][1]=m.values[1][2];
00097       TreePoseGraph2::Vertex* v1=vertex(id1);
00098       TreePoseGraph2::Vertex* v2=vertex(id2);
00099       Transformation t(p);
00100       addEdge(v1, v2,t ,m);
00101          //DEBUG(2) << "E " << id1 << " " << id2 <<  endl;
00102     }
00103   }
00104   return true;
00105 }
00106 
00107 bool TreePoseGraph2::loadEquivalences(const char* filename){
00108   ifstream is(filename);
00109   if (!is)
00110     return false;
00111   EdgeList suppressed;
00112   uint equivCount=0;
00113   while (is){
00114     char buf[LINESIZE];
00115     is.getline(buf, LINESIZE);
00116     istringstream ls(buf);
00117     string tag;
00118     ls >> tag;
00119     if (tag=="EQUIV"){
00120       int id1, id2;
00121       ls >> id1 >> id2;
00122       Edge* e=edge(id1,id2);
00123       if (!e)
00124         e=edge(id2,id1);
00125       if (e){
00126         suppressed.push_back(e);
00127         equivCount++;
00128       }
00129     }
00130   }
00131   for (EdgeList::iterator it=suppressed.begin(); it!=suppressed.end(); it++){
00132     Edge* e=*it;
00133     if (e->v1->id > e->v2->id)
00134       revertEdge(e);
00135     collapseEdge(e);
00136   }
00137   for (TreePoseGraph2::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
00138     Vertex* v=it->second;
00139     v->edges.clear();
00140   }
00141   for (TreePoseGraph2::EdgeMap::iterator it=edges.begin(); it!=edges.end(); it++){
00142     TreePoseGraph2::Edge * e=it->second;
00143     e->v1->edges.push_back(e);
00144     e->v2->edges.push_back(e);
00145   }
00146   return true;
00147 }
00148 
00149 bool TreePoseGraph2::saveGnuplot(const char* filename){
00150   ofstream os(filename);
00151   if (!os)
00152     return false;
00153 
00154   for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
00155     const TreePoseGraph2::Edge * e=it->second;
00156     const Vertex* v1=e->v1;
00157     const Vertex* v2=e->v2;
00158     
00159     os << v1->pose.x() << " " << v1->pose.y() << " " << v1->pose.theta() << endl;
00160     os << v2->pose.x() << " " << v2->pose.y() << " " << v2->pose.theta() << endl;
00161     os << endl;
00162   }
00163   return true;
00164 
00165 }
00166 
00167 bool TreePoseGraph2::save(const char* filename){
00168   ofstream os(filename);
00169   if (!os)
00170     return false;
00171   
00172   for (TreePoseGraph2::VertexMap::const_iterator it=vertices.begin(); it!=vertices.end(); it++){
00173     const TreePoseGraph2::Vertex* v=it->second;
00174     os << "VERTEX " 
00175        << v->id << " " 
00176        << v->pose.x() << " " 
00177        << v->pose.y() << " " 
00178        << v->pose.theta()<< endl; 
00179   }
00180   for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
00181     const TreePoseGraph2::Edge * e=it->second;
00182     os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
00183     Pose p=e->transformation.toPoseType();
00184     os << p.x() << " " << p.y() << " " << p.theta() << " ";
00185     os << e->informationMatrix.values[0][0] << " "
00186        << e->informationMatrix.values[0][1] << " "
00187        << e->informationMatrix.values[1][1] << " "
00188        << e->informationMatrix.values[2][2] << " "
00189        << e->informationMatrix.values[0][2] << " "
00190        << e->informationMatrix.values[1][2] << endl;
00191     }
00192   return true;
00193 }
00194 
00197 struct IdPrinter{
00198   IdPrinter(std::ostream& _os):os(_os){}
00199   std::ostream& os;
00200   void perform(TreePoseGraph2::Vertex* v){
00201     std::cout << "(" << v->id << "," << v->level << ")" << endl;
00202   }
00203 };
00204 
00205 void TreePoseGraph2::printDepth( std::ostream& os ){
00206   IdPrinter ip(os);
00207   treeDepthVisit(ip, root);
00208 }
00209 
00210 void TreePoseGraph2::printWidth( std::ostream& os ){
00211   IdPrinter ip(os);
00212   treeBreadthVisit(ip);
00213 }
00214 
00218 struct PosePropagator{
00219   void perform(TreePoseGraph2::Vertex* v){
00220     if (!v->parent)
00221       return;
00222     TreePoseGraph2::Transformation tParent(v->parent->pose);
00223     TreePoseGraph2::Transformation tNode=tParent*v->parentEdge->transformation;
00224 
00225     //cerr << "EDGE(" << v->parentEdge->v1->id << "," << v->parentEdge->v2->id <<"): " << endl;
00226     //Pose pParent=v->parent->pose;
00227     //cerr << "     p=" << pParent.x() << "," << pParent.y() << "," << pParent.theta() <<  endl;
00228     //Pose pEdge=v->parentEdge->transformation.toPoseType();
00229     //cerr << "     m=" << pEdge.x() << "," << pEdge.y() << "," << pEdge.theta() <<  endl;
00230     //Pose pNode=tNode.toPoseType();
00231     //cerr << "     n=" << pNode.x() << "," << pNode.y() << "," << pNode.theta() <<  endl;
00232 
00233     assert(v->parentEdge->v1==v->parent);
00234     assert(v->parentEdge->v2==v);
00235     v->pose=tNode.toPoseType();
00236   }
00237 };
00238 
00239 void TreePoseGraph2::initializeOnTree(){
00240   PosePropagator pp;
00241   treeDepthVisit(pp, root);
00242 }
00243 
00244 
00245 void TreePoseGraph2::printEdgesStat(std::ostream& os){
00246   for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
00247     const TreePoseGraph2::Edge * e=it->second;
00248     os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
00249     Pose p=e->transformation.toPoseType();
00250     os << p.x() << " " << p.y() << " " << p.theta() << " ";
00251     os << e->informationMatrix.values[0][0] << " "
00252        << e->informationMatrix.values[0][1] << " "
00253        << e->informationMatrix.values[1][1] << " "
00254        << e->informationMatrix.values[2][2] << " "
00255        << e->informationMatrix.values[0][2] << " "
00256        << e->informationMatrix.values[1][2] << endl;
00257     os << "   top=" << e->top->id << " length=" << e->length << endl;
00258   }
00259 }
00260 
00261 void TreePoseGraph2::revertEdgeInfo(Edge* e){
00262   Transformation it=e->transformation.inv();
00263   InformationMatrix R;
00264   R.values[0][0]=e->transformation.rotationMatrix[0][0];
00265   R.values[0][1]=e->transformation.rotationMatrix[0][1];
00266   R.values[0][2]=0;
00267 
00268   R.values[1][0]=e->transformation.rotationMatrix[1][0];
00269   R.values[1][1]=e->transformation.rotationMatrix[1][1];
00270   R.values[1][2]=0;
00271 
00272   R.values[2][0]=0;
00273   R.values[2][1]=0;
00274   R.values[2][2]=1;
00275 
00276   InformationMatrix IM=R.transpose()*e->informationMatrix*R;
00277 
00278 
00279   //Pose np=e->transformation.toPoseType();
00280  
00281   //Pose ip=it.toPoseType();
00282 
00283   //Transformation tc=it*e->transformation;
00284   //Pose pc=tc.toPoseType();
00285 
00286   e->transformation=it;
00287   e->informationMatrix=IM;
00288 };
00289 
00290 void TreePoseGraph2::initializeFromParentEdge(Vertex* v){
00291   Transformation tp=Transformation(v->parent->pose)*v->parentEdge->transformation;
00292   v->transformation=tp;
00293   v->pose=tp.toPoseType();
00294   v->parameters=v->pose;
00295   v->parameters.x()-=v->parent->pose.x();
00296   v->parameters.y()-=v->parent->pose.y();
00297   v->parameters.theta()-=v->parent->pose.theta();
00298   v->parameters.theta()=atan2(sin(v->parameters.theta()), cos(v->parameters.theta()));
00299 }
00300 
00301 void TreePoseGraph2::collapseEdge(Edge* e){
00302   EdgeMap::iterator ie_it=edges.find(e);
00303   if (ie_it==edges.end())
00304     return;
00305   //VertexMap::iterator it1=vertices.find(e->v1->id);
00306   //VertexMap::iterator it2=vertices.find(e->v2->id);
00307   assert(vertices.find(e->v1->id)!=vertices.end());
00308   assert(vertices.find(e->v2->id)!=vertices.end());
00309 
00310   Vertex* v1=e->v1;
00311   Vertex* v2=e->v2;
00312   
00313   
00314   // all the edges of v2 become outgoing
00315   for (EdgeList::iterator it=v2->edges.begin(); it!=v2->edges.end(); it++){
00316     if ( (*it)->v1!=v2 )
00317       revertEdge(*it);
00318   }
00319 
00320  // all the edges of v1 become outgoing
00321   for (EdgeList::iterator it=v1->edges.begin(); it!=v1->edges.end(); it++){
00322     if ( (*it)->v1!=v1 )
00323       revertEdge(*it);
00324   }
00325 
00326   assert(e->v1==v1);
00327 
00328   InformationMatrix I12=e->informationMatrix;
00329   CovarianceMatrix  C12=I12.inv();
00330   Transformation    T12=e->transformation;
00331   //Pose              p12=T12.toPoseType();
00332 
00333   //Transformation iT12=T12.inv();
00334 
00335   //compute the marginal information of the nodes in the path v1-v2-v*
00336   for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
00337     Edge* e2=*it2;
00338     if (e2->v1==v2){ //edge leaving v2
00339       //Transformation T2x=e2->transformation;
00340       //Pose           p2x=T2x.toPoseType();
00341       InformationMatrix I2x=e2->informationMatrix;
00342       CovarianceMatrix  C2x=I2x.inv();
00343       
00344       //compute the estimate of the vertex based on the path v1-v2-vx
00345       
00346       //Transformation tr=iT12*T2x;
00347       
00348       //InformationMatrix R;
00349       //R.values[0][0]=tr.rotationMatrix[0][0];
00350       //R.values[0][1]=tr.rotationMatrix[0][1];
00351       //R.values[0][2]=0;
00352       
00353       //R.values[1][0]=tr.rotationMatrix[1][0];
00354       //R.values[1][1]=tr.rotationMatrix[1][1];
00355       //R.values[1][2]=0;
00356       
00357       //R.values[2][0]=0;
00358       //R.values[2][1]=0;
00359       //R.values[2][2]=1;
00360 
00361       //CovarianceMatrix CM=R.transpose()*C2x*R;
00362       
00363       
00364       Transformation T1x_pred=T12*e2->transformation;
00365       Covariance C1x_pred=C12+C2x;
00366       InformationMatrix I1x_pred=C1x_pred.inv();
00367 
00368       e2->transformation=T1x_pred;
00369       e2->informationMatrix=I1x_pred;
00370     }
00371   }
00372 
00373   //all the edges leaving v1 and leaving v2 and leading to the same point are merged
00374   std::list<Transformation> tList;
00375   std::list<InformationMatrix> iList;
00376   std::list<Vertex*> vList;
00377 
00378   //others are transformed and added to v1
00379   for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
00380     Edge* e1x=0;
00381     Edge* e2x=0;
00382     if ( ((*it2)->v1!=v1)){
00383       e2x=*it2;
00384       for (EdgeList::iterator it1=v1->edges.begin(); it1!=v1->edges.end(); it1++){
00385         if ((*it1)->v2==(*it2)->v2)
00386           e1x=*it1;
00387       }
00388 
00389     }
00390     if (e1x && e2x){
00391       Transformation t1x=e1x->transformation;
00392       InformationMatrix I1x=e1x->informationMatrix;
00393       Pose p1x=t1x.toPoseType();
00394 
00395       Transformation t2x=e2x->transformation;
00396       InformationMatrix I2x=e2x->informationMatrix;;
00397       Pose p2x=t2x.toPoseType();
00398 
00399       InformationMatrix IM=I1x+I2x;
00400       CovarianceMatrix  CM=IM.inv();
00401       InformationMatrix scale1=CM*I1x;
00402       InformationMatrix scale2=CM*I2x;
00403 
00404 
00405       Pose p1=scale1*p1x;
00406       Pose p2=scale2*p2x;
00407 
00408 
00409       //need to recover the angles in a decent way.
00410       double s=scale1.values[2][2]*sin(p1x.theta())+ scale2.values[2][2]*sin(p2x.theta());
00411       double c=scale1.values[2][2]*cos(p1x.theta())+ scale2.values[2][2]*cos(p2x.theta());
00412       
00413       //DEBUG(2) << "p1x= " << p1x.x() << " " << p1x.y() << " " << p1x.theta() << endl;
00414       //DEBUG(2) << "p1x_pred= " << p2x.x() << " " << p2x.y() << " " << p2x.theta() << endl;
00415       
00416       Pose pFinal(p1.x()+p2.x(), p1.y()+p2.y(), atan2(s,c));
00417       //DEBUG(2) << "p1x_final= " << pFinal.x() << " " << pFinal.y() << " " << pFinal.theta() << endl;
00418       
00419       e1x->transformation=Transformation(pFinal);
00420       e1x->informationMatrix=IM;
00421     }
00422     if (!e1x && e2x){
00423       tList.push_back(e2x->transformation);
00424       iList.push_back(e2x->informationMatrix);
00425       vList.push_back(e2x->v2);
00426     }
00427   }
00428   removeVertex(v2->id);
00429 
00430   std::list<Transformation>::iterator t=tList.begin();
00431   std::list<InformationMatrix>::iterator i=iList.begin();
00432   std::list<Vertex*>::iterator v=vList.begin();
00433   while (i!=iList.end()){
00434     addEdge(v1,*v,*t,*i);
00435     i++;
00436     t++;
00437     v++;
00438   }
00439 }
00440 
00441 }; //namespace AISNavigation


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