treeoptimizer3.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 
00044 #include "treeoptimizer3.hh"
00045 #include <fstream>
00046 #include <sstream>
00047 #include <string>
00048 
00049 using namespace std;
00050 
00051 namespace AISNavigation {
00052 
00053 //#define DEBUG(i) if (verboseLevel>i) cerr
00054 
00055 
00056 TreeOptimizer3::TreeOptimizer3(){
00057   restartOnDivergence=false;
00058   sortedEdges=0;
00059   mpl=-1;
00060   edgeCompareMode=EVComparator<Edge*>::CompareLevel;
00061 }
00062 
00063 
00064 TreeOptimizer3::~TreeOptimizer3(){
00065 }
00066 
00067 void TreeOptimizer3::initializeTreeParameters(){
00068   ParameterPropagator pp;
00069   treeDepthVisit(pp,root);
00070 }
00071 
00072 
00073 void TreeOptimizer3::iterate(TreePoseGraph3::EdgeSet* eset, bool noPreconditioner){
00074   TreePoseGraph3::EdgeSet* temp=sortedEdges;
00075   if (eset){
00076     sortedEdges=eset;
00077   }
00078 
00079   if (noPreconditioner)
00080     propagateErrors(false);
00081   else {
00082     if (iteration==1)
00083       computePreconditioner();  
00084     propagateErrors(true);
00085   }
00086   sortedEdges=temp;
00087 
00088   onRestartBegin();
00089   if (restartOnDivergence){
00090     double mte, ate;
00091     double mre, are;
00092     error(&mre, &mte, &are, &ate);
00093     maxTranslationalErrors.push_back(mte);
00094     maxRotationalErrors.push_back(mre);
00095     int interval=3;
00096     if ((int)maxRotationalErrors.size()>=interval){
00097       uint s=(uint)maxRotationalErrors.size();
00098       double re0 = maxRotationalErrors[s-interval];
00099       double re1 = maxRotationalErrors[s-1];
00100 
00101       if ((re1-re0)>are || sqrt(re1)>0.99*M_PI){
00102         double rg=rotGain;
00103         if (sqrt(re1)>M_PI/4){
00104           cerr << "RESTART!!!!! : Angular wraparound may be occourring" << endl;
00105           cerr << " err=" << re0 << " -> " << re1 << endl; 
00106           cerr << "Restarting optimization and reducing the rotation factor" << endl;
00107           cerr << rg << " -> ";
00108           initializeOnTree();
00109           initializeTreeParameters();
00110           initializeOptimization();
00111           error(&mre, &mte);
00112           maxTranslationalErrors.push_back(mte);
00113           maxRotationalErrors.push_back(mre);
00114           rg*=0.1;
00115           rotGain=rg;
00116           cerr << rotGain << endl;
00117         }
00118         else {
00119           cerr << "decreasing angular gain" << rotGain*0.1 << endl;
00120           rotGain*=0.1;
00121         }
00122       }
00123     }
00124   }
00125   onRestartDone();
00126 }
00127 
00128 
00129 void TreeOptimizer3::recomputeTransformations(Vertex*v, Vertex* top){
00130   if (v==top)
00131     return;
00132   recomputeTransformations(v->parent, top);
00133   v->transformation=v->parent->transformation*v->parameters;
00134 }
00135 
00136 
00137 void TreeOptimizer3::recomputeParameters(Vertex*v, Vertex* top){
00138   while (v!=top){
00139     v->parameters=v->parent->transformation.inv()*v->transformation;
00140     v=v->parent;
00141   }
00142 }
00143 
00144 
00145 TreeOptimizer3::Transformation TreeOptimizer3::getPose(Vertex*v, Vertex* top){
00146   Transformation t(0.,0.,0.,0.,0.,0.);
00147   if (v==top)
00148     return v->transformation;
00149   while (v!=top){
00150     t=v->parameters*t;
00151     v=v->parent;
00152   }
00153   return top->transformation*t;
00154 }
00155 
00156 TreeOptimizer3::Rotation TreeOptimizer3::getRotation(Vertex*v, Vertex* top){
00157   Rotation r(0.,0.,0.);
00158   if (v==top)
00159     return v->transformation.rotation();
00160   while (v!=top){
00161     r=v->parameters.rotation()*r;
00162     v=v->parent;
00163   }
00164   return top->transformation.rotation()*r;
00165 }
00166 
00167 
00168 
00169 double TreeOptimizer3::error(const Edge* e) const{
00170   const Vertex* v1=e->v1;
00171   const Vertex* v2=e->v2;
00172   
00173   Transformation et=e->transformation;
00174   Transformation t1=v1->transformation;
00175   Transformation t2=v2->transformation;
00176   
00177   Transformation t12=(t1*et)*t2.inv();
00178   Pose p12=t12.toPoseType();
00179 
00180   Pose ps=e->informationMatrix*p12;
00181   double err=p12*ps;
00182   //DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
00183   return err;
00184  
00185 }
00186 
00187 double TreeOptimizer3::traslationalError(const Edge* e) const{
00188   const Vertex* v1=e->v1;
00189   const Vertex* v2=e->v2;
00190   
00191   Transformation et=e->transformation;
00192   Transformation t1=v1->transformation;
00193   Transformation t2=v2->transformation;
00194   
00195   Translation t12=(t2.inv()*(t1*et)).translation();
00196   return t12*t12;;
00197  
00198 }
00199 
00200 double TreeOptimizer3::rotationalError(const Edge* e) const{
00201   const Vertex* v1=e->v1;
00202   const Vertex* v2=e->v2;
00203   
00204   Rotation er=e->transformation.rotation();
00205   Rotation r1=v1->transformation.rotation();
00206   Rotation r2=v2->transformation.rotation();
00207   
00208   Rotation r12=r2.inverse()*(r1*er);
00209   double r=r12.angle();
00210   return r*r;
00211 }
00212 
00213 
00214 double TreeOptimizer3::loopError(const Edge* e) const{
00215   double err=0;
00216   const Vertex* v=e->v1;
00217   while (v!=e->top){
00218     err+=error(v->parentEdge);
00219     v=v->parent;
00220   }
00221   v=e->v2;
00222   while (v==e->top){
00223     err+=error(v->parentEdge);
00224     v=v->parent;
00225   }
00226   if (e->v2->parentEdge!=e && e->v1->parentEdge!=e)
00227     err+=error(e);
00228   return err;
00229 }
00230 
00231 double TreeOptimizer3::loopRotationalError(const Edge* e) const{
00232   double err=0;
00233   const Vertex* v=e->v1;
00234   while (v!=e->top){
00235     err+=rotationalError(v->parentEdge);
00236     v=v->parent;
00237   }
00238   v=e->v2;
00239   while (v!=e->top){
00240     err+=rotationalError(v->parentEdge);
00241     v=v->parent;
00242   }
00243   if (e->v2->parentEdge!=e && e->v1->parentEdge!=e)
00244     err+=rotationalError(e);
00245   return err;
00246 }
00247 
00248 
00249 double TreeOptimizer3::error(double* mre, double* mte, double* are, double* ate, TreePoseGraph3::EdgeSet* eset) const{
00250   double globalRotError=0.;
00251   double maxRotError=0;
00252   double globalTrasError=0.;
00253   double maxTrasError=0;
00254 
00255   int c=0;
00256   if (! eset){
00257     for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
00258       double re=rotationalError(it->second);
00259       globalRotError+=re;
00260       maxRotError=maxRotError>re?maxRotError:re;
00261       double te=traslationalError(it->second);
00262       globalTrasError+=te;
00263       maxTrasError=maxTrasError>te?maxTrasError:te;
00264       c++;
00265     }
00266   } else {
00267     for (TreePoseGraph3::EdgeSet::const_iterator it=eset->begin(); it!=eset->end(); it++){
00268       const TreePoseGraph3::Edge* edge=*it;
00269       double re=rotationalError(edge);
00270       globalRotError+=re;
00271       maxRotError=maxRotError>re?maxRotError:re;
00272       double te=traslationalError(edge);
00273       globalTrasError+=te;
00274       maxTrasError=maxTrasError>te?maxTrasError:te;
00275       c++;
00276     }
00277   }
00278 
00279   if (mte)
00280     *mte=maxTrasError;
00281   if (mre)
00282     *mre=maxRotError;
00283   if (ate)
00284     *ate=globalTrasError/c;
00285   if (are)
00286     *are=globalRotError/c;
00287   return globalRotError+globalTrasError;
00288 }
00289 
00290 
00291 
00292 void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
00293   edgeCompareMode=mode;
00294   // compute the size of the preconditioning matrix
00295   int sz=maxIndex()+1;
00296   //DEBUG(1) << "Size= " << sz << endl;
00297   M.resize(sz);
00298 
00299   //DEBUG(1) << "allocating M(" << sz << ")" << endl;
00300   iteration=1;
00301 
00302   // sorting edges
00303   if (sortedEdges!=0){
00304     delete sortedEdges;
00305     sortedEdges=0;
00306   }
00307   sortedEdges=sortEdges();
00308   mpl=maxPathLength();
00309   rotGain=1.;
00310   trasGain=1.;
00311 }
00312 
00313 void TreeOptimizer3::initializeOnlineIterations(){
00314   int sz=maxIndex()+1;
00315   //DEBUG(1) << "Size= " << sz << endl;
00316   M.resize(sz);
00317   //DEBUG(1) << "allocating M(" << sz << ")" << endl;
00318   iteration=1;
00319   maxRotationalErrors.clear();
00320   maxTranslationalErrors.clear();
00321   rotGain=1.;
00322   trasGain=1.;
00323 }
00324 
00325 void TreeOptimizer3::initializeOnlineOptimization(EdgeCompareMode mode){
00326   edgeCompareMode=mode;
00327   // compute the size of the preconditioning matrix
00328   clear();
00329   Vertex* v0=addVertex(0,Pose(0,0,0,0,0,0));
00330   root=v0;
00331   v0->parameters=Transformation(v0->pose);
00332   v0->parentEdge=0;
00333   v0->parent=0;
00334   v0->level=0;
00335   v0->transformation=Transformation(TreePoseGraph3::Pose(0,0,0,0,0,0));
00336 }
00337 
00338 void TreeOptimizer3::onStepStart(Edge* e){
00339   //DEBUG(5) << "entering edge" << e << endl;
00340 }  
00341 void TreeOptimizer3::onStepFinished(Edge* e){
00342   //DEBUG(5) << "exiting edge" << e << endl;
00343 }
00344 
00345 void TreeOptimizer3::onIterationStart(int iteration){
00346   //DEBUG(5) << "entering iteration " << iteration << endl;
00347 }
00348 
00349 void TreeOptimizer3::onIterationFinished(int iteration){
00350   //DEBUG(5) << "exiting iteration " << iteration << endl;
00351 }
00352 
00353 void TreeOptimizer3::onRestartBegin(){}
00354 void TreeOptimizer3::onRestartDone(){}
00355 bool TreeOptimizer3::isDone(){
00356   return false;
00357 }
00358 
00359 
00360 }; //namespace AISNavigation


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