optimizer.h
Go to the documentation of this file.
1 #ifndef _OPTIMIZER_H_
2 #define _OPTIMIZER_H_
3 
4 #include "gmapping/utils/point.h"
5 
6 namespace GMapping {
7 
8 struct OptimizerParams{
9  double discretization;
10  double angularStep, linearStep;
11  int iterations;
12  double maxRange;
13 };
14 
15 template <typename Likelihood, typename Map>
16 struct Optimizer {
17  Optimizer(const OptimizerParams& params);
18  OptimizerParams params;
19  Map lmap;
20  Likelihood likelihood;
21  OrientedPoint gradientDescent(const RangeReading& oldReading, const RangeReading& newReading);
22  OrientedPoint gradientDescent(const RangeReading& oldReading, const OrientedPoint& pose, OLocalMap& Map);
23  enum Move {Forward, Backward, Left, Right, TurnRight, TurnLeft};
24 };
25 
26 template <typename Likelihood, typename Map>
27 Optimizer<Likelihood, Map>::Optimizer(const OptimizerParams& p):
28  params(p),
29  lmap(p.discretization){}
30 
31 template <typename Likelihood, typename Map>
32 OrientedPoint Optimizer<Likelihood, Map>::gradientDescent(const RangeReading& oldReading, const RangeReading& newReading){
33  lmap.clear();
34  lmap.update(oldReading, OrientedPoint(0,0,0), params.maxRange);
35  OrientedPoint delta=absoluteDifference(newReading.getPose(), oldReading.getPose());
36  OrientedPoint bestPose=delta;
37  double bestScore=likelihood(lmap, newReading, bestPose, params.maxRange);
38  int it=0;
39  double lstep=params.linearStep, astep=params.angularStep;
40  bool increase;
41 /* cerr << "bestScore=" << bestScore << endl;;*/
42  do {
43  increase=false;
44  OrientedPoint itBestPose=bestPose;
45  double itBestScore=bestScore;
46  bool itIncrease;
47  do {
48  itIncrease=false;
49  OrientedPoint testBestPose=itBestPose;
50  double testBestScore=itBestScore;
51  for (Move move=Forward; move<=TurnLeft; move=(Move)((int)move+1)){
52  OrientedPoint testPose=itBestPose;
53  switch(move){
54  case Forward: testPose.x+=lstep;
55  break;
56  case Backward: testPose.x-=lstep;
57  break;
58  case Left: testPose.y+=lstep;
59  break;
60  case Right: testPose.y-=lstep;
61  break;
62  case TurnRight: testPose.theta-=astep;
63  break;
64  case TurnLeft: testPose.theta+=astep;
65  break;
66  }
67  double score=likelihood(lmap, newReading, testPose, params.maxRange);
68  if (score>testBestScore){
69  testBestScore=score;
70  testBestPose=testPose;
71  }
72  }
73  if (testBestScore > itBestScore){
74  itBestScore=testBestScore;
75  itBestPose=testBestPose;
76 /* cerr << "s=" << itBestScore << " ";*/
77  itIncrease=true;
78  }
79  } while(itIncrease);
80  if (itBestScore > bestScore){
81 /* cerr << "S(" << itBestScore << "," << bestScore<< ")";*/
82  bestScore=itBestScore;
83  bestPose=itBestPose;
84  increase=true;
85  } else {
86  it++;
87  lstep*=0.5;
88  astep*=0.5;
89  }
90  } while (it<params.iterations);
91 /* cerr << "FinalBestScore" << bestScore << endl;*/
92  cerr << endl;
93  return bestPose;
94 }
95 
96 template <typename Likelihood, typename Map>
97 OrientedPoint Optimizer<Likelihood, Map>::gradientDescent(const RangeReading& reading, const OrientedPoint& pose, OLocalMap& lmap){
98  OrientedPoint bestPose=pose;
99  double bestScore=likelihood(lmap, reading, bestPose, params.maxRange);
100  int it=0;
101  double lstep=params.linearStep, astep=params.angularStep;
102  bool increase;
103 /* cerr << "bestScore=" << bestScore << endl;;*/
104  do {
105  increase=false;
106  OrientedPoint itBestPose=bestPose;
107  double itBestScore=bestScore;
108  bool itIncrease;
109  do {
110  itIncrease=false;
111  OrientedPoint testBestPose=itBestPose;
112  double testBestScore=itBestScore;
113  for (Move move=Forward; move<=TurnLeft; move=(Move)((int)move+1)){
114  OrientedPoint testPose=itBestPose;
115  switch(move){
116  case Forward: testPose.x+=lstep;
117  break;
118  case Backward: testPose.x-=lstep;
119  break;
120  case Left: testPose.y+=lstep;
121  break;
122  case Right: testPose.y-=lstep;
123  break;
124  case TurnRight: testPose.theta-=astep;
125  break;
126  case TurnLeft: testPose.theta+=astep;
127  break;
128  }
129  double score=likelihood(lmap, reading, testPose, params.maxRange);
130  if (score>testBestScore){
131  testBestScore=score;
132  testBestPose=testPose;
133  }
134  }
135  if (testBestScore > itBestScore){
136  itBestScore=testBestScore;
137  itBestPose=testBestPose;
138 /* cerr << "s=" << itBestScore << " ";*/
139  itIncrease=true;
140  }
141  } while(itIncrease);
142  if (itBestScore > bestScore){
143 /* cerr << "S(" << itBestScore << "," << bestScore<< ")";*/
144  bestScore=itBestScore;
145  bestPose=itBestPose;
146  increase=true;
147  } else {
148  it++;
149  lstep*=0.5;
150  astep*=0.5;
151  }
152  } while (it<params.iterations);
153 /* cerr << "FinalBestScore" << bestScore << endl;*/
154  cerr << endl;
155  return bestPose;
156 }
157 
158 } // end namespace GMapping
159 #endif
delta
const char *const *argv double delta
Definition: gfs2stream.cpp:19
GMapping::Optimizer::likelihood
Likelihood likelihood
Definition: optimizer.h:20
GMapping::absoluteDifference
orientedpoint< T, A > absoluteDifference(const orientedpoint< T, A > &p1, const orientedpoint< T, A > &p2)
Definition: point.h:107
GMapping::Optimizer::Optimizer
Optimizer(const OptimizerParams &params)
Definition: optimizer.h:27
GMapping::Optimizer::params
OptimizerParams params
Definition: optimizer.h:18
GMapping::Optimizer::gradientDescent
OrientedPoint gradientDescent(const RangeReading &oldReading, const RangeReading &newReading)
Definition: optimizer.h:32
GMapping::RangeReading::getPose
const OrientedPoint & getPose() const
Definition: rangereading.h:22
GMapping::OrientedPoint
orientedpoint< double, double > OrientedPoint
Definition: point.h:203

openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20