treeoptimizer3_iteration.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 
00037 #include "treeoptimizer3.hh"
00038 #include <fstream>
00039 #include <string>
00040 
00041 using namespace std;
00042 
00043 namespace AISNavigation {
00044 
00045 //#define DEBUG(i) if (verboseLevel>i) cerr
00046 
00047 //helper functions. Should I explain :-)?
00048 inline double max3( const double& a, const double& b, const double& c){
00049   double m=a>b?a:b;
00050   return m>c?m:c;
00051 }
00052 inline double min3( const double& a, const double& b, const double& c){
00053   double m=a<b?a:b;
00054   return m<c?m:c;
00055 }
00056 
00057 
00058 struct NodeInfo{
00059   TreeOptimizer3::Vertex* n;
00060   double translationalWeight;
00061   double rotationalWeight;
00062   int direction;
00063   TreeOptimizer3::Transformation transformation;
00064   TreeOptimizer3::Transformation parameters;
00065   NodeInfo(TreeOptimizer3::Vertex* v=0, double tw=0, double rw=0, int dir=0,
00066            TreeOptimizer3::Transformation t=TreeOptimizer3::Transformation(0,0,0,0,0,0), 
00067            TreeOptimizer3::Parameters p=TreeOptimizer3::Transformation(0,0,0,0,0,0)){
00068     n=v;
00069     translationalWeight=tw;
00070     rotationalWeight=rw;
00071     direction=dir;
00072     transformation=t;
00073     parameters=p;
00074   }
00075 };
00076 
00077 typedef std::vector<NodeInfo> NodeInfoVector;
00078 
00079 
00080 /********************************** Preconditioned and unpreconditioned error distribution ************************************/
00081 
00082 
00083 
00084 
00085 
00086 void TreeOptimizer3::computePreconditioner(){
00087   for (uint i=0; i<M.size(); i++){
00088     M[i][0]=0;
00089     M[i][1]=0;
00090   }
00091   gamma[0] = gamma[1] = numeric_limits<double>::max();
00092 
00093   int edgeCount=0;
00094   for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
00095     edgeCount++;
00096     //if (! (edgeCount%1000))
00097     //  DEBUG(1) << "m";
00098 
00099     Edge* e=*it;
00100     //Transformation t=e->transformation;
00101     InformationMatrix W=e->informationMatrix;
00102     
00103     Vertex* top=e->top;
00104     for (int dir=0; dir<2; dir++){
00105       Vertex* n = (dir==0)? e->v1 : e->v2;
00106       while (n!=top){
00107         uint i=n->id;
00108         double rW=min3(W[0][0], W[1][1], W[2][2]);
00109         double tW=min3(W[3][3], W[4][4], W[5][5]);
00110         M[i][0]+=rW;
00111         M[i][1]+=tW;
00112         gamma[0]=gamma[0]<rW?gamma[0]:rW;
00113         gamma[1]=gamma[1]<tW?gamma[1]:tW;
00114         n=n->parent;
00115       }
00116     }
00117 
00118   }
00119   
00120   if (verboseLevel>1){
00121     for (uint i=0; i<M.size(); i++){
00122       cerr << "M[" << i << "]=" << M[i][0] << " " << M[i][1] << endl;
00123     }
00124   }
00125 }
00126 
00127 
00128 void TreeOptimizer3::propagateErrors(bool usePreconditioner){
00129   iteration++;
00130   int edgeCount=0;
00131   // this is the workspace for computing the paths without 
00132   // bothering too much the memory allocation
00133   static NodeInfoVector path;
00134   path.resize(edges.size()+1);
00135   static Rotation zero(0.,0.,0.);
00136 
00137   onIterationStart(iteration);
00138   for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
00139     edgeCount++;
00140     //if (! (edgeCount%1000))
00141     //  DEBUG(1) << "c";
00142 
00143     if (isDone())
00144       return;
00145     
00146     Edge* e=*it;
00147     Vertex* top=e->top;
00148     Vertex* v1=e->v1;
00149     Vertex* v2=e->v2;
00150     int l=e->length;
00151     onStepStart(e);
00152     
00153     recomputeTransformations(v1,top);
00154     recomputeTransformations(v2,top);
00155     //DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
00156 
00157     //BEGIN: Path and weight computation 
00158     int pc=0;
00159     Vertex* aux=v1;
00160     double totTW=0, totRW=0;
00161     while(aux!=top){
00162       int index=aux->id;
00163       double tw=1./(double)l, rw=1./(double)l;
00164       if (usePreconditioner){
00165         tw=1./M[index][0]; 
00166         rw=1./M[index][1];
00167       }
00168       totTW+=tw;
00169       totRW+=rw;
00170       path[pc++]=NodeInfo(aux,tw,rw,-1,aux->transformation, aux->parameters);
00171       aux=aux->parent;
00172     }
00173     int topIndex=pc;
00174     path[pc++]=NodeInfo(top,0.,0.,0, top->transformation, top->parameters);
00175     pc=l;
00176     aux=v2;
00177     while(aux!=top){
00178       int index=aux->id;
00179       double tw=1./l, rw=1./l;
00180       if (usePreconditioner){
00181         tw=1./M[index][0]; 
00182         rw=1./M[index][1];
00183       }
00184       totTW+=tw;
00185       totRW+=rw;
00186       path[pc--]=NodeInfo(aux,tw,rw,1,aux->transformation, aux->parameters);
00187       aux=aux->parent;
00188     }
00189 
00190     //store the transformations relative to the top node
00191     //Transformation topTransformation=top->transformation;
00192     //Transformation topParameters=top->parameters;
00193 
00194     //END: Path and weight computation
00195     
00196 
00197 
00198     //BEGIN: Rotational Error
00199     Rotation r1=getRotation(v1, top);
00200     Rotation r2=getRotation(v2, top);
00201     Rotation re=e->transformation.rotation();
00202     Rotation rR=r2.inverse()*(r1*re);
00203 
00204     double rotationFactor=(usePreconditioner)?
00205       sqrt(double(l))* min3(e->informationMatrix[0][0],
00206                             e->informationMatrix[1][1], 
00207                             e->informationMatrix[2][2])/
00208       ( gamma[0]* (double)iteration ):
00209       sqrt(double(l))*rotGain/(double)iteration;
00210 
00211 //     double rotationFactor=(usePreconditioner)?
00212 //       sqrt(double(l))*rotGain/
00213 //       ( gamma[0]* (double)iteration * min3(e->informationMatrix[0][0],
00214 //                                         e->informationMatrix[1][1], 
00215 //                                         e->informationMatrix[2][2])):
00216 //       sqrt(double(l))*rotGain/(double)iteration;
00217 
00218     if (rotationFactor>1)
00219       rotationFactor=1;
00220 
00221     Rotation totalRotation = path[l].transformation.rotation() * rR * path[l].transformation.rotation().inverse();
00222 
00223     Translation axis   = totalRotation.axis();
00224     double angle=totalRotation.angle();
00225 
00226     double cw=0;
00227     for (int i= 1; i<=topIndex; i++){
00228       cw+=path[i-1].rotationalWeight/totRW;
00229       Rotation R=path[i].transformation.rotation();
00230       Rotation B(axis, angle*cw*rotationFactor);
00231       R= B*R;
00232       path[i].transformation.setRotation(R);
00233     }
00234     for (int i= topIndex+1; i<=l; i++){
00235       cw+=path[i].rotationalWeight/totRW;
00236       Rotation R=path[i].transformation.rotation();
00237       Rotation B(axis, angle*cw*rotationFactor);
00238       R= B*R;
00239       path[i].transformation.setRotation(R);
00240     }
00241 
00242     //recompute the parameters based on the transformation
00243     for (int i=0; i<topIndex; i++){
00244       Vertex* n=path[i].n;
00245       n->parameters.setRotation(path[i+1].transformation.rotation().inverse()*path[i].transformation.rotation());
00246     }
00247     for (int i= topIndex+1; i<=l; i++){
00248       Vertex* n=path[i].n;
00249       n->parameters.setRotation(path[i-1].transformation.rotation().inverse()*path[i].transformation.rotation());
00250     }
00251 
00252     //END: Rotational Error
00253 
00254 
00255     //now spread the parameters
00256     recomputeTransformations(v1,top);
00257     recomputeTransformations(v2,top);
00258 
00259     //BEGIN: Translational Error
00260     //Translation topTranslation=top->transformation.translation();
00261    
00262     Transformation tr12=v1->transformation*e->transformation;
00263     Translation tR=tr12.translation()-v2->transformation.translation();
00264 
00265 
00266 //     double translationFactor=(usePreconditioner)?
00267 //       trasGain*l/( gamma[1]* (double)iteration * min3(e->informationMatrix[3][3],
00268 //                                                    e->informationMatrix[4][4], 
00269 //                                                    e->informationMatrix[5][5])):
00270 //       trasGain*l/(double)iteration;
00271 
00272     double translationFactor=(usePreconditioner)?
00273       trasGain*l*min3(e->informationMatrix[3][3],
00274                       e->informationMatrix[4][4], 
00275                       e->informationMatrix[5][5])/( gamma[1]* (double)iteration):
00276       trasGain*l/(double)iteration;
00277 
00278     if (translationFactor>1)
00279       translationFactor=1;
00280     Translation dt=tR*translationFactor;
00281 
00282     //left wing
00283     double lcum=0;
00284     for (int i=topIndex-1; i>=0; i--){
00285       Vertex* n=path[i].n;
00286       lcum-=(usePreconditioner) ? path[i].translationalWeight/totTW :  1./(double)l;
00287       double fraction=lcum;
00288       Translation offset= dt*fraction;
00289       Translation T=n->transformation.translation()+offset;
00290       n->transformation.setTranslation(T);
00291     }
00292     //right wing
00293     
00294     double rcum=0;
00295     for (int i=topIndex+1; i<=l; i++){
00296       Vertex* n=path[i].n;
00297       rcum+=(usePreconditioner) ? path[i].translationalWeight/totTW :  1./(double)l;
00298       double fraction=rcum;
00299       Translation offset= dt*fraction;
00300       Translation T=n->transformation.translation()+offset;
00301       n->transformation.setTranslation(T);
00302     }
00303     assert(fabs(lcum+rcum)-1<1e-6);
00304 
00305     recomputeParameters(v1, top);
00306     recomputeParameters(v2, top);
00307 
00308     //END: Translational Error
00309 
00310     onStepFinished(e);
00311 
00312     if (verboseLevel>2){
00313       Rotation newRotResidual=v2->transformation.rotation().inverse()*(v1->transformation.rotation()*re);
00314       Translation newRotResidualAxis=newRotResidual.axis();
00315       double      newRotResidualAngle=newRotResidual.angle();
00316       Translation rotResidualAxis=rR.axis();
00317       double      rotResidualAngle=rR.angle();
00318       Translation newTransResidual=(v1->transformation*e->transformation).translation()-v2->transformation.translation();
00319 
00320       cerr << "RotationalFraction:  " << rotationFactor << endl;
00321       cerr << "Rotational residual: " 
00322            << " axis " << rotResidualAxis.x()  << "\t" << rotResidualAxis.y()  << "\t" << rotResidualAxis.z() << " --> "
00323            << "   ->  " << newRotResidualAxis.x()  << "\t" << newRotResidualAxis.y()  << "\t" << newRotResidualAxis.z()  << endl;
00324       cerr << " angle " << rotResidualAngle  << "\t" << newRotResidualAngle  << endl;
00325       
00326       cerr << "Translational Fraction:  " << translationFactor << endl;
00327       cerr << "Translational Residual" << endl;
00328       cerr << "    " << tR.x()  << "\t" << tR.y()  << "\t" << tR.z()  << endl;
00329       cerr << "    " << newTransResidual.x()  << "\t" << newTransResidual.y()  << "\t" << newTransResidual.z()  << endl;
00330     }
00331     
00332     if (verboseLevel>101){
00333       char filename [1000];
00334       sprintf(filename, "po-%02d-%03d-%03d-.dat", iteration, v1->id, v2->id);
00335       recomputeAllTransformations();
00336       saveGnuplot(filename);
00337     }
00338   }
00339   onIterationFinished(iteration);
00340 }
00341 
00342 };//namespace AISNavigation


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