optimizer.h
Go to the documentation of this file.
00001 #ifndef _OPTIMIZER_H_
00002 #define _OPTIMIZER_H_
00003 
00004 #include "point.h"
00005 
00006 namespace GMapping {
00007 
00008 struct OptimizerParams{
00009         double discretization;
00010         double angularStep, linearStep;
00011         int iterations;
00012         double maxRange;
00013 };
00014 
00015 template <typename Likelihood, typename Map>
00016 struct Optimizer {
00017         Optimizer(const OptimizerParams& params);
00018         OptimizerParams params;
00019         Map lmap;
00020         Likelihood likelihood;
00021         OrientedPoint gradientDescent(const RangeReading& oldReading, const RangeReading& newReading);
00022         OrientedPoint gradientDescent(const RangeReading& oldReading, const OrientedPoint& pose, OLocalMap& Map);
00023         enum Move {Forward, Backward, Left, Right, TurnRight, TurnLeft};
00024 };
00025 
00026 template <typename Likelihood, typename Map>
00027 Optimizer<Likelihood, Map>::Optimizer(const OptimizerParams& p):
00028         params(p),
00029         lmap(p.discretization){}
00030 
00031 template <typename Likelihood, typename Map>
00032 OrientedPoint Optimizer<Likelihood, Map>::gradientDescent(const RangeReading& oldReading, const RangeReading& newReading){
00033         lmap.clear();
00034         lmap.update(oldReading, OrientedPoint(0,0,0), params.maxRange);
00035         OrientedPoint delta=absoluteDifference(newReading.getPose(), oldReading.getPose());
00036         OrientedPoint bestPose=delta;
00037         double bestScore=likelihood(lmap, newReading, bestPose, params.maxRange);
00038         int it=0;
00039         double lstep=params.linearStep, astep=params.angularStep;
00040         bool increase;
00041 /*      cerr << "bestScore=" << bestScore << endl;;*/
00042         do {
00043                 increase=false;
00044                 OrientedPoint itBestPose=bestPose;
00045                 double itBestScore=bestScore;
00046                 bool itIncrease;
00047                 do {
00048                         itIncrease=false;
00049                         OrientedPoint testBestPose=itBestPose;
00050                         double testBestScore=itBestScore;
00051                         for (Move move=Forward; move<=TurnLeft; move=(Move)((int)move+1)){
00052                                 OrientedPoint testPose=itBestPose;
00053                                 switch(move){
00054                                         case Forward: testPose.x+=lstep;
00055                                                 break;
00056                                         case Backward: testPose.x-=lstep;
00057                                                 break;
00058                                         case Left:    testPose.y+=lstep;
00059                                                 break;
00060                                         case Right:   testPose.y-=lstep;
00061                                                 break;
00062                                         case TurnRight: testPose.theta-=astep;
00063                                                 break;
00064                                         case TurnLeft: testPose.theta+=astep;
00065                                                 break;
00066                                 }
00067                                 double score=likelihood(lmap, newReading, testPose, params.maxRange);
00068                                 if (score>testBestScore){
00069                                         testBestScore=score;
00070                                         testBestPose=testPose;
00071                                 }
00072                         }
00073                         if (testBestScore > itBestScore){
00074                                 itBestScore=testBestScore;
00075                                 itBestPose=testBestPose;
00076 /*                              cerr << "s=" << itBestScore << " ";*/
00077                                 itIncrease=true;
00078                         }
00079                 } while(itIncrease);
00080                 if (itBestScore > bestScore){
00081 /*                      cerr << "S(" << itBestScore << "," <<  bestScore<< ")";*/
00082                         bestScore=itBestScore;
00083                         bestPose=itBestPose;
00084                         increase=true;
00085                 } else {
00086                         it++;
00087                         lstep*=0.5;
00088                         astep*=0.5;
00089                 }
00090         } while (it<params.iterations);
00091 /*      cerr << "FinalBestScore" << bestScore << endl;*/
00092         cerr  << endl;
00093         return bestPose;
00094 }
00095 
00096 template <typename Likelihood, typename Map>
00097 OrientedPoint Optimizer<Likelihood, Map>::gradientDescent(const RangeReading& reading, const OrientedPoint& pose, OLocalMap& lmap){
00098         OrientedPoint bestPose=pose;
00099         double bestScore=likelihood(lmap, reading, bestPose, params.maxRange);
00100         int it=0;
00101         double lstep=params.linearStep, astep=params.angularStep;
00102         bool increase;
00103 /*      cerr << "bestScore=" << bestScore << endl;;*/
00104         do {
00105                 increase=false;
00106                 OrientedPoint itBestPose=bestPose;
00107                 double itBestScore=bestScore;
00108                 bool itIncrease;
00109                 do {
00110                         itIncrease=false;
00111                         OrientedPoint testBestPose=itBestPose;
00112                         double testBestScore=itBestScore;
00113                         for (Move move=Forward; move<=TurnLeft; move=(Move)((int)move+1)){
00114                                 OrientedPoint testPose=itBestPose;
00115                                 switch(move){
00116                                         case Forward: testPose.x+=lstep;
00117                                                 break;
00118                                         case Backward: testPose.x-=lstep;
00119                                                 break;
00120                                         case Left:    testPose.y+=lstep;
00121                                                 break;
00122                                         case Right:   testPose.y-=lstep;
00123                                                 break;
00124                                         case TurnRight: testPose.theta-=astep;
00125                                                 break;
00126                                         case TurnLeft: testPose.theta+=astep;
00127                                                 break;
00128                                 }
00129                                 double score=likelihood(lmap, reading, testPose, params.maxRange);
00130                                 if (score>testBestScore){
00131                                         testBestScore=score;
00132                                         testBestPose=testPose;
00133                                 }
00134                         }
00135                         if (testBestScore > itBestScore){
00136                                 itBestScore=testBestScore;
00137                                 itBestPose=testBestPose;
00138 /*                              cerr << "s=" << itBestScore << " ";*/
00139                                 itIncrease=true;
00140                         }
00141                 } while(itIncrease);
00142                 if (itBestScore > bestScore){
00143 /*                      cerr << "S(" << itBestScore << "," <<  bestScore<< ")";*/
00144                         bestScore=itBestScore;
00145                         bestPose=itBestPose;
00146                         increase=true;
00147                 } else {
00148                         it++;
00149                         lstep*=0.5;
00150                         astep*=0.5;
00151                 }
00152         } while (it<params.iterations);
00153 /*      cerr << "FinalBestScore" << bestScore << endl;*/
00154         cerr  << endl;
00155         return bestPose;
00156 }
00157 
00158 } // end namespace GMapping
00159 #endif


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13