Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/SelfLocator/GT2004SelfLocator.cpp

Go to the documentation of this file.

    00001 /**
    00002  * @file Modules/SelfLocator/GT2004SelfLocator.cpp
    00003  *
    00004  * This file contains a class for self-localization based on the Monte Carlo approach
    00005  * using goals, landmarks, and field lines.
    00006  *
    00007  * @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
    00008  */
    00009
    00010 #include "Platform/GTAssert.h"
    00011 #include "GT2004SelfLocator.h"
    00012 #include "Platform/SystemCall.h"
    00013 #include "Tools/FieldDimensions.h"
    00014 #include "Tools/Player.h"
    00015 #include "Tools/Debugging/Debugging.h"
    00016 #include "Tools/Math/Geometry.h"
    00017 #include "Tools/Streams/InStreams.h"
    00018 #include "Tools/Debugging/GenericDebugData.h"
    00019
    00020 double GT2004SelfLocator::paramUp = 0.01, // step size for increasing qualities
    00021        GT2004SelfLocator::paramDown = 0.005, // step size for decreasing qualities
    00022        GT2004SelfLocator::paramDelay = 3, // factor
    00023        GT2004SelfLocator::paramHeight = 150, // hypothetic height of head
    00024        GT2004SelfLocator::paramZ = 10, // sharpness of Gauss for rotational errors
    00025        GT2004SelfLocator::paramY = 2; // sharpness of Gauss for translational errors
    00026
    00027 GT2004SelfLocator::Sample::Sample()
    00028 {
    00029   for(int i = 0; i < numberOfQualities; ++i)
    00030     quality[i] = 0.5;
    00031   probability = 2;
    00032 }
    00033
    00034 GT2004SelfLocator::Sample::Sample(const Pose2D& pose,const double* quality)
    00035 : PoseSample(pose)
    00036 {
    00037   for(int i = 0; i < numberOfQualities; ++i)
    00038     this->quality[i] = 2;
    00039   probability = 2;
    00040 }
    00041
    00042 void GT2004SelfLocator::Sample::updateQuality(const double* average)
    00043 {
    00044   probability = 1;
    00045   for(int i = 0; i < numberOfQualities; ++i)
    00046     probability *= quality[i] == 2 ? max(average[i] - paramDelay * paramUp, 0.000001) : quality[i];
    00047 }
    00048
    00049 void GT2004SelfLocator::Sample::setProbability(LinesPercept::LineType type,double value)
    00050 {
    00051   double& q = quality[type > LinesPercept::yellowGoal ? type - 1 : type];
    00052   if(type > LinesPercept::yellowGoal)
    00053   {
    00054     if(q == 2)
    00055       q = value;
    00056     else if(value < q)
    00057       if(value < q - 0.05)
    00058         q -= 0.05;
    00059       else
    00060         q = value;
    00061     else
    00062       if(value > q + 0.1)
    00063         q += 0.1;
    00064       else
    00065         q = value;
    00066   }
    00067   else
    00068   {
    00069     if(q == 2)
    00070       q = max(value - paramDelay * paramUp, 0.000001);
    00071     else if(value < q)
    00072       if(value < q - paramDown)
    00073         q -= paramDown;
    00074       else
    00075         q = value;
    00076     else
    00077       if(value > q + paramUp)
    00078         q += paramUp;
    00079       else
    00080         q = value;
    00081   }
    00082 }
    00083
    00084 GT2004SelfLocator::GT2004SelfLocator(const SelfLocatorInterfaces& interfaces)
    00085 : SelfLocator(interfaces)
    00086 {
    00087   int i;
    00088   for(i = 0; i < Sample::numberOfQualities; ++i)
    00089     average[i] = 0.5;
    00090   for(i = 0; i < SAMPLES_MAX; ++i)
    00091     (Pose2D&) sampleSet[i] = field.randomPose();
    00092   numberOfTypes = 0;
    00093   numOfFlags = 0;
    00094   sensorUpdated = false;
    00095   timeStamp = 0;
    00096   speed = 0;
    00097 }
    00098
    00099 void GT2004SelfLocator::execute()
    00100 {
    00101   NDECLARE_DEBUGDRAWING( "gt04-sl" , "drawingOnField" , "GT2004SelfLocator debug drawings");
    00102   const int maxPercepts = 3;
    00103 /*  observationTable[1].draw();
    00104   DEBUG_DRAWING_FINISHED(selfLocatorField);
    00105   return;*/
    00106   robotPose.setFrameNumber(landmarksPercept.frameNumber);
    00107   robotPose.setTimestamp(SystemCall::getCurrentSystemTime());
    00108
    00109   Pose2D odometry = odometryData - lastOdometry;
    00110   lastOdometry = odometryData;
    00111   if(SystemCall::getTimeSince(timeStamp) > 500)
    00112   {
    00113     speed = (odometryData - lastOdometry2).translation.abs() / SystemCall::getTimeSince(timeStamp) * 1000.0;
    00114     lastOdometry2 = odometryData;
    00115     timeStamp = SystemCall::getCurrentSystemTime();
    00116   }
    00117   paramZ = 10 - 9 * speed / 400;
    00118
    00119   // First step: action update
    00120   updateByOdometry(odometry,
    00121                    Pose2D(landmarksPercept.cameraOffset.x,
    00122                           landmarksPercept.cameraOffset.y),
    00123                           sensorUpdated);
    00124
    00125   // Second step: observation update
    00126   numberOfTypes = 0;
    00127   sensorUpdated = false;
    00128   if(cameraMatrix.isValid)
    00129   {
    00130     int i;
    00131     for(i = 0; i < 2; ++i)
    00132       for(int j = 0; j < linesPercept.points[i].numberOfPoints && j < maxPercepts; ++j)
    00133       {
    00134         updateByPoint(linesPercept.points[i].pointsOnField[rand() % linesPercept.points[i].numberOfPoints],
    00135                       LinesPercept::LineType(i));
    00136         sensorUpdated = true;
    00137       }
    00138
    00139     if(linesPercept.points[LinesPercept::yellowGoal].numberOfPoints)
    00140       types[numberOfTypes++] = LinesPercept::yellowGoal;
    00141     if(linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints)
    00142       types[numberOfTypes++] = LinesPercept::skyblueGoal;
    00143
    00144     if(linesPercept.points[LinesPercept::yellowGoal].numberOfPoints ||
    00145        linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints)
    00146       for(int j = 0; j < linesPercept.points[LinesPercept::yellowGoal].numberOfPoints +
    00147                          linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints && j < maxPercepts; ++j)
    00148       { // This works if only one goal or both were seen
    00149         LinesPercept::LineType select = (random() + 1e-6) * linesPercept.points[LinesPercept::yellowGoal].numberOfPoints >
    00150                                         linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints
    00151                                         ? LinesPercept::yellowGoal
    00152                                         : LinesPercept::skyblueGoal;
    00153         updateByPoint(linesPercept.points[select].pointsOnField[rand() % linesPercept.points[select].numberOfPoints],
    00154                       select);
    00155         sensorUpdated = true;
    00156       }
    00157     for(i = 0; i < landmarksPercept.numberOfFlags; ++i)
    00158     {
    00159       const Flag& flag = landmarksPercept.flags[i];
    00160       if(!flag.isOnBorder(flag.x.max))
    00161         updateByFlag(flag.position,
    00162                      LEFT_SIDE_OF_FLAG,
    00163                      flag.x.max);
    00164       if(!flag.isOnBorder(flag.x.min))
    00165         updateByFlag(flag.position,
    00166                      RIGHT_SIDE_OF_FLAG,
    00167                      flag.x.min);
    00168     }
    00169     for(i = 0; i < landmarksPercept.numberOfGoals; ++i)
    00170     {
    00171       const Goal& goal = landmarksPercept.goals[i];
    00172       if(!goal.isOnBorder(goal.x.max))
    00173         updateByGoalPost(goal.leftPost,
    00174                          goal.x.max);
    00175       if(!goal.isOnBorder(goal.x.min))
    00176         updateByGoalPost(goal.rightPost,
    00177                          goal.x.min);
    00178     }
    00179
    00180     if(sensorUpdated)
    00181     {
    00182       // generate templates for calculated sampleSet
    00183       generatePoseTemplates(landmarksPercept,odometry);
    00184       resample();
    00185     }
    00186   }
    00187
    00188   Pose2D pose;
    00189   double validity;
    00190   calcPose(pose,validity);
    00191   robotPose.setPose(pose);
    00192   robotPose.setValidity(validity);
    00193   sampleSet.link(selfLocatorSamples);
    00194
    00195   //COMPLEX_DRAWING(selfLocator,drawEllipse());
    00196
    00197
    00198   DEBUG_DRAWING_FINISHED(selfLocatorField);
    00199   DEBUG_DRAWING_FINISHED(selfLocator);
    00200
    00201   landmarksState.update(landmarksPercept);
    00202 }
    00203
    00204 static double fmax(double a,double b)
    00205 {
    00206   return a> b ? a : b;
    00207 }
    00208
    00209 void GT2004SelfLocator::updateByOdometry(const Pose2D& odometry,
    00210                                          const Pose2D& camera,
    00211                                          bool noise)
    00212 {
    00213   double transNoise = noise ? 200 : 0,
    00214          rotNoise = noise ? 0.5 : 0;
    00215   double dist = odometry.translation.abs();
    00216   double angle = fabs(odometry.getAngle());
    00217   double f;
    00218
    00219   for(int i = 0; i < SAMPLES_MAX; ++i)
    00220   {
    00221     Sample& s = sampleSet[i];
    00222     s += odometry;
    00223     if(s.probability == 2)
    00224       f = 0;
    00225     else
    00226       f = (1 - s.probability) * (1 - s.probability);
    00227     double rotError = ((random() * 2 - 1) * fmax(fmax(dist * 0.002,angle * 0.2),rotNoise * f));
    00228     Vector2<double> transError((random() * 2 - 1) * max(fmax(fabs(odometry.translation.x) * 0.1,
    00229                                                              fabs(odometry.translation.y) * 0.02),
    00230                                                         transNoise * f),
    00231                                (random() * 2 - 1) * max(fmax(fabs(odometry.translation.y) * 0.1,
    00232                                                              fabs(odometry.translation.x) * 0.02),
    00233                                                         transNoise * f));
    00234     s += Pose2D(rotError,transError);
    00235     s.camera = s + camera;
    00236   }
    00237 }
    00238
    00239 void GT2004SelfLocator::updateByPoint(const LinesPercept::LinePoint& point,LinesPercept::LineType type)
    00240 {
    00241   NCOMPLEX_DRAWING("gt04-sl", draw(point,type));
    00242
    00243   int index = type;
    00244   if(getPlayer().getTeamColor() == Player::blue && type >= LinesPercept::yellowGoal)
    00245     index = LinesPercept::yellowGoal + LinesPercept::skyblueGoal - type;
    00246   double d = point.abs();
    00247   double zObs = atan2(paramHeight,d),
    00248          yObs = atan2((double)point.y,(double)point.x);
    00249   Pose2D point2(point.angle, Vector2<double>(point.x, point.y));
    00250   for(int i = 0; i < SAMPLES_MAX; ++i)
    00251   {
    00252     Sample& s = sampleSet[i];
    00253     Pose2D p = s + point2;
    00254     Vector2<double> vMin;
    00255     bool isY = index == LinesPercept::field && (fabs(p.getAngle()) < pi / 4 || fabs(p.getAngle()) > 3 * pi / 4);
    00256     if(isY)
    00257       vMin = observationTable[LinesPercept::numberOfLineTypes].getClosestPoint(p.translation);
    00258     else
    00259       vMin = observationTable[index].getClosestPoint(p.translation);
    00260     if(vMin.x != 1000000)
    00261     {
    00262       Vector2<double> diff = vMin - p.translation;
    00263       Vector2<double> v = (Pose2D(vMin) - s).translation;
    00264       double zModel = atan2(paramHeight,v.abs()),
    00265              yModel = atan2(v.y,v.x);
    00266       s.setProbability(isY ? LinesPercept::LineType(LinesPercept::numberOfLineTypes + 1) : type, sigmoid((zModel - zObs) * paramZ) * sigmoid((yModel - yObs) * paramY));
    00267     }
    00268     else
    00269       s.setProbability(type,0.000001);
    00270   }
    00271 }
    00272
    00273 void GT2004SelfLocator::updateByFlag(const Vector2<double>& flag,
    00274                                      FlagSides sideOfFlag,
    00275                                      double measuredBearing)
    00276 {
    00277   sensorUpdated = true;
    00278   for(int i = 0; i < SAMPLES_MAX; ++i)
    00279   {
    00280     Sample& s = sampleSet[i];
    00281     Pose2D p(flag.x,flag.y);
    00282     p -= s.camera;
    00283     if (p.translation.abs() > flagRadius)
    00284     {
    00285       double assumedBearing = atan2(p.translation.y,p.translation.x) + sideOfFlag * asin(flagRadius / p.translation.abs());
    00286       double similarity = fabs((assumedBearing - measuredBearing)) / pi;
    00287       if(similarity > 1)
    00288         similarity = 2.0 - similarity;
    00289       s.setProbability(LinesPercept::numberOfLineTypes,sigmoid(similarity * 7));
    00290     }
    00291     else
    00292       s.setProbability(LinesPercept::numberOfLineTypes,0.000001);
    00293   }
    00294 }
    00295
    00296 void GT2004SelfLocator::updateByGoalPost(const Vector2<double>& goalPost,
    00297                                          double measuredBearing)
    00298 {
    00299   sensorUpdated = true;
    00300   for(int i = 0; i < SAMPLES_MAX; ++i)
    00301   {
    00302     Sample& s = sampleSet[i];
    00303     Pose2D p(goalPost.x,goalPost.y);
    00304     p -= s.camera;
    00305     double assumedBearing = atan2(p.translation.y,p.translation.x);
    00306     double similarity = fabs((assumedBearing - measuredBearing)) / pi;
    00307     if(similarity > 1)
    00308       similarity = 2 - similarity;
    00309     s.setProbability(LinesPercept::numberOfLineTypes,sigmoid(similarity * 7));
    00310   }
    00311 }
    00312
    00313 void GT2004SelfLocator::resample()
    00314 {
    00315   int i,j,
    00316       count[Sample::numberOfQualities];
    00317
    00318   for(i = 0; i < Sample::numberOfQualities; ++i)
    00319   {
    00320     average[i] = 0;
    00321     count[i] = 0;
    00322   }
    00323
    00324   for(i = 0; i < SAMPLES_MAX; ++i)
    00325   {
    00326     Sample& s = sampleSet[i];
    00327     for(j = 0; j < Sample::numberOfQualities; ++j)
    00328       if(s.quality[j] != 2)
    00329       {
    00330         average[j] += s.quality[j];
    00331         ++count[j];
    00332       }
    00333   }
    00334
    00335   double average2 = 1;
    00336   for(i = 0; i < Sample::numberOfQualities; ++i)
    00337   {
    00338     if(count[i])
    00339       average[i] /= count[i];
    00340     else
    00341       average[i] = 0.5;
    00342     average2 *= average[i];
    00343   }
    00344
    00345   // move the quality of all samples towards the probability
    00346   for(i = 0; i < SAMPLES_MAX; ++i)
    00347     sampleSet[i].updateQuality(average);
    00348
    00349   // swap sample arrays
    00350   Sample* oldSet = sampleSet.swap();
    00351
    00352   double probSum = 0;
    00353   for(i = 0; i < SAMPLES_MAX; ++i)
    00354     probSum += oldSet[i].getQuality();
    00355   double prob2Index = (double) SAMPLES_MAX / probSum;
    00356
    00357   double indexSum = 0;
    00358   j = 0;
    00359   for(i = 0; i < SAMPLES_MAX; ++i)
    00360   {
    00361     indexSum += oldSet[i].getQuality() * prob2Index;
    00362     while(j < SAMPLES_MAX && j < indexSum - 0.5)
    00363       sampleSet[j++] = oldSet[i];
    00364   }
    00365
    00366   for(i = 0; i < SAMPLES_MAX; ++i)
    00367   {
    00368     Sample& s = sampleSet[i];
    00369     // if quality is too low, select a new random pose for the sample
    00370     if(numOfTemplates && random() * average2 > s.getQuality())
    00371       s = getTemplate();
    00372     else
    00373     {
    00374       NCOMPLEX_DRAWING("gt04-sl",draw(s));
    00375     }
    00376   }
    00377 }
    00378
    00379 void GT2004SelfLocator::calcPose(Pose2D& pose,double& validity)
    00380 {
    00381   Cell cells[GRID_MAX][GRID_MAX][GRID_MAX];
    00382   double width = field.x.getSize(),
    00383          height = field.y.getSize();
    00384
    00385   // attach the samples to the corresponding grid cells
    00386   for(int i = 0; i < SAMPLES_MAX; ++i)
    00387   {
    00388     Sample& p = sampleSet[i];
    00389     if(p.isValid())
    00390     {
    00391       int r = static_cast<int>((p.getAngle() / pi2 + 0.5) * GRID_MAX),
    00392           y = static_cast<int>((p.translation.y / height + 0.5) * GRID_MAX),
    00393           x = static_cast<int>((p.translation.x / width + 0.5) * GRID_MAX);
    00394       if(x < 0)
    00395         x = 0;
    00396       else if(x >= GRID_MAX)
    00397         x = GRID_MAX-1;
    00398       if(y < 0)
    00399         y = 0;
    00400       else if(y >= GRID_MAX)
    00401         y = GRID_MAX-1;
    00402       if(r < 0)
    00403         r = GRID_MAX-1;
    00404       else if(r >= GRID_MAX)
    00405         r = 0;
    00406       Cell& c = cells[r][y][x];
    00407       p.next = c.first;
    00408       c.first = &p;
    00409       ++c.count;
    00410     }
    00411   }
    00412
    00413   // determine the 2x2x2 sub-cube that contains most samples
    00414   int rMax = 0,
    00415       yMax = 0,
    00416       xMax = 0,
    00417       countMax = 0,
    00418       r;
    00419   for(r = 0; r < GRID_MAX; ++r)
    00420   {
    00421     int rNext = (r + 1) % GRID_MAX;
    00422     for(int y = 0; y < GRID_MAX - 1; ++y)
    00423       for(int x = 0; x < GRID_MAX - 1; ++x)
    00424       {
    00425         int count = cells[r][y][x].count +
    00426                     cells[r][y][x+1].count +
    00427                     cells[r][y+1][x].count +
    00428                     cells[r][y+1][x+1].count +
    00429                     cells[rNext][y][x].count +
    00430                     cells[rNext][y][x+1].count +
    00431                     cells[rNext][y+1][x].count +
    00432                     cells[rNext][y+1][x+1].count;
    00433         if(count > countMax)
    00434         {
    00435           countMax = count;
    00436           rMax = r;
    00437           yMax = y;
    00438           xMax = x;
    00439         }
    00440       }
    00441   }
    00442
    00443   if(countMax > 0)
    00444   {
    00445     // determine the average pose of all samples in the winner sub-cube
    00446     double xSum = 0,
    00447                   ySum = 0,
    00448                   cosSum = 0,
    00449                   sinSum = 0,
    00450                   qualitySum = 0;
    00451     for(r = 0; r < 2; ++r)
    00452     {
    00453       int r2 = (rMax + r) % GRID_MAX;
    00454       for(int y = 0; y < 2; ++y)
    00455         for(int x = 0; x < 2; ++x)
    00456         {
    00457           for(Sample* p = cells[r2][yMax + y][xMax + x].first; p; p = p->next)
    00458             if(p->isValid())
    00459             {
    00460               xSum += p->translation.x * p->getQuality();
    00461               ySum += p->translation.y * p->getQuality();
    00462               cosSum += p->getCos() * p->getQuality();
    00463               sinSum += p->getSin() * p->getQuality();
    00464               qualitySum += p->getQuality();
    00465             }
    00466         }
    00467     }
    00468     if(qualitySum)
    00469     {
    00470       pose = Pose2D(atan2(sinSum,cosSum),
    00471                             Vector2<double>(xSum / qualitySum,ySum / qualitySum));
    00472       validity = qualitySum / SAMPLES_MAX;
    00473       double diff = field.clip(pose.translation);
    00474       if(diff > 1)
    00475         validity /= sqrt(diff);
    00476       return;
    00477     }
    00478   }
    00479   validity = 0;
    00480 }
    00481
    00482 ///////////////////////////////////////////////////////////////////////
    00483 // New samples, i.e. random and templates
    00484
    00485 bool GT2004SelfLocator::poseFromBearings(double dir0,double dir1,double dir2,
    00486                                          const Vector2<double>& mark0,
    00487                                          const Vector2<double>& mark1,
    00488                                          const Vector2<double>& mark2,
    00489                                          const Vector2<double>& cameraOffset,
    00490                                          Pose2D& resultingPose) const
    00491 {
    00492   double w1 = dir1 - dir0,
    00493          w2 = dir2 - dir1;
    00494   if(sin(w1) != 0 && sin(w2) != 0)
    00495   {
    00496     Vector2<double> p = mark2 - mark1;
    00497     double a2 = p.abs();
    00498     p = (Pose2D(mark0) - Pose2D(atan2(p.y,p.x),mark1)).translation;
    00499     double a1 = p.abs();
    00500     if(a1 > 0 && a2 > 0)
    00501     {
    00502       double w3 = pi2 - w1 - w2 - atan2(p.y,p.x);
    00503       double t = a1 * sin(w2) / (a2 * sin(w1)) + cos(w3);
    00504       if(t != 0)
    00505       {
    00506         double w = atan(sin(w3) / t);
    00507         double b = a1 * sin(w) / sin(w1);
    00508         w = pi - w1 - w;
    00509         p = mark0 - mark1;
    00510         resultingPose = Pose2D(atan2(p.y,p.x) - w,mark1)
    00511                         + Pose2D(Vector2<double>(b,0));
    00512         p = (Pose2D(mark1) - resultingPose).translation;
    00513         resultingPose += Pose2D(atan2(p.y,p.x) - dir1);
    00514         resultingPose += Pose2D(Vector2<double>(-cameraOffset.x,
    00515                                                 -cameraOffset.y));
    00516         return true;
    00517       }
    00518     }
    00519   }
    00520   return false;
    00521 }
    00522
    00523 int GT2004SelfLocator::poseFromBearingsAndDistance(
    00524   double dir0,double dir1,double dist,
    00525   const Vector2<double>& mark0,
    00526   const Vector2<double>& mark1,
    00527   const Vector2<double>& cameraOffset,
    00528   Pose2D& resultingPose1,
    00529   Pose2D& resultingPose2) const
    00530 {
    00531   double alpha = dir0 - dir1;
    00532   Vector2<double> diff = mark0 - mark1;
    00533   double dir10 = atan2(diff.y,diff.x);
    00534   double f = dist * sin(alpha) / diff.abs();
    00535   if(fabs(f) <= 1)
    00536   {
    00537     double gamma = asin(f),
    00538            beta = pi - alpha - gamma;
    00539     resultingPose1 = Pose2D(dir10 + beta,mark1) +
    00540                      Pose2D(pi - dir1,Vector2<double>(dist,0)) +
    00541                      Pose2D(Vector2<double>(-cameraOffset.x,
    00542                             -cameraOffset.y));
    00543     if(fabs(alpha) < pi_2 || fabs(alpha) > 3 * pi_2)
    00544     {
    00545       if(gamma > 0)
    00546         gamma = pi - gamma;
    00547       else
    00548         gamma = -pi - gamma;
    00549       beta = pi - alpha - gamma;
    00550       resultingPose2 = Pose2D(dir10 + beta,mark1) +
    00551                        Pose2D(pi - dir1,Vector2<double>(dist,0)) +
    00552                        Pose2D(Vector2<double>(-cameraOffset.x,
    00553                               -cameraOffset.y));
    00554       return 2;
    00555     }
    00556     else
    00557       return 1;
    00558   }
    00559   else
    00560     return 0;
    00561 }
    00562
    00563 bool GT2004SelfLocator::getBearing(const LandmarksPercept& landmarksPercept,int i,
    00564                                    Vector2<double>& mark,
    00565                                    double& dir,double& dist) const
    00566 {
    00567   if(i < numOfFlags)
    00568   {
    00569     const Flag& flag = flags[i];
    00570     mark = flag.position;
    00571     dir = flag.angle;
    00572     dist = flag.distance;
    00573     return true;
    00574   }
    00575   else
    00576   {
    00577     i -= numOfFlags;
    00578     const Goal& goal = landmarksPercept.goals[i >> 1];
    00579     if((i & 1) == 0 && !goal.isOnBorder(goal.x.min))
    00580     {
    00581       mark = goal.rightPost;
    00582       dir = goal.x.min;
    00583       dist = goal.distance;
    00584       return true;
    00585     }
    00586     else if((i & 1) == 1 && !goal.isOnBorder(goal.x.max))
    00587     {
    00588       mark = goal.leftPost;
    00589       dir = goal.x.max;
    00590       dist = goal.distance;
    00591       return true;
    00592     }
    00593     else
    00594       return false;
    00595   }
    00596 }
    00597
    00598 void GT2004SelfLocator::addFlag(const Flag& flag)
    00599 {
    00600   if(!flag.isOnBorder(flag.x.min) && !flag.isOnBorder(flag.x.max))
    00601   {
    00602     int i;
    00603     for(i = 0; i < numOfFlags && flags[i].type != flag.type; ++i)
    00604       ;
    00605     if(i < numOfFlags)
    00606       --numOfFlags; // remove one
    00607     if(i < FLAGS_MAX)
    00608       ++i; // still room at the end
    00609     while(--i > 0)
    00610       flags[i] = flags[i - 1];
    00611     flags[0] = flag;
    00612     if(numOfFlags < FLAGS_MAX)
    00613       ++numOfFlags; // one was added
    00614   }
    00615 }
    00616
    00617 void GT2004SelfLocator::generatePoseTemplates(const LandmarksPercept& landmarksPercept,
    00618                                               const Pose2D& odometry)
    00619 {
    00620   randomFactor = 0;
    00621   int i;
    00622   for(i = 0; i < numOfFlags; ++i)
    00623   {
    00624     Flag& flag = flags[i];
    00625     Vector2<double> p = (Pose2D(flag.angle)
    00626       + Pose2D(Vector2<double>(flag.distance,0)) - odometry).translation;
    00627     flag.angle = atan2(p.y,p.x);
    00628     flag.distance = p.abs();
    00629   }
    00630   for(i = 0; i < landmarksPercept.numberOfFlags; ++i)
    00631     addFlag(landmarksPercept.flags[i]);
    00632
    00633   numOfTemplates = 0;
    00634   nextTemplate = 0;
    00635   double dir0,
    00636          dir1,
    00637          dir2;
    00638   double dist0,
    00639          dist1;
    00640   Vector2<double> mark0,
    00641                   mark1,
    00642                   mark2,
    00643                   cameraOffset(landmarksPercept.cameraOffset.x,
    00644                                landmarksPercept.cameraOffset.y);
    00645   int n = numOfFlags + 2 * landmarksPercept.numberOfGoals;
    00646   for(i = 0; i < n - 2; ++i)
    00647     if(getBearing(landmarksPercept,i,mark0,dir0,dist0))
    00648       for(int j = i + 1; j < n - 1; ++j)
    00649         if(getBearing(landmarksPercept,j,mark1,dir1,dist1))
    00650         {
    00651           if(numOfTemplates >= SAMPLES_MAX - 4)
    00652             return;
    00653           numOfTemplates += poseFromBearingsAndDistance(dir0,dir1,dist1,mark0,mark1,cameraOffset,
    00654                                                         templates[numOfTemplates],templates[numOfTemplates + 1]);
    00655           numOfTemplates += poseFromBearingsAndDistance(dir1,dir0,dist0,mark1,mark0,cameraOffset,
    00656                                                         templates[numOfTemplates],templates[numOfTemplates + 1]);
    00657           for(int k = j + 1; k < n; ++k)
    00658             if(getBearing(landmarksPercept,k,mark2,dir2,dist0))
    00659               if(poseFromBearings(dir0,dir1,dir2,mark0,mark1,mark2,cameraOffset,
    00660                                   templates[numOfTemplates]))
    00661                 if(++numOfTemplates == SAMPLES_MAX)
    00662                   return;
    00663         }
    00664   for(i = 0; i < numberOfTypes; ++i)
    00665   {
    00666     LinesPercept::LineType type = types[i];
    00667     for(int j = 0; j < linesPercept.points[type].numberOfPoints; ++j)
    00668     {
    00669       const Vector2<int>& point = linesPercept.points[type].pointsOnField[j];
    00670       int index = type;
    00671       if(getPlayer().getTeamColor() == Player::blue && type >= LinesPercept::yellowGoal)
    00672         index = LinesPercept::yellowGoal + LinesPercept::skyblueGoal - type;
    00673       templates[numOfTemplates++] = templateTable[index - LinesPercept::yellowGoal].sample(point.abs()) + Pose2D(-atan2((double)point.y,(double)point.x));
    00674       if(numOfTemplates == SAMPLES_MAX)
    00675         return;
    00676     }
    00677   }
    00678 }
    00679
    00680 GT2004SelfLocator::Sample GT2004SelfLocator::getTemplate()
    00681 {
    00682   if(nextTemplate >= numOfTemplates)
    00683   {
    00684     nextTemplate = 0;
    00685     ++randomFactor;
    00686   }
    00687   while(nextTemplate < numOfTemplates)
    00688   {
    00689     const Pose2D& t = templates[nextTemplate++];
    00690     if(field.isInside(t.translation))
    00691     {
    00692       return Sample(Pose2D::random(Range<double>(t.translation.x - randomFactor * 50,
    00693                                                  t.translation.x + randomFactor * 50),
    00694                                    Range<double>(t.translation.y - randomFactor * 50,
    00695                                                  t.translation.y + randomFactor * 50),
    00696                                    Range<double>(t.getAngle() - randomFactor * 0.1,
    00697                                                  t.getAngle() + randomFactor * 0.1)),
    00698                     average);
    00699     }
    00700   }
    00701   return Sample(field.randomPose(),average);
    00702 }
    00703
    00704 ///////////////////////////////////////////////////////////////////////
    00705 // Debug drawing
    00706
    00707 void GT2004SelfLocator::draw(const Sample& pose) const
    00708 {
    00709   Drawings::Color color;
    00710
    00711   if (pose.probability == 2 || pose.probability == 0)
    00712     color = Drawings::black;
    00713   else if (pose.probability < 0.1)
    00714     color = Drawings::blue;
    00715   else if (pose.probability < 0.2)
    00716     color = Drawings::yellow;
    00717   else if (pose.probability < 0.5)
    00718     color = Drawings::orange;
    00719   else if (pose.probability < 0.8)
    00720     color = Drawings::red;
    00721   else
    00722     color = Drawings::pink;
    00723
    00724   Pose2D p = pose + Pose2D(-100,0);
    00725   NLINE("gt04-sl",
    00726        pose.translation.x,
    00727        pose.translation.y,
    00728        p.translation.x,
    00729        p.translation.y,
    00730        1,
    00731        Drawings::ps_solid,
    00732        color);
    00733   p = pose + Pose2D(-40,-40);
    00734   NLINE("gt04-sl",
    00735        pose.translation.x,
    00736        pose.translation.y,
    00737        p.translation.x,
    00738        p.translation.y,
    00739        1,
    00740        Drawings::ps_solid,
    00741        color);
    00742   p = pose + Pose2D(-40,40);
    00743   NLINE("gt04-sl",
    00744        pose.translation.x,
    00745        pose.translation.y,
    00746        p.translation.x,
    00747        p.translation.y,
    00748        1,
    00749        Drawings::ps_solid,
    00750        color);
    00751 }
    00752
    00753 void GT2004SelfLocator::draw(const Vector2<int>& point,LinesPercept::LineType type) const
    00754 {
    00755   static const Drawings::Color colors[] =
    00756   {
    00757     Drawings::red,
    00758     Drawings::green,
    00759     Drawings::yellow,
    00760     Drawings::skyblue
    00761   };
    00762
    00763   CameraInfo cameraInfo(getRobotConfiguration().getRobotDesign());
    00764
    00765   Vector2<int> p;
    00766   Geometry::calculatePointInImage(
    00767     point,
    00768     cameraMatrix, cameraInfo,
    00769     p);
    00770   NCIRCLE("gt04-sl",p.x,p.y,3,1,Drawings::ps_solid,colors[type]);
    00771 }
    00772
    00773 void GT2004SelfLocator::drawEllipse()
    00774 {
    00775   //DEBUG Daniel>
    00776
    00777   double mx,my,sxx,sxy,syy;       // means and covariance variables
    00778   mx = my = sxx = sxy = syy = 0;
    00779   double l1, l2, ev1x, ev1y, ev2x, ev2y;    // EigenValues and EigenVectors
    00780   l1 = l2 = ev1x = ev1y = ev2x = ev2y = 0;
    00781
    00782   int i ;
    00783   // Mean calculation
    00784   for (i = 0; i < selfLocatorSamples.getNumberOfSamples(); i++)
    00785   {
    00786   mx += selfLocatorSamples[i].translation.x;
    00787   my += selfLocatorSamples[i].translation.y;
    00788   }
    00789   mx /= selfLocatorSamples.getNumberOfSamples();
    00790   my /= selfLocatorSamples.getNumberOfSamples();
    00791
    00792   // Covariance calculation
    00793   for (i = 0; i < selfLocatorSamples.getNumberOfSamples(); i++)
    00794   {
    00795   sxx += (selfLocatorSamples[i].translation.x - mx) * (selfLocatorSamples[i].translation.x - mx);
    00796   sxy += (selfLocatorSamples[i].translation.x - mx) * (selfLocatorSamples[i].translation.y - my);
    00797   syy += (selfLocatorSamples[i].translation.y - my) * (selfLocatorSamples[i].translation.y - my);
    00798   }
    00799
    00800   sxx /= selfLocatorSamples.getNumberOfSamples();
    00801   sxy /= selfLocatorSamples.getNumberOfSamples();
    00802   syy /= selfLocatorSamples.getNumberOfSamples();
    00803
    00804   l1 = (sxx + syy)/2 + sqrt((sxx + syy)/2*(sxx + syy)/2 - sxx * syy + sxy * sxy);
    00805   l2 = (sxx + syy)/2 - sqrt((sxx + syy)/2*(sxx + syy)/2 - sxx * syy + sxy * sxy);
    00806
    00807   // Calculate the Eigenvectors
    00808     double amount;  //vector amount
    00809   ev1x =  (syy - l1 - sxy) / (sxx - l1 - sxy);
    00810   amount = sqrt(ev1x*ev1x + 1);
    00811
    00812   ev1x /= amount;
    00813   ev1y  = 1.0 / amount;
    00814
    00815   ev2x =  (syy - l2 - sxy) / (sxx - l2 - sxy);
    00816   amount = sqrt(ev2x*ev2x + 1);
    00817
    00818   ev2x /= amount;
    00819   ev2y  = 1.0 / amount;
    00820
    00821   double alpha = atan2(ev1y,ev1x);
    00822
    00823   double a = sqrt(l1*1);
    00824   double b = sqrt(l2*1);
    00825   double x = 0;
    00826   double y = 0;
    00827     int step = (int)(a/200);
    00828       if (!step)
    00829       {
    00830         step = 1;
    00831       }
    00832
    00833       for (int xc = 0; xc < a; xc += step)
    00834       {
    00835         x = xc;
    00836         y = (sqrt(1-sqr(x/a))* b);
    00837
    00838     NDOT("gt04-sl",  cos(alpha)*(x + mx) - sin(alpha)*(y + my),
    00839                  sin(alpha)*(x + mx) + cos(alpha)*(y + my),
    00840                  2, Drawings::red);
    00841     NDOT("gt04-sl",  cos(alpha)*(x + mx) - sin(alpha)*(-y + my),
    00842                  sin(alpha)*(x + mx) + cos(alpha)*(-y + my),
    00843                  2, Drawings::red);
    00844     NDOT("gt04-sl",  cos(alpha)*(-x + mx) - sin(alpha)*(y + my),
    00845                  sin(alpha)*(-x + mx) + cos(alpha)*(y + my),
    00846                  2, Drawings::red);
    00847     NDOT("gt04-sl",  cos(alpha)*(-x + mx) - sin(alpha)*(-y + my),
    00848                  sin(alpha)*(-x + mx) + cos(alpha)*(-y + my),
    00849                  2, Drawings::red);
    00850
    00851   }
    00852     //OUTPUT(idText, text, "Mitte: " << mx << ";" << my << " l1: " << l1 << " l2: " << l2 << " phi " << alpha * 180 / 3.1415 << " Grad");
    00853 }
    00854
    00855
    00856 bool GT2004SelfLocator::handleMessage(InMessage& message)
    00857 {
    00858   switch(message.getMessageID())
    00859   {
    00860     case idLinesSelfLocatorParameters:
    00861       GenericDebugData d;
    00862       message.bin >> d;
    00863       paramUp = d.data[0];
    00864       paramDown = d.data[1];
    00865       paramDelay = d.data[2];
    00866       paramHeight = d.data[3];
    00867       paramZ = d.data[4];
    00868       paramY = d.data[5];
    00869       return true;
    00870   }
    00871   return false;
    00872 }