15 template <
typename Likelihood,
typename Map>
26 template <
typename Likelihood,
typename Map>
29 lmap(p.discretization){}
31 template <
typename Likelihood,
typename Map>
34 lmap.update(oldReading,
OrientedPoint(0,0,0), params.maxRange);
37 double bestScore=likelihood(lmap, newReading, bestPose, params.maxRange);
39 double lstep=params.linearStep, astep=params.angularStep;
45 double itBestScore=bestScore;
50 double testBestScore=itBestScore;
51 for (
Move move=Forward; move<=TurnLeft; move=(
Move)((
int)move+1)){
54 case Forward: testPose.
x+=lstep;
56 case Backward: testPose.
x-=lstep;
58 case Left: testPose.
y+=lstep;
60 case Right: testPose.
y-=lstep;
62 case TurnRight: testPose.
theta-=astep;
64 case TurnLeft: testPose.
theta+=astep;
67 double score=likelihood(lmap, newReading, testPose, params.maxRange);
68 if (score>testBestScore){
70 testBestPose=testPose;
73 if (testBestScore > itBestScore){
74 itBestScore=testBestScore;
75 itBestPose=testBestPose;
80 if (itBestScore > bestScore){
82 bestScore=itBestScore;
90 }
while (it<params.iterations);
96 template <
typename Likelihood,
typename Map>
99 double bestScore=likelihood(lmap, reading, bestPose, params.maxRange);
101 double lstep=params.linearStep, astep=params.angularStep;
107 double itBestScore=bestScore;
112 double testBestScore=itBestScore;
113 for (
Move move=Forward; move<=TurnLeft; move=(
Move)((
int)move+1)){
116 case Forward: testPose.
x+=lstep;
118 case Backward: testPose.
x-=lstep;
120 case Left: testPose.
y+=lstep;
122 case Right: testPose.
y-=lstep;
124 case TurnRight: testPose.
theta-=astep;
126 case TurnLeft: testPose.
theta+=astep;
129 double score=likelihood(lmap, reading, testPose, params.maxRange);
130 if (score>testBestScore){
132 testBestPose=testPose;
135 if (testBestScore > itBestScore){
136 itBestScore=testBestScore;
137 itBestPose=testBestPose;
142 if (itBestScore > bestScore){
144 bestScore=itBestScore;
152 }
while (it<params.iterations);