treeoptimizer2.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 "treeoptimizer2.hh"
00045 #include <fstream>
00046 #include <sstream>
00047 #include <string>
00048 
00049 typedef unsigned int uint;
00050 using namespace std;
00051 
00052 namespace AISNavigation {
00053 
00054 #define DEBUG(i) \
00055         if (verboseLevel>i) cerr
00056 
00058 struct ParameterPropagator{
00059   void perform(TreePoseGraph2::Vertex* v){
00060     if (!v->parent){
00061       v->parameters=TreePoseGraph2::Pose(0.,0.,0.);
00062       return;
00063     }
00064     v->parameters=TreePoseGraph2::Pose(v->pose.x()-v->parent->pose.x(), 
00065                       v->pose.y()-v->parent->pose.y(),
00066                       v->pose.theta()-v->parent->pose.theta());
00067   }
00068 };
00069 
00070 TreeOptimizer2::TreeOptimizer2(){
00071   sortedEdges=0;
00072 }
00073 
00074 TreeOptimizer2::~TreeOptimizer2(){
00075 }
00076 
00077 void TreeOptimizer2::initializeTreeParameters(){
00078   ParameterPropagator pp;
00079   treeDepthVisit(pp, root);
00080 }
00081 
00082 void TreeOptimizer2::initializeOptimization(){
00083   // compute the size of the preconditioning matrix
00084   int sz=maxIndex()+1;
00085   DEBUG(1) << "Size= " << sz << endl;
00086   M.resize(sz);
00087   DEBUG(1) << "allocating M(" << sz << ")" << endl;
00088   iteration=1;
00089 
00090   // sorting edges
00091   if (sortedEdges!=0){
00092     delete sortedEdges;
00093     sortedEdges=0;
00094   }
00095   sortedEdges=sortEdges();
00096 }
00097 
00098 void TreeOptimizer2::initializeOnlineOptimization(){
00099   // compute the size of the preconditioning matrix
00100   int sz=maxIndex()+1;
00101   DEBUG(1) << "Size= " << sz << endl;
00102   M.resize(sz);
00103   DEBUG(1) << "allocating M(" << sz << ")" << endl;
00104   iteration=1;
00105 }
00106 
00107 void TreeOptimizer2::computePreconditioner(){
00108   gamma[0] = gamma[1] =  gamma[2] = numeric_limits<double>::max();
00109 
00110   for (uint i=0; i<M.size(); i++)
00111     M[i]=Pose(0.,0.,0.);
00112 
00113   int edgeCount=0;
00114   for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
00115     edgeCount++;
00116     if (! (edgeCount%10000))
00117       DEBUG(1) << "m";
00118 
00119     Edge* e=*it;
00120     Transformation t=e->transformation;
00121     InformationMatrix S=e->informationMatrix;
00122     
00123     InformationMatrix R;
00124     R.values[0][0]=t.rotationMatrix[0][0];
00125     R.values[0][1]=t.rotationMatrix[0][1];
00126     R.values[0][2]=0;
00127     
00128     R.values[1][0]=t.rotationMatrix[1][0];
00129     R.values[1][1]=t.rotationMatrix[1][1];
00130     R.values[1][2]=0;
00131     
00132     R.values[2][0]=0;
00133     R.values[2][1]=0;
00134     R.values[2][2]=1;
00135 
00136     InformationMatrix W =R*S*R.transpose();
00137     
00138     Vertex* top=e->top;
00139     for (int dir=0; dir<2; dir++){
00140       Vertex* n = (dir==0)? e->v1 : e->v2;
00141       while (n!=top){
00142         uint i=n->id;
00143         M[i].values[0]+=W.values[0][0];
00144         M[i].values[1]+=W.values[1][1];
00145         M[i].values[2]+=W.values[2][2];
00146         gamma[0]=gamma[0]<W.values[0][0]?gamma[0]:W.values[0][0];
00147         gamma[1]=gamma[1]<W.values[1][1]?gamma[1]:W.values[1][1];
00148         gamma[2]=gamma[2]<W.values[2][2]?gamma[2]:W.values[2][2];
00149         n=n->parent;
00150       }
00151     }
00152   }
00153   
00154   if (verboseLevel>1){
00155     for (uint i=0; i<M.size(); i++){
00156       cerr << "M[" << i << "]=" << M[i].x() << " " << M[i].y() << " " << M[i].theta() <<endl;
00157     }
00158   }
00159 }
00160 
00161 
00162 void TreeOptimizer2::propagateErrors(){
00163   iteration++;
00164   int edgeCount=0;
00165   
00166   for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
00167       edgeCount++;
00168       if (! (edgeCount%10000))  DEBUG(1) << "c";
00169       
00170       Edge* e=*it;
00171       Vertex* top=e->top;
00172       
00173 
00174       Vertex* v1=e->v1;
00175       Vertex* v2=e->v2;
00176 
00177       double l=e->length;
00178       DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
00179       
00180       Pose p1=getPose(v1, top);
00181       Pose p2=getPose(v2, top);
00182 
00183       DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
00184       DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
00185       
00186       Transformation et=e->transformation;
00187       Transformation t1(p1);
00188       Transformation t2(p2);
00189 
00190       Transformation t12=t1*et;
00191     
00192       Pose p12=t12.toPoseType();
00193       DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
00194 
00195       Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
00196       double angle=r.theta();
00197       angle=atan2(sin(angle),cos(angle));
00198       r.theta()=angle;
00199       DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
00200     
00201       InformationMatrix S=e->informationMatrix;
00202       InformationMatrix R;
00203       R.values[0][0]=t1.rotationMatrix[0][0];
00204       R.values[0][1]=t1.rotationMatrix[0][1];
00205       R.values[0][2]=0;
00206       
00207       R.values[1][0]=t1.rotationMatrix[1][0];
00208       R.values[1][1]=t1.rotationMatrix[1][1];
00209       R.values[1][2]=0;
00210       
00211       R.values[2][0]=0;
00212       R.values[2][1]=0;
00213       R.values[2][2]=1;
00214 
00215       InformationMatrix W=R*S*R.transpose();
00216       Pose d=W*r*2.;
00217 
00218       DEBUG(2) << " d=" << d.x() << " " << d.y() << " " << d.theta() << endl;
00219     
00220       assert(l>0);
00221 
00222       double alpha[3] = { 1./(gamma[0]*iteration), 1./(gamma[1]*iteration), 1./(gamma[2]*iteration) };
00223 
00224       double tw[3]={0.,0.,0.};
00225       for (int dir=0; dir<2; dir++) {
00226         Vertex* n = (dir==0)? v1 : v2;
00227         while (n!=top){
00228           uint i=n->id;
00229           tw[0]+=1./M[i].values[0];
00230           tw[1]+=1./M[i].values[1];
00231           tw[2]+=1./M[i].values[2];
00232           n=n->parent;
00233         }
00234       }
00235 
00236       double beta[3] = {l*alpha[0]*d.values[0], l*alpha[1]*d.values[1], l*alpha[2]*d.values[2]};
00237       beta[0]=(fabs(beta[0])>fabs(r.values[0]))?r.values[0]:beta[0];
00238       beta[1]=(fabs(beta[1])>fabs(r.values[1]))?r.values[1]:beta[1];
00239       beta[2]=(fabs(beta[2])>fabs(r.values[2]))?r.values[2]:beta[2];
00240 
00241       DEBUG(2) << " alpha=" << alpha[0] << " " << alpha[1] << " " << alpha[2] << endl;
00242       DEBUG(2) << " beta=" << beta[0] << " " << beta[1] << " " << beta[2] << endl;
00243       
00244       for (int dir=0; dir<2; dir++) {
00245         Vertex* n = (dir==0)? v1 : v2;
00246         double sign=(dir==0)? -1. : 1.;
00247         while (n!=top){
00248           uint i=n->id;
00249           assert(M[i].values[0]>0);
00250           assert(M[i].values[1]>0);
00251           assert(M[i].values[2]>0);
00252 
00253           Pose delta( beta[0]/(M[i].values[0]*tw[0]), beta[1]/(M[i].values[1]*tw[1]), beta[2]/(M[i].values[2]*tw[2]));
00254           delta=delta*sign;
00255           DEBUG(2) << "   " << dir << ":"  << i <<"," << n->parent->id << ":"
00256                << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta() << " -> ";
00257 
00258           n->parameters.x()+=delta.x();
00259           n->parameters.y()+=delta.y();
00260           n->parameters.theta()+=delta.theta();
00261           DEBUG(2) << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta()<< endl;
00262           n=n->parent;
00263         }
00264       }
00265       updatePoseChain(v1,top);
00266       updatePoseChain(v2,top);
00267 
00268       Pose pf1=v1->pose;
00269       Pose pf2=v2->pose;
00270       
00271       DEBUG(2) << " pf1=" << pf1.x() << " " << pf1.y() << " " << pf1.theta() << endl;
00272       DEBUG(2) << " pf2=" << pf2.x() << " " << pf2.y() << " " << pf2.theta() << endl;
00273       DEBUG(2) << " en=" << p12.x()-pf2.x() << " " << p12.y()-pf2.y() << " " << p12.theta()-pf2.theta() << endl;
00274     }
00275   
00276 }
00277 
00278 void TreeOptimizer2::iterate(TreePoseGraph2::EdgeSet* eset){
00279   TreePoseGraph2::EdgeSet* temp=sortedEdges;
00280   if (eset){
00281     sortedEdges=eset;
00282   }
00283   computePreconditioner();
00284   propagateErrors();
00285   sortedEdges=temp;
00286 }
00287 
00288 void TreeOptimizer2::updatePoseChain(Vertex* v, Vertex* top){
00289   if (v!=top){
00290     updatePoseChain(v->parent, top);
00291     v->pose.x()=v->parent->pose.x()+v->parameters.x();
00292     v->pose.y()=v->parent->pose.y()+v->parameters.y();
00293     v->pose.theta()=v->parent->pose.theta()+v->parameters.theta();
00294     return;
00295   } 
00296 }
00297 
00298 TreeOptimizer2::Pose TreeOptimizer2::getPose(Vertex*v, Vertex* top){
00299   Pose p(0,0,0);
00300   Vertex* aux=v;
00301   while (aux!=top){
00302     p.x()+=aux->parameters.x();
00303     p.y()+=aux->parameters.y();
00304     p.theta()+=aux->parameters.theta();
00305     aux=aux->parent;
00306   }
00307   p.x()+=aux->pose.x();
00308   p.y()+=aux->pose.y();
00309   p.theta()+=aux->pose.theta();
00310   return p;
00311 }
00312 
00313 
00314 double TreeOptimizer2::error(const Edge* e) const{
00315   const Vertex* v1=e->v1;
00316   const Vertex* v2=e->v2;
00317   
00318   Pose p1=v1->pose;
00319   Pose p2=v2->pose;
00320   
00321   DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
00322   DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
00323   
00324   Transformation et=e->transformation;
00325   Transformation t1(p1);
00326   Transformation t2(p2);
00327   
00328   Transformation t12=t1*et;
00329     
00330   Pose p12=t12.toPoseType();
00331   DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
00332 
00333   Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
00334   double angle=r.theta();
00335   angle=atan2(sin(angle),cos(angle));
00336   r.theta()=angle;
00337   DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
00338     
00339   InformationMatrix S=e->informationMatrix;
00340   InformationMatrix R;
00341   R.values[0][0]=t1.rotationMatrix[0][0];
00342   R.values[0][1]=t1.rotationMatrix[0][1];
00343   R.values[0][2]=0;
00344       
00345   R.values[1][0]=t1.rotationMatrix[1][0];
00346   R.values[1][1]=t1.rotationMatrix[1][1];
00347   R.values[1][2]=0;
00348       
00349   R.values[2][0]=0;
00350   R.values[2][1]=0;
00351   R.values[2][2]=1;
00352 
00353   InformationMatrix W=R*S*R.transpose();
00354 
00355   Pose r1=W*r;
00356   return r.x()*r1.x()+r.y()*r1.y()+r.theta()*r1.theta();
00357 }
00358 
00359 double TreeOptimizer2::error() const{
00360   double globalError=0.;
00361   for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
00362     globalError+=error(it->second);
00363   }
00364   return globalError;
00365 }
00366 
00367 }; //namespace AISNavigation


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