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

# Modules/SelfLocator/GT2005SelfLocator/GT2005SelfLocator.cpp

Go to the documentation of this file.

    00001
    00002 #include "GT2005SelfLocator.h"
    00003 #include "Tools/Math/Common.h"
    00004
    00005 #ifdef _WIN32
    00006 #pragma warning(disable:4355) // VC++: "this" in initializer list is ok
    00007 #endif
    00008
    00009 /*
    00010   Particle-filter based Self-Locator based on the GT2004 SelfLocator
    00011
    00012
    00013   todo
    00014     template generation by line types is not yet done properly
    00015     needs change in class structure first
    00016     implement encapsuled motion and observation model
    00017
    00018     negative information update not working properly, now commented-out
    00019
    00020     changing number of percept types: change probability calculation in sample
    00021                                       change average calculation in the self locator
    00022
    00023     DEBUG_RESPONSE output, comments adapt to gt2004
    00024     declare some functions inline
    00025
    00026     diffs to gt04
    00027       -execute split up in new methods
    00028       -sample.setPerceptProbability replaces setProbability
    00029         -enum PerceptType to avoid strange LineType enum usage for setProbability
    00030       -usage of line crossings percepts
    00031       -middle circle as flag
    00032       -possible crash fixed: no flag update for particle position inside a flag (MR)
    00033       -adjustable parameters moved into separate class (MR)
    00034       -added percept and odometry update probabilities (MR)
    00035
    00036     @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
    00037     @author <A href=mailto:c_rohde@web.de>Carsten Rohde</A>
    00038     @author <A href=mailto:judith-winter@web.de>Judith Winter</A>
    00039     @author Max Risler
    00040 */
    00041
    00042 const double GT2005SelfLocator::QUASI_ZERO = 0.000001;
    00043
    00044 /*---------------------------- class GT2005SelfLocator methods ------------------------------*/
    00045
    00046
    00047 GT2005SelfLocator::GT2005SelfLocator(const SelfLocatorInterfaces& interfaces)
    00048 : SelfLocator(interfaces), sampleTemplateGenerator(parameters)
    00049 {
    00050   timeStamp = 0;
    00051   testSample = 0;
    00052   testSampleIndex = 0;
    00053   speed = 0;
    00054
    00055   resetSamples();
    00056
    00057   timeStamp = 0;
    00058
    00059   calculateTrust();
    00060
    00061   OUTPUT(idText, text, "GT2005 SelfLocator initialized");
    00062
    00063   OUTPUT(idText,text,"line Z trust: " << linePointZAngleTrust);
    00064   OUTPUT(idText,text,"line Y trust: " << linePointYAngleTrust);
    00065   OUTPUT(idText,text,"crossings Z trust: " << crossingZAngleTrust);
    00066   OUTPUT(idText,text,"crossings Y trust: " << crossingYAngleTrust);
    00067   //OUTPUT(idText,text,"flags Z trust: " << flagZAngleTrust);
    00068   OUTPUT(idText,text,"flags Y trust: " << flagYAngleTrust);
    00069   OUTPUT(idText,text,"goal Z trust: " << goalZAngleTrust);
    00070   OUTPUT(idText,text,"goal Y trust: " << goalYAngleTrust);
    00071 }
    00072
    00073
    00074 void GT2005SelfLocator::motionUpdate(const Pose2D& odometry, const Pose2D& camera, bool noise)
    00075 {
    00076   double transNoise = noise ? parameters.translationNoise : 0;
    00077   double rotNoise   = noise ? parameters.rotationNoise : 0;
    00078   double transX     = odometry.translation.x;
    00079   double transY     = odometry.translation.y;
    00080   double dist     = odometry.translation.abs();
    00081   double angle    = fabs(odometry.getAngle());
    00082
    00083   /* precalculate rotational error that has to be adapted to all samples */
    00084   double rotError   = max(dist*parameters.movedDistWeight,
    00085                            angle*parameters.movedAngleWeight);
    00086
    00087   /* precalculate translational error that has to be adapted to all samples */
    00088   double transXError = max( fabs(transX*parameters.majorDirTransWeight),
    00089                              fabs(transY*parameters.minorDirTransWeight));
    00090   double transYError = max( fabs(transY*parameters.majorDirTransWeight),
    00091                              fabs(transX*parameters.minorDirTransWeight));
    00092
    00093   double improbability;
    00094
    00095   /* for each sample:
    00096     -move by odometry
    00097     -apply motion dependent error
    00098     -apply probability dependent error
    00099     */
    00100   int i;
    00101   for (i = 0; i < SAMPLES_MAX; i++)
    00102   {
    00103     GT2005SelfLocatorSample& s = sampleSet[i];
    00104
    00105     if (s.isUsingOdometry)
    00106       s += odometry;
    00107
    00108     if (s.probability>1) improbability = 0;           // for uninitialized or faulty samples we can't calc noise
    00109     else improbability = (1-s.probability)*(1-s.probability);
    00110
    00111     // improbability = 1;
    00112
    00113     rotError = (random()*2-1) * max(rotError,rotNoise*improbability);
    00114
    00115     /* the translational error vector*/
    00116     double sampleTransXError = max(transXError,transNoise*improbability);
    00117     double sampleTransYError = max(transYError,transNoise*improbability);
    00118     Vector2<double> transErrorVec( (random()*2-1)*sampleTransXError,
    00119                                    (random()*2-1)*sampleTransYError );
    00120
    00121     /* update the sample */
    00122     s += Pose2D(rotError, transErrorVec);
    00123     s.camera = s + camera;
    00124   }
    00125
    00126 }
    00127
    00128 // for the moment this is just inserted into the line crossings percept value
    00129 void GT2005SelfLocator::updateByCenterCircle(const LinesPercept::CenterCircle& centerCircle)
    00130 {
    00131   Vector2<int> point = centerCircle.location;
    00132   double pointDist = point.abs();
    00133
    00134   // seen angle to the linepoint in z-direction (height) assuming the robots height is paramHeight
    00135   double observedZAngle = atan2(parameters.headHeightEstimation,pointDist);
    00136   // seen angle to the linepoint in y-direction
    00137   double observedYAngle = atan2((double)point.y,(double)point.x);
    00138
    00139   // pose describing where the linepoint is seen from the robot and under which angle
    00140   lastSeenCenterCircle = Pose2D(centerCircle.orientation,(double)point.x,(double)point.y);
    00141
    00142   // OUTPUT(idText,text,"seen angle: " << centerCircle.orientation << " field angle: " << pointFromPose.rotation);
    00143
    00144   // update all samples..
    00145   for(int i = 0; i < SAMPLES_MAX; ++i)
    00146     if (random() < parameters.updateProbCenterCircle)
    00147     {
    00148       GT2005SelfLocatorSample& s = sampleSet[i];
    00149       // the absolute position of the point if it was seen from the samples position
    00150       Pose2D pointFromSample = s + lastSeenCenterCircle;
    00151       Vector2<double> vMin(0,0);
    00152
    00153       Vector2<double> v = (Pose2D(vMin) - s).translation;
    00154
    00155       // the z and y directed angle derived from the closest point
    00156       double zModel = atan2(parameters.headHeightEstimation,v.abs()),
    00157             yModel = atan2(v.y,v.x);
    00158
    00159       double orientationDiff;
    00160       if (centerCircle.orientationKnown)
    00161       {
    00162         double orientation = fabs(normalize(pointFromSample.rotation));
    00163         orientationDiff = pi_2 - orientation;
    00164       }
    00165       else
    00166         orientationDiff = 0;
    00167
    00168       double val = gaussian((zModel - observedZAngle) * centerCircleZAngleTrust) *
    00169                   gaussian((yModel - observedYAngle) * centerCircleYAngleTrust) *
    00170                   gaussian(orientationDiff * centerCircleOrientationAngleTrust);
    00171       //
    00172
    00173       s.setPerceptProbability(GT2005SelfLocatorSample::lineCrossing, val, parameters);
    00174       if (i==testSampleIndex)
    00175       {
    00176         Drawings::Color color = getSampleColor(val);
    00177         NCIRCLE("gt05-sl:testSample", 0, 0, 40, 1, Drawings::ps_solid, color);
    00178         NCIRCLE("gt05-sl:testSample", pointFromSample.translation.x, pointFromSample.translation.y, 40, 1, Drawings::ps_solid, color);
    00179         NLINE("gt05-sl:testSample", 0, 0, pointFromSample.translation.x, pointFromSample.translation.y, 1, Drawings::ps_solid, color);
    00180       }
    00181     }
    00182 }
    00183
    00184 void GT2005SelfLocator::updateByCrossing(const LinesPercept::LineCrossingPoint& crossingPoint)
    00185 {
    00186   // COMPLEX_DRAWING(selfLocator,draw(point,type));
    00187   Vector2<int> point = crossingPoint.locationOnField;
    00188
    00189   // pose describing where the linepoint is seen from the robot and under which angle
    00190   Pose2D point2(crossingPoint.angleOnField, (double)point.x, (double)point.y);
    00191   Pose2D pointFromPose = robotPose + point2;
    00192
    00193   // crossing filtering
    00194   Vector2<double> testCrossing = lineCrossingsTable.getClosestPoint(pointFromPose.translation, GT2005LineCrossingsTable::falseCrossing, 0);
    00195   Vector2<double> testDiff = testCrossing - pointFromPose.translation;
    00196   if (testDiff.abs()<300)
    00197   {
    00198     NCIRCLE("gt05-sl:lastCrossing",pointFromPose.translation.x,pointFromPose.translation.y,40,40,Drawings::ps_solid,Drawings::yellow);
    00199     return;
    00200   }
    00201
    00202   //debug stuff
    00203   Vector2<double> vMin;
    00204   vMin = lineCrossingsTable.getClosestPoint(pointFromPose, crossingPoint, lastSeenCrossingClass);
    00205   lastSeenCrossing = pointFromPose.translation;
    00206   if(vMin.x != GT2005LineCrossingsTable::NO_POINT_DISTANCE)
    00207   {
    00208     lastModelCrossing = vMin;
    00209   }
    00210   else
    00211   {
    00212     lastModelCrossing = pointFromPose.translation;
    00213     // If a crossing cannot be matched it must be a wrong percept since all possible correct crossings
    00214     // are included in the tables. Therefore we do not update with this crossing.
    00215     return;
    00216   }
    00217
    00218   double pointDist = point.abs();
    00219          // seen angle to the linepoint in z-direction (height) assuming the robots height is paramHeight
    00220   double observedZAngle = atan2(parameters.headHeightEstimation,pointDist),
    00221          // seen angle to the linepoint in y-direction
    00222          observedYAngle = atan2((double)point.y,(double)point.x);
    00223
    00224   // update all samples..
    00225   for(int i = 0; i < SAMPLES_MAX; ++i)
    00226     if (random() < parameters.updateProbCrossing)
    00227     {
    00228       GT2005SelfLocatorSample& s = sampleSet[i];
    00229       // the absolute position of the point if it was seen from the samples position
    00230       Pose2D pointFromSample = s + point2;
    00231       // stores the vector pointing to the nearest point of the given type
    00232       Vector2<double> vMin;
    00233       GT2005LineCrossingsTable::CrossingClass crossingClass;
    00234       vMin = lineCrossingsTable.getClosestPoint(pointFromSample, crossingPoint, crossingClass);
    00235
    00236       double val;
    00237       // if we got a closest point..
    00238       if(vMin.x != GT2005LineCrossingsTable::NO_POINT_DISTANCE)
    00239       {
    00240         // Vector2<double> diff = vMin - pointFromSample.translation;
    00241         Vector2<double> v = (Pose2D(vMin) - s).translation;
    00242         // the z and y directed angle derived from the closest point
    00243         double zModel = atan2(parameters.headHeightEstimation,v.abs()),
    00244               yModel = atan2(v.y,v.x);
    00245
    00246         // testing this way this equals the similarity function of flags and goals
    00247         /*
    00248         double normedSimilarity = fabs((zModel - observedZAngle)*0.8) + fabs((yModel - observedYAngle)*0.2);
    00249         s.setPerceptProbability(isY ? (LinesPercept::LineType)GT2005SelfLocatorSample::perceptTypeYFieldLine : type,
    00250                                 gaussian(normedSimilarity*7));
    00251                                 */
    00252
    00253         val = gaussian((zModel - observedZAngle) * crossingZAngleTrust) *
    00254                     gaussian((yModel - observedYAngle) * crossingYAngleTrust);
    00255         // if (val>0.3) OUTPUT(idText,text,"crossings value" << val);
    00256         s.setPerceptProbability(GT2005SelfLocatorSample::lineCrossing, val, parameters);
    00257         if (i==testSampleIndex)
    00258         {
    00259           Drawings::Color color = getSampleColor(val);
    00260           NCIRCLE("gt05-sl:testSample", vMin.x, vMin.y, 40, 1, Drawings::ps_solid, color);
    00261           NCIRCLE("gt05-sl:testSample", pointFromSample.translation.x, pointFromSample.translation.y, 40, 1, Drawings::ps_solid, color);
    00262           NLINE("gt05-sl:testSample", vMin.x, vMin.y, pointFromSample.translation.x, pointFromSample.translation.y, 1, Drawings::ps_solid, color);
    00263         }
    00264       }
    00265     }
    00266
    00267 }
    00268
    00269 // TODO: document, rewrite
    00270 void GT2005SelfLocator::updateByPoint(const LinesPercept::LinePoint& point,GT2005SelfLocatorSample::PerceptType type, LinesPercept::LineType observationTableIndex)
    00271 {
    00272   // COMPLEX_DRAWING(selfLocator,draw(point,type));
    00273
    00274   double pointDist = point.abs();
    00275
    00276          // seen angle to the linepoint in z-direction (height) assuming the robots height is paramHeight
    00277   double observedZAngle = atan2(parameters.headHeightEstimation,pointDist),
    00278          // seen angle to the linepoint in y-direction
    00279          observedYAngle = atan2((double)point.y,(double)point.x);
    00280
    00281
    00282   // pose describing where the linepoint is seen from the robot and under which angle
    00283   Pose2D point2(point.angle, Vector2<double>(point.x, point.y));
    00284
    00285   // update all samples..
    00286   for(int i = 0; i < SAMPLES_MAX; ++i)
    00287   {
    00288     GT2005SelfLocatorSample& s = sampleSet[i];
    00289     // the absolute position of the point if it was seen from the samples position
    00290     Pose2D pointFromSample = s + point2;
    00291
    00292
    00293     // stores the vector pointing to the nearest point of the given type
    00294     Vector2<double> vMin;
    00295
    00296     // is this a line running in y direction?
    00297     bool isY = (type == GT2005SelfLocatorSample::xFieldLine || type==GT2005SelfLocatorSample::yFieldLine) &&
    00298               (fabs(pointFromSample.getAngle()) < pi / 4 ||
    00299                fabs(pointFromSample.getAngle()) > 3 * pi / 4);
    00300     // we find the closest point in the corresponding observationTable
    00301     vMin = observationTable[observationTableIndex].getClosestPoint(pointFromSample);
    00302
    00303     // if we got a closest point..
    00304     if(vMin.x != 1000000)
    00305     {
    00306       // Vector2<double> diff = vMin - pointFromSample.translation;
    00307       Vector2<double> v = (Pose2D(vMin) - s).translation;
    00308       // the z and y directed angle derived from the closest point
    00309       double zModel = atan2(parameters.headHeightEstimation,v.abs()),
    00310              yModel = atan2(v.y,v.x);
    00311
    00312       // testing this way this equals the similarity function of flags and goals
    00313       /*
    00314       double normedSimilarity = fabs((zModel - observedZAngle)*0.8) + fabs((yModel - observedYAngle)*0.2);
    00315       s.setPerceptProbability(isY ? (LinesPercept::LineType)GT2005SelfLocatorSample::perceptTypeYFieldLine : type,
    00316                               gaussian(normedSimilarity*7));
    00317                               */
    00318
    00319
    00320       double val = gaussian((zModel - observedZAngle) * linePointZAngleTrust) *
    00321                    gaussian((yModel - observedYAngle) * linePointYAngleTrust);
    00322       s.setPerceptProbability(isY ? GT2005SelfLocatorSample::yFieldLine : type, val, parameters);
    00323       if (i==testSampleIndex)
    00324       {
    00325         Drawings::Color color = getSampleColor(val);
    00326         NCIRCLE("gt05-sl:testSample", vMin.x, vMin.y, 40, 1, Drawings::ps_solid, color);
    00327         NCIRCLE("gt05-sl:testSample", pointFromSample.translation.x, pointFromSample.translation.y, 40, 1, Drawings::ps_solid, color);
    00328         NLINE("gt05-sl:testSample", vMin.x, vMin.y, pointFromSample.translation.x, pointFromSample.translation.y, 1, Drawings::ps_solid, color);
    00329       }
    00330     }
    00331     else
    00332     {
    00333       s.setPerceptProbability(isY ? GT2005SelfLocatorSample::yFieldLine : type, QUASI_ZERO, parameters);
    00334       if (i==testSampleIndex)
    00335       {
    00336         Drawings::Color color = getSampleColor(0);
    00337         NCIRCLE("gt05-sl:testSample", pointFromSample.translation.x, pointFromSample.translation.y, 40, 1, Drawings::ps_solid, color);
    00338       }
    00339     }
    00340   }
    00341 }
    00342
    00343
    00344 void GT2005SelfLocator::lineObservationUpdate(const LinesPercept& linesPercept)
    00345 {
    00346     int i;
    00347
    00348     // updating by center circle
    00349     if (linesPercept.centerCircleFound && parameters.centerCircleWeight > 0){
    00350       updateByCenterCircle(linesPercept.centerCircle);
    00351     }
    00352
    00353
    00354     // this wil be replaced by a nicer solution in the LinesPercept
    00355     // OUTPUT(idText,text,"crossings " << linesPercept.numberOfLineCrossings);
    00356     if (parameters.crossingWeight > 0)
    00357       for(i=0;i<linesPercept.numberOfLineCrossings && i < parameters.maxCrossings;i++)
    00358       {
    00359         updateByCrossing(linesPercept.lineCrossings[i]);
    00360         observationUpdateDone = true;
    00361       }
    00362
    00363     for(int j = 0; j < linesPercept.points[LinesPercept::border].numberOfPoints && j < parameters.maxBorderpoints; ++j)
    00364     {
    00365       updateByPoint(linesPercept.points[LinesPercept::border].pointsOnField[rand() % linesPercept.points[LinesPercept::border].numberOfPoints],
    00366         GT2005SelfLocatorSample::border,
    00367         LinesPercept::border
    00368       );
    00369       observationUpdateDone = true;
    00370     }
    00371
    00372     for(int j = 0; j < linesPercept.points[LinesPercept::field].numberOfPoints && j < parameters.maxLinepoints; ++j)
    00373     {
    00374       updateByPoint(linesPercept.points[LinesPercept::field].pointsOnField[rand() % linesPercept.points[LinesPercept::field].numberOfPoints],
    00375         GT2005SelfLocatorSample::xFieldLine,
    00376         LinesPercept::field
    00377       );
    00378       observationUpdateDone = true;
    00379     }
    00380
    00381     if(linesPercept.points[LinesPercept::yellowGoal].numberOfPoints ||
    00382       linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints)
    00383       for(int j = 0; j < linesPercept.points[LinesPercept::yellowGoal].numberOfPoints +
    00384                          linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints && j < parameters.maxGoalpoints; ++j)
    00385       { // This works if only one goal or both were seen
    00386         LinesPercept::LineType select = (random() + 1e-6) * linesPercept.points[LinesPercept::yellowGoal].numberOfPoints >
    00387                                         linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints
    00388                                         ? LinesPercept::yellowGoal
    00389                                         : LinesPercept::skyblueGoal;
    00390         LinesPercept::LineType observationTableIndex;
    00391         if(getPlayer().getTeamColor() == Player::blue)
    00392           // switch observation table index for blue as tables are calculated for red players only
    00393           observationTableIndex = (select == LinesPercept::yellowGoal) ? LinesPercept::skyblueGoal : LinesPercept::yellowGoal;
    00394         else
    00395           observationTableIndex = select;
    00396         updateByPoint(linesPercept.points[select].pointsOnField[rand() % linesPercept.points[select].numberOfPoints],
    00397           GT2005SelfLocatorSample::goal,
    00398           observationTableIndex
    00399         );
    00400         observationUpdateDone = true;
    00401       }
    00402 }
    00403
    00404
    00405 // TODO: maybe use vertical angles too?
    00406 // make similarity factor configurable
    00407 void GT2005SelfLocator::updateByFlag(const Vector2<double>& flagFieldPosition,
    00408                                      FlagSides sideOfFlag,
    00409                                      double measuredBearing)
    00410 {
    00411   observationUpdateDone = true;
    00412   int i;
    00413   for(i = 0; i < SAMPLES_MAX; ++i)
    00414     if (random() < parameters.updateProbFlag)
    00415     {
    00416       GT2005SelfLocatorSample& s = sampleSet[i];
    00417
    00418       // flagPos: position of the flag relative to sample s
    00419       Pose2D flagPos(flagFieldPosition.x,flagFieldPosition.y);
    00420       flagPos -= s.camera;
    00421
    00422       // if particle position is inside a flag similarity cannot be determined
    00423       if (flagPos.translation.abs() > flagRadius)
    00424       {
    00425         // horizontal angle between flag and camera coordinate system
    00426         double assumedBearing = atan2(flagPos.translation.y,flagPos.translation.x) + sideOfFlag * asin(flagRadius / flagPos.translation.abs());
    00427
    00428         // similarity in numbers of pi - handy as we need a similarity between 0 and 1
    00429         double similarity = fabs((assumedBearing - measuredBearing)) / pi;
    00430
    00431         // use symmetry for full circle
    00432         if(similarity > 1)
    00433           similarity = 2.0 - similarity;
    00434
    00435         // now we have a similarity measure between 0..1
    00436         // make similarity * 7 configurable...
    00437         double val = gaussian(similarity * flagYAngleTrust);
    00438         s.setPerceptProbability(GT2005SelfLocatorSample::flag, val, parameters);
    00439         if (i==testSampleIndex)
    00440         {
    00441           Drawings::Color color = getSampleColor(val);
    00442           NCIRCLE("gt05-sl:testSample", flagFieldPosition.x, flagFieldPosition.y, 40, 1, Drawings::ps_solid, color);
    00443         }
    00444       }
    00445       else
    00446       {
    00447         s.setPerceptProbability(GT2005SelfLocatorSample::flag, QUASI_ZERO, parameters);
    00448         if (i==testSampleIndex)
    00449         {
    00450           Drawings::Color color = getSampleColor(0);
    00451           NCIRCLE("gt05-sl:testSample", flagFieldPosition.x, flagFieldPosition.y, 40, 1, Drawings::ps_solid, color);
    00452         }
    00453       }
    00454     }
    00455 }
    00456
    00457
    00458 // look at updateByFlag
    00459 // goalPost is a Vector pointing on the used goalpost
    00460 // TODO: maybe introduce new PerceptType GoalPost instead of using "Flag"
    00461 void GT2005SelfLocator::updateByGoalPost(const Vector2<double>& goalPost,
    00462                                          double measuredBearing)
    00463 {
    00464   observationUpdateDone = true;
    00465   int i;
    00466   for(i = 0; i < SAMPLES_MAX; ++i)
    00467     if (random() < parameters.updateProbGoalPost)
    00468     {
    00469       GT2005SelfLocatorSample& s = sampleSet[i];
    00470       Pose2D p(goalPost.x,goalPost.y);
    00471       p -= s.camera;
    00472       double assumedBearing = atan2(p.translation.y,p.translation.x);
    00473       double similarity = fabs((assumedBearing - measuredBearing)) / pi;
    00474       if(similarity > 1)
    00475         similarity = 2 - similarity;
    00476
    00477       double val = gaussian(similarity * goalYAngleTrust);
    00478       s.setPerceptProbability(GT2005SelfLocatorSample::flag, val, parameters);
    00479       if (i==testSampleIndex)
    00480       {
    00481         Drawings::Color color = getSampleColor(val);
    00482         NCIRCLE("gt05-sl:testSample", goalPost.x, goalPost.y, 40, 1, Drawings::ps_solid, color);
    00483       }
    00484     }
    00485 }
    00486
    00487
    00488
    00489 /*
    00490 *  update observation information by all flags and goalposts seen
    00491 */
    00492 void GT2005SelfLocator::landmarksObservationUpdate(const LandmarksPercept& landmarksPercept)
    00493 {
    00494    int i;
    00495    for(i = 0; i < landmarksPercept.numberOfFlags; i++)
    00496     {
    00497       // DEBUG_RESPONSE("selfLocator:showObservationUpdateDetails", OUTPUT(idText, text, "SLou: flag found"));
    00498       const Flag& flag = landmarksPercept.flags[i];
    00499
    00500     /* if the leftmost position of the flag is not on the robots image border.. */
    00501       if(!flag.isOnBorder(flag.x.max))
    00502         updateByFlag(flag.position,
    00503                      LEFT_SIDE_OF_FLAG,
    00504                      flag.x.max);
    00505     /* if the rightmost position of the flag is not on the robots image border.. */
    00506       if(!flag.isOnBorder(flag.x.min))
    00507         updateByFlag(flag.position,
    00508                      RIGHT_SIDE_OF_FLAG,
    00509                      flag.x.min);
    00510
    00511     /* for flag on border no update is done! */
    00512     }
    00513
    00514     /* same thing for goals */
    00515     bool skyblueGoalSeen = false;
    00516     bool yellowGoalSeen = false;
    00517     for (i = 0; i < landmarksPercept.numberOfGoals; i++)
    00518     {
    00519       // DEBUG_RESPONSE("selfLocator:showObservationUpdateDetails", OUTPUT(idText, text, "SLou: goal found"));
    00520       const Goal& goal = landmarksPercept.goals[i];
    00521       if (goal.color == skyblue)
    00522         skyblueGoalSeen = true;
    00523       else if (goal.color == yellow)
    00524         yellowGoalSeen = true;
    00525
    00526       if(!goal.isOnBorder(goal.x.max))
    00527         updateByGoalPost(goal.leftPost,
    00528                          goal.x.max);
    00529       if(!goal.isOnBorder(goal.x.min))
    00530         updateByGoalPost(goal.rightPost,
    00531                          goal.x.min);
    00532       /*
    00533       if(!goal.isOnBorder(goal.x.min) && !goal.isOnBorder(goal.x.max))
    00534         OUTPUT(idText,text,"distance: " << goal.distance << "angle: " << goal.angle << "rot: " << goal.rotation);
    00535         */
    00536     }
    00537
    00538
    00539     // update by not seen goals here..
    00540     /*
    00541     if (!skyblueGoalSeen)
    00542       updateByNotSeenGoal(skyblue);
    00543     if (!yellowGoalSeen)
    00544       updateByNotSeenGoal(yellow);
    00545       */
    00546 }
    00547
    00548
    00549
    00550 void GT2005SelfLocator::updateVariancesBySpeed(double speed)
    00551 {
    00552   linePointZAngleMotionDependentVariance = parameters.linePointZAngleVariance * (1 + speed * parameters.linePointZAngleMotionDependency);
    00553   linePointZAngleTrust = parameters.linePointWeight / (linePointZAngleMotionDependentVariance + QUASI_ZERO );
    00554
    00555 }
    00556
    00557 void GT2005SelfLocator::calculateTrust()
    00558 {
    00559   linePointZAngleTrust =
    00560     parameters.linePointWeight / ( parameters.linePointZAngleVariance + QUASI_ZERO ),
    00561   linePointYAngleTrust =
    00562     parameters.linePointWeight / ( parameters.linePointYAngleVariance + QUASI_ZERO ),
    00563
    00564   crossingZAngleTrust =
    00565     parameters.crossingWeight / ( parameters.crossingZAngleVariance + QUASI_ZERO ),
    00566   crossingYAngleTrust =
    00567     parameters.crossingWeight / ( parameters.crossingYAngleVariance + QUASI_ZERO ),
    00568
    00569   centerCircleZAngleTrust =
    00570     parameters.centerCircleWeight / ( parameters.centerCircleZAngleVariance + QUASI_ZERO ),
    00571   centerCircleYAngleTrust =
    00572     parameters.centerCircleWeight / ( parameters.centerCircleYAngleVariance + QUASI_ZERO ),
    00573   centerCircleOrientationAngleTrust =
    00574     parameters.centerCircleWeight / ( parameters.centerCircleOrientationAngleVariance + QUASI_ZERO ),
    00575
    00576
    00577   //flagZAngleTrust =
    00578   //  parameters.flagWeight / ( parameters.flagZAngleVariance + QUASI_ZERO ),
    00579   flagYAngleTrust =
    00580     parameters.flagWeight / ( parameters.flagYAngleVariance + QUASI_ZERO ),
    00581
    00582   goalZAngleTrust =
    00583     parameters.goalWeight / ( parameters.goalZAngleVariance + QUASI_ZERO ),
    00584   goalYAngleTrust =
    00585     parameters.goalWeight / ( parameters.goalYAngleVariance + QUASI_ZERO );
    00586 }
    00587
    00588 void GT2005SelfLocator::execute()
    00589 {
    00590   NDECLARE_DEBUGDRAWING( "gt05-sl:sampleSet" , "drawingOnField" , "shows the current sample set");
    00591   NDECLARE_DEBUGDRAWING( "gt05-sl:templates" , "drawingOnField" , "shows the current template positions for new samples");
    00592   NDECLARE_DEBUGDRAWING( "gt05-sl:flags" , "drawingOnField" , "shows the currently buffered flags for template calculation");
    00593   NDECLARE_DEBUGDRAWING( "gt05-sl:testSample" , "drawingOnField" , "shows the currently selected test sample");
    00594   NDECLARE_DEBUGDRAWING( "gt05-sl:lastSeenCenterCircle", "drawingOnField", "shows the last seen center circle");
    00595   NDECLARE_DEBUGDRAWING( "gt05-sl:lastCrossing", "drawingOnField", "shows the last seen line crossing");
    00596   NDECLARE_DEBUGDRAWING( "gt05-sl:covarianceEllipse", "drawingOnField", "shows the ellipse describing the sample distribution covariance matrix");
    00597
    00598   MODIFY( "gt05-sl:parameters", parameters );
    00599
    00600   DEBUG_RESPONSE( "gt05-sl:recalculate trust values",
    00601     calculateTrust();
    00602   );
    00603
    00604   DEBUG_RESPONSE( "gt05-sl:resetSamples",
    00605     resetSamples();
    00606   );
    00607
    00608   DEBUG_RESPONSE( "gt05-sl:resetSamplesWhenPenalized",
    00609     resetSamplesWhenPenalized();
    00610   );
    00611
    00612   DEBUG_RESPONSE( "gt05-sl:resetSamplesToZero",
    00613     resetSamples(Pose2D(0, Vector2<double>(0,0)));
    00614   );
    00615
    00616   DEBUG_RESPONSE( "gt05-sl:resetSamplesToCurrentRobotPose",
    00617     resetSamples(robotPose);
    00618   );
    00619
    00620   MODIFY( "gt05-sl:testSampleIndex", testSampleIndex);
    00621
    00622   testSample = &sampleSet[testSampleIndex];
    00623
    00624   MODIFY( "gt05-sl:testSample", *testSample);
    00625
    00626
    00627   if (parameters.resetSamplesWhenPenalized && gameControlData.getRobot().penalty != GameControlData::notPenalized)
    00628   {
    00629     // robot is penalized
    00630     resetSamplesWhenPenalized();
    00631     return;
    00632   }
    00633
    00634   teamColorBlue = (getPlayer().getTeamColor() == Player::blue);
    00635
    00636 /*
    00637   headHeightBuffer.add(cameraMatrix.translation.z);
    00638   headHeight = headHeightBuffer.getNumberOfEntries()>0 ?
    00639   headHeightBuffer.getSum() / headHeightBuffer.getNumberOfEntries() : headHeight;
    00640   */
    00641   // OUTPUT(idText,text,paramHeadHeight);
    00642   // OUTPUT(idText,text,sampleSet.getNumberOfSamples());
    00643
    00644   robotPose.setFrameNumber(landmarksPercept.frameNumber);
    00645   robotPose.setTimestamp(SystemCall::getCurrentSystemTime());
    00646
    00647
    00648   Pose2D odometryDelta = odometryData - lastOdometry;
    00649   lastOdometry = odometryData;
    00650
    00651   // this is about to go to the motion model
    00652   if(SystemCall::getTimeSince(timeStamp) > 500)
    00653   {
    00654     speed = (odometryData - lastOdometry2).translation.abs() / SystemCall::getTimeSince(timeStamp) * 1000.0;
    00655     lastOdometry2 = odometryData;
    00656     timeStamp = SystemCall::getCurrentSystemTime();
    00657   }
    00658
    00659   // updateVariancesBySpeed(speed);
    00660
    00661   // motion updating
    00662   motionUpdate(odometryDelta,Pose2D(landmarksPercept.cameraOffset.x,
    00663                landmarksPercept.cameraOffset.y),
    00664                observationUpdateDone || parameters.noiseWithoutObservationUpdate);
    00665
    00666   // age sample probabilities
    00667   for(int i = 0; i < SAMPLES_MAX; ++i)
    00668     sampleSet[i].scaleProbabilities(parameters.agingFactor);
    00669
    00670   observationUpdateDone = false;
    00671
    00672   // observation updating
    00673   if (cameraMatrix.isValid)
    00674   {
    00675     lineObservationUpdate(linesPercept);
    00676     landmarksObservationUpdate(landmarksPercept);
    00677
    00678     if (observationUpdateDone)
    00679     {
    00680       sampleTemplateGenerator.generateTemplates(landmarksPercept,linesPercept,odometryDelta);
    00681       resample();
    00682     }
    00683   }
    00684   // taken from gt04..
    00685
    00686   Pose2D pose;
    00687   double validity;
    00688   calcPose(pose,validity);
    00689   robotPose.setPose(pose);
    00690   robotPose.setValidity(validity);
    00691
    00692   // OUTPUT(idText,text,validity);
    00693
    00694   draw();
    00695
    00696   sampleSet.link(selfLocatorSamples);
    00697
    00698   landmarksState.update(landmarksPercept);
    00699 }
    00700
    00701
    00702 // contains the part of the gt04 resampling method that calculates
    00703 // the average or each percept type probability and all samples
    00704 // averagePerceptTypeProb[i] = average for all samples if there are any, 1.0 else
    00705 // returns the product over all average percepttype probabilities
    00706
    00707 double GT2005SelfLocator::calcAveragePerceptTypeProbabilities()
    00708 {
    00709   int count[GT2005SelfLocatorSample::numberOfPerceptTypes]; // number of samples per perceptType
    00710   double probability = 1;                 //probability of all average possibilities = a1*a2*a3...
    00711
    00712   // initialize count and averagePerceptTypeProb
    00713   for (int k=0; k<GT2005SelfLocatorSample::numberOfPerceptTypes; k++){
    00714     count[k] = 0;
    00715     averagePerceptTypeProb[k] = 0;
    00716   }
    00717
    00718   int i;
    00719   for (i=0;i<SAMPLES_MAX;i++)
    00720   {
    00721     GT2005SelfLocatorSample& sample = sampleSet[i];
    00722     for (int k=0; k<GT2005SelfLocatorSample::numberOfPerceptTypes; k++){
    00723       if(sample.perceptProbabilities[k]<=1)
    00724       {
    00725         count[k]++;
    00726         averagePerceptTypeProb[k] += sample.perceptProbabilities[k];
    00727       }
    00728     }
    00729   }
    00730
    00731   for (i=0;i<GT2005SelfLocatorSample::numberOfPerceptTypes;i++)
    00732   {
    00733     if (count[i]>0) averagePerceptTypeProb[i] /= count[i];
    00734     else averagePerceptTypeProb[i] = 1.0;
    00735     // OUTPUT(idText,text,"avg type "<< i << "  "<<averagePerceptTypeProb[i]);
    00736     //if (/*i!=GT2005SelfLocatorSample::lineCrossing &&*/)
    00737     probability *= averagePerceptTypeProb[i];
    00738   }
    00739   return probability;
    00740 }
    00741
    00742
    00743 /**
    00744 * redistibution of the sampleSet according to the probabilities of the samples
    00745 * reimplementation of the gt04 resampling method.
    00746 * TODO: template generation
    00747 */
    00748 void GT2005SelfLocator::resample()
    00749 {
    00750   // this fills averagePerceptTypeProb[] and returns the whole averageProbability
    00751   double averageProbability =
    00752     calcAveragePerceptTypeProbabilities();
    00753   // OUTPUT(idText,text,averageProbability);
    00754
    00755   //MODIFY( "gt05-sl:average percept probabilities", averagePerceptTypeProb);
    00756
    00757   // calculate probability for all samples from their perceptType probabilities
    00758   int i;
    00759   for(i = 0; i < SAMPLES_MAX; ++i){
    00760     sampleSet[i].updateProbability(averagePerceptTypeProb, parameters);
    00761     // sampleSet[i].updateProbability();
    00762   }
    00763
    00764   // if (testSample!=0) OUTPUT(idText,text,"last Samples prob: "<< testSample->probability << " average = " << averageProbability);
    00765   // swap sample arrays
    00766   GT2005SelfLocatorSample* oldSet = sampleSet.swap();
    00767
    00768   /* calculate the sum over the probabilities of all samples
    00769    to determine the number of samples per 1.0 probability (prob2Index)
    00770    this is needed to calculate how often a sample is multiplied
    00771    -> a sample with probability 1.0 would be inserted prob2Index times
    00772    into the new distribution  */
    00773   double probSum = 0;
    00774   for(i = 0; i < SAMPLES_MAX; ++i)
    00775    probSum += oldSet[i].getProbability();
    00776
    00777   double prob2Index;
    00778   int resamplingSize;
    00779   if (parameters.applyConstantResampling)
    00780   {
    00781     prob2Index = (double) SAMPLES_MAX * (1.0 - parameters.constantResamplingRate) / probSum;
    00782     resamplingSize = (int)( (double)SAMPLES_MAX * (1.0 - parameters.constantResamplingRate) );
    00783   }
    00784   else
    00785   {
    00786     prob2Index = (double) SAMPLES_MAX / probSum;
    00787     resamplingSize = SAMPLES_MAX;
    00788   }
    00789
    00790   double indexSum = 0;  // number of indices we covered in the new
    00791                         // sample set.
    00792   int j = 0;
    00793
    00794   testSampleIndex = -1;
    00795
    00796   // here starts resampling step, we copy the samples according
    00797   // to their probability, keeping the probabilities within
    00798   // narrow bounds
    00799   for(i = 0; i < SAMPLES_MAX; ++i)
    00800   {
    00801     // calculate how far we will proceed in the new sampleset with this sample
    00802     indexSum += oldSet[i].getProbability() * prob2Index;
    00803     while(j < resamplingSize && j < indexSum - 0.5)
    00804     {
    00805       sampleSet[j++] = oldSet[i];
    00806       if (testSampleIndex == -1 && testSample == &oldSet[i])
    00807       {
    00808         testSample = &sampleSet[j-1];
    00809         testSampleIndex = j-1;
    00810       }
    00811       // reset odometry flag for further copies of the sample
    00812       oldSet[i].isUsingOdometry = (random() < (collisionPercept.getCollisionAggregate() ? parameters.odometryProbCollision : parameters.odometryProbNoCollision));
    00813     }
    00814   }
    00815
    00816   if (parameters.applyConstantResampling)
    00817   {
    00818     // fill up remaining samples with new poses
    00819     for(; j < SAMPLES_MAX; ++j)
    00820     {
    00821       sampleSet[j] = sampleTemplateGenerator.getTemplate();
    00822       sampleSet[j].isUsingOdometry = (random() < (collisionPercept.getCollisionAggregate() ? parameters.odometryProbCollision : parameters.odometryProbNoCollision));
    00823       if (testSampleIndex == -1)
    00824       {
    00825         testSample = &sampleSet[j];
    00826         testSampleIndex = j;
    00827       }
    00828     }
    00829   }
    00830   else
    00831   {
    00832     // determine which samples we will replace randomly by
    00833     // template samples
    00834     for(i = 0; i < SAMPLES_MAX; i++)
    00835     {
    00836       GT2005SelfLocatorSample& s = sampleSet[i];
    00837       // if quality is too low, select a new random pose for the sample
    00838       /* template generation not yet implemented*/
    00839       // OUTPUT(idText,text,"number of templates" << sampleTemplateGenerator.getNumberOfTemplates());
    00840       if(s.getProbability()<=1 && sampleTemplateGenerator.getNumberOfTemplates() && random() * averageProbability > s.getProbability())
    00841       {
    00842         s = sampleTemplateGenerator.getTemplate();
    00843         s.isUsingOdometry = (random() < (collisionPercept.getCollisionAggregate() ? parameters.odometryProbCollision : parameters.odometryProbNoCollision));
    00844         if (testSampleIndex == -1)
    00845         {
    00846           testSample = &s;
    00847           testSampleIndex = i;
    00848         }
    00849       }
    00850       if(random() * averageProbability > s.getProbability())
    00851       {
    00852         s = GT2005SelfLocatorSample(field.randomPose());
    00853         s.isUsingOdometry = (random() < (collisionPercept.getCollisionAggregate() ? parameters.odometryProbCollision : parameters.odometryProbNoCollision));
    00854         if (testSampleIndex == -1)
    00855         {
    00856           testSample = &s;
    00857           testSampleIndex = i;
    00858         }
    00859       }
    00860     }
    00861     if (testSampleIndex == -1)
    00862     {
    00863       testSample = &sampleSet[0];
    00864       testSampleIndex = 0;
    00865     }
    00866   }
    00867 }
    00868
    00869
    00870
    00871 double GT2005SelfLocator::calcDistributionValidityByStandardDeviation()
    00872 { //data reset starts
    00873   averageX = 0;
    00874   averageY = 0;
    00875   averageRot = 0;
    00876   stdDevRot = 0;
    00877
    00878   //double eigenVal0, eigenVal1, eigenSwap;
    00879   eigenVal0 = eigenVal1 = eigenSwap = 0;
    00880   eigenVec0 = Vector2<double>(0,0);
    00881   eigenVec1 = Vector2<double>(0,0);
    00882
    00883   varianceCovarianceMatrix[0][0] = varianceCovarianceMatrix[1][0]=
    00884   varianceCovarianceMatrix[0][1] = varianceCovarianceMatrix[1][1] = 0;
    00885
    00886   int i = 0;
    00887
    00888   Vector2<double> angle(0,0);
    00889   double pSum = 0;
    00890   //data reset ends
    00891
    00892   for (i=0;i<SAMPLES_MAX;i++)
    00893   {
    00894     if(sampleSet[i].isValid())
    00895     {
    00896       averageX += sampleSet[i].translation.x * sampleSet[i].getProbability();
    00897       averageY += sampleSet[i].translation.y * sampleSet[i].getProbability();
    00898       angle.x += cos(sampleSet[i].rotation) * sampleSet[i].getProbability();
    00899       angle.y += sin(sampleSet[i].rotation) * sampleSet[i].getProbability();
    00900       pSum += sampleSet[i].getProbability();
    00901     }
    00902   }
    00903
    00904   //calculate the average particle values
    00905   averageX /= pSum;
    00906   averageY /= pSum;
    00907   averageRot = atan2(angle.y,angle.x);
    00908
    00909   // Covariance calculation
    00910
    00911   for (i = 0; i < SAMPLES_MAX; i++)
    00912   {
    00913     if(sampleSet[i].isValid())
    00914     {
    00915       double xErr = (sampleSet[i].translation.x - averageX);
    00916       double yErr = (sampleSet[i].translation.y - averageY);
    00917       double rErr = normalize(sampleSet[i].rotation - averageRot);
    00918       varianceCovarianceMatrix[0][0] += sampleSet[i].getProbability() * xErr * xErr;
    00919       varianceCovarianceMatrix[0][1] += sampleSet[i].getProbability() * xErr * yErr;
    00920       varianceCovarianceMatrix[1][1] += sampleSet[i].getProbability() * yErr * yErr;
    00921       stdDevRot += sampleSet[i].getProbability() * rErr * rErr;
    00922     }
    00923   }
    00924
    00925   varianceCovarianceMatrix[0][0] /= pSum;
    00926   varianceCovarianceMatrix[0][1] /= pSum;
    00927   varianceCovarianceMatrix[1][1] /= pSum;
    00928   stdDevRot /= pSum;
    00929
    00930   varianceCovarianceMatrix[1][0] = varianceCovarianceMatrix[0][1];
    00931
    00932   // eigenvalues
    00933   double p_2,q;
    00934   p_2 = (varianceCovarianceMatrix[0][0] + varianceCovarianceMatrix[1][1])/2;
    00935   q = varianceCovarianceMatrix[0][0] * varianceCovarianceMatrix[1][1] -
    00936       varianceCovarianceMatrix[0][1] * varianceCovarianceMatrix[0][1];
    00937
    00938   eigenVal0 = p_2 + sqrt(fabs(p_2*p_2 - q));
    00939   eigenVal1 = p_2 - sqrt(fabs(p_2*p_2 - q));
    00940
    00941   //eigenVal0 must be bigger than eigenVal1
    00942   if (eigenVal1 > eigenVal0)
    00943   {
    00944     eigenSwap = eigenVal0;
    00945     eigenVal0 = eigenVal1;
    00946     eigenVal1 = eigenSwap;
    00947   }
    00948
    00949   // first eigenvector
    00950   double amount;  //vector amount
    00951
    00952   eigenVec0.y = ((varianceCovarianceMatrix[0][0] - eigenVal0) * varianceCovarianceMatrix[0][1]) /
    00953                  (-varianceCovarianceMatrix[1][1] + eigenVal0 + varianceCovarianceMatrix[0][1]*varianceCovarianceMatrix[0][1]);
    00954   eigenVec0.x = (-varianceCovarianceMatrix[1][1] + eigenVal0) * eigenVec0.y / varianceCovarianceMatrix[0][1];
    00955
    00956   amount = sqrt(eigenVec0.x*eigenVec0.x + eigenVec0.y*eigenVec0.y);
    00957
    00958   //normalize
    00959   if (amount != 0)
    00960   {
    00961     eigenVec0.x  /= amount;
    00962     eigenVec0.y  /= amount;
    00963   }
    00964
    00965   //calculate the 2nd eigenvector
    00966   eigenVec1.y = ((varianceCovarianceMatrix[0][0] - eigenVal1) * varianceCovarianceMatrix[0][1]) /
    00967                  (-varianceCovarianceMatrix[1][1] + eigenVal1 + varianceCovarianceMatrix[0][1]*varianceCovarianceMatrix[0][1]);
    00968   eigenVec1.x = (-varianceCovarianceMatrix[1][1] + eigenVal1) * eigenVec1.y / varianceCovarianceMatrix[0][1];
    00969
    00970   amount = sqrt(eigenVec1.x*eigenVec1.x + eigenVec1.y*eigenVec1.y);
    00971
    00972   //normalize
    00973   if (amount != 0)
    00974   {
    00975     eigenVec1.x  /= amount;
    00976     eigenVec1.y  /= amount;
    00977   }
    00978
    00979   //both eigenvectors should have positive x-coordinates
    00980   if (eigenVec0.x < 0)
    00981   {
    00982     eigenVec0 *= -1;
    00983   }
    00984
    00985   if (eigenVec1.x < 0)
    00986   {
    00987     eigenVec1 *= -1;
    00988   }
    00989
    00990   eigenVal0 = sqrt(eigenVal0); //eigenVal is now the new std-deviation for position
    00991   eigenVal1 = sqrt(eigenVal1);
    00992   stdDevRot = sqrt(stdDevRot);
    00993
    00994   eigenVec0 *= eigenVal0;
    00995   eigenVec1 *= eigenVal1;
    00996   double validity = max(eigenVal0,eigenVal1);
    00997   robotPose.setPositionVariance(validity*validity);
    00998
    00999   robotPose.directionOfGreatestUncertaintyExists = (eigenVal0 >= 1.7 * eigenVal1);
    01000   robotPose.greatestUncertainty.translation.x = averageX;
    01001   robotPose.greatestUncertainty.translation.y = averageY;
    01002   robotPose.greatestUncertainty.rotation = atan2(eigenVec0.y, eigenVec0.x);
    01003
    01004   validity = gaussian(validity/500);
    01005   validity = min(validity,gaussian(stdDevRot/0.5));  //validity is min of position and angle validity
    01006   DEBUG_RESPONSE("gt05-sl:display eigenvalues", OUTPUT(idText,text,"gt05-sl " << eigenVal0 <<  "   " << eigenVal1 << "  " << validity););
    01007
    01008   NCOMPLEX_DRAWING("gt05-sl:covarianceEllipse",
    01009
    01010     NLINE("gt05-sl:covarianceEllipse",averageX,averageY,averageX+300*cos(averageRot),averageY+300*sin(averageRot),10,Drawings::ps_solid,Drawings::black);
    01011     NLINE("gt05-sl:covarianceEllipse",averageX,
    01012       averageY,
    01013       averageX+600*cos(averageRot+stdDevRot),
    01014       averageY+600*sin(averageRot+stdDevRot),
    01015       10,Drawings::ps_solid,Drawings::red);
    01016     NLINE("gt05-sl:covarianceEllipse",averageX,
    01017       averageY,
    01018       averageX+600*cos(averageRot-stdDevRot),
    01019       averageY+600*sin(averageRot-stdDevRot),
    01020       10,Drawings::ps_solid,Drawings::red);
    01021
    01022     double alpha = atan2(eigenVec0.y,eigenVec0.x);
    01023     double a = eigenVal0;
    01024     double b = eigenVal1;
    01025     double x1 = 0;
    01026     double x2 = 0;
    01027     double y1 = 0;
    01028     double y2 = 0;
    01029
    01030     int step = (int)(a/10);
    01031     if (!step)
    01032     {
    01033       step = 1;
    01034     }
    01035
    01036     for (int xc = 0; (xc + step) < a; xc += step)
    01037     {
    01038       x1 = xc;
    01039       y1 = (sqrt(1-sqr(x1/a))* b);
    01040       x2 = xc + step;
    01041       y2 = (sqrt(1-sqr(x2/a))* b);
    01042
    01043
    01044       NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(x1) + averageX - sin(alpha)*(y1),
    01045         sin(alpha)*(x1) + cos(alpha)*(y1) + averageY,
    01046         cos(alpha)*(x2) + averageX - sin(alpha)*(y2),
    01047         sin(alpha)*(x2) + cos(alpha)*(y2) + averageY,
    01048         4, Drawings::ps_solid,
    01049         Drawings::red);
    01050       NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(x1) + averageX - sin(alpha)*(-y1),
    01051         sin(alpha)*(x1) + cos(alpha)*(-y1) + averageY,
    01052         cos(alpha)*(x2) + averageX - sin(alpha)*(-y2),
    01053         sin(alpha)*(x2) + cos(alpha)*(-y2) + averageY,
    01054         4, Drawings::ps_solid,
    01055         Drawings::red);
    01056       NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(-x1) + averageX - sin(alpha)*(y1),
    01057         sin(alpha)*(-x1) + cos(alpha)*(y1) + averageY,
    01058         cos(alpha)*(-x2) + averageX - sin(alpha)*(y2),
    01059         sin(alpha)*(-x2) + cos(alpha)*(y2) + averageY,
    01060         4, Drawings::ps_solid,
    01061         Drawings::red);
    01062       NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(-x1) + averageX - sin(alpha)*(-y1),
    01063         sin(alpha)*(-x1) + cos(alpha)*(-y1) + averageY,
    01064         cos(alpha)*(-x2) + averageX - sin(alpha)*(-y2),
    01065         sin(alpha)*(-x2) + cos(alpha)*(-y2) + averageY,
    01066         4, Drawings::ps_solid,
    01067         Drawings::red);
    01068     }
    01069
    01070     x1 = a;
    01071     y1 = 0;
    01072
    01073     NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(x1) + averageX - sin(alpha)*(y1),
    01074       sin(alpha)*(x1) + cos(alpha)*(y1) + averageY,
    01075       cos(alpha)*(x2) + averageX - sin(alpha)*(y2),
    01076       sin(alpha)*(x2) + cos(alpha)*(y2) + averageY,
    01077       4, Drawings::ps_solid,
    01078       Drawings::red);
    01079     NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(x1) + averageX - sin(alpha)*(-y1),
    01080       sin(alpha)*(x1) + cos(alpha)*(-y1) + averageY,
    01081       cos(alpha)*(x2) + averageX - sin(alpha)*(-y2),
    01082       sin(alpha)*(x2) + cos(alpha)*(-y2) + averageY,
    01083       4, Drawings::ps_solid,
    01084       Drawings::red);
    01085     NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(-x1) + averageX - sin(alpha)*(y1),
    01086       sin(alpha)*(-x1) + cos(alpha)*(y1) + averageY,
    01087       cos(alpha)*(-x2) + averageX - sin(alpha)*(y2),
    01088       sin(alpha)*(-x2) + cos(alpha)*(y2) + averageY,
    01089       4, Drawings::ps_solid,
    01090       Drawings::red);
    01091     NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(-x1) + averageX - sin(alpha)*(-y1),
    01092       sin(alpha)*(-x1) + cos(alpha)*(-y1) + averageY,
    01093       cos(alpha)*(-x2) + averageX - sin(alpha)*(-y2),
    01094       sin(alpha)*(-x2) + cos(alpha)*(-y2) + averageY,
    01095       4, Drawings::ps_solid,
    01096       Drawings::red);
    01097   );
    01098
    01099
    01100   // for now, dont use the covariances
    01101   return validity;//gaussian( sqrt(varianceCovarianceMatrix[0][0] + varianceCovarianceMatrix[1][1] + averageRotDeviation*averageRotDeviation) / 300);
    01102   // using the maximum eigenvalue seems to work fine, too;
    01103   //eigenvalue is sqrt(eigenvalue) now
    01104
    01105 }
    01106
    01107
    01108 double GT2005SelfLocator::calcDistributionValidityByEntropy()
    01109 {
    01110   int gridX, gridY, gridR;
    01111   gridX = 5;
    01112   gridY = 5;
    01113   gridR = 5;
    01114
    01115   int grid[15][15][15];
    01116   double entropy = 0;
    01117
    01118   double x = 6000;
    01119   double y = 3600;
    01120   int transx = 0;
    01121   int transy = 0;
    01122   int rot = 0;
    01123   int i;
    01124
    01125   for(i = 0; i < (gridX * 3); ++i)
    01126     for(int j = 0; j < (gridY * 3); ++j)
    01127       for(int k = 0; k < (gridR * 3); ++k)
    01128       {
    01129         grid[i][j][k] = 0;
    01130       }
    01131
    01132
    01133   for(i = 0; i < SAMPLES_MAX; ++i)
    01134   {
    01135     transx = (int)((sampleSet[i].translation.x + 1.5*x) / x / 3 * gridX);
    01136     transy = (int)((sampleSet[i].translation.y + 1.5*y) / y / 3 * gridY);
    01137     rot = (int)((sampleSet[i].rotation + pi) / (2*pi) * gridR);
    01138
    01139     if(transx < 0) {transx = 0;}
    01140     if(transy < 0) {transy = 0;}
    01141     while(rot < 0) {rot = rot + gridR;}
    01142
    01143     if(transx > gridX) {transx = (gridX-1);}
    01144     if(transy > gridY) {transy = (gridY-1);}
    01145     while(rot >= gridR) {rot = (rot - gridR);}
    01146
    01147     grid[transx][transy][rot]++;
    01148   }
    01149
    01150   for(i = 0; i < gridX; ++i)
    01151     for(int j = 0; j < gridY; ++j)
    01152       for(int k = 0; k < gridR; ++k)
    01153       {
    01154         if (grid[i][j][k])
    01155         {
    01156           entropy -= ((double)grid[i][j][k]/SAMPLES_MAX)*log((double)grid[i][j][k]/SAMPLES_MAX);
    01157         }
    01158       }
    01159   entropy /= log(2.0);
    01160
    01161   return  gaussian(entropy);
    01162 }
    01163
    01164 RobotPose GT2005SelfLocator::calcPoseFromSubCube(Cell poseSpace[POSE_SPACE_GRID][POSE_SPACE_GRID][POSE_SPACE_GRID],int xMax, int yMax, int rMax)
    01165 {
    01166     // determine the average pose of all samples in the winner sub-cube
    01167     double xSum = 0,
    01168            ySum = 0,
    01169            cosSum = 0,
    01170            sinSum = 0,
    01171            probabilitySum = 0;
    01172
    01173     RobotPose result;
    01174
    01175   // iterate over the rotation axis of the sub-cube
    01176     for(int r = 0; r < 2; r++)
    01177     {
    01178     // just as before: r axis is cyclic
    01179     int rNext = (rMax + r) % POSE_SPACE_GRID;
    01180
    01181     // iterate over the traslation axes...
    01182     for(int y = 0; y < 2; y++)
    01183       for(int x = 0; x < 2; x++)
    01184       {
    01185         // iterate over the list of samples inside the cell
    01186         // cell.first is 0 if the queue is empty
    01187         // else p->next is 0 at the end of the queue
    01188         for(GT2005SelfLocatorSample* sample = poseSpace[rNext][yMax + y][xMax + x].first; sample; sample = sample->next)
    01189           if(sample->isValid())
    01190           {
    01191             // sum the x and y positions of the samples weighted by probability
    01192             // sum the sine and cosine of the samples rotation weighted by probability
    01193             // calculate the whole sum of probabilities to calculate the weighted averages
    01194             xSum += sample->translation.x * sample->getProbability();
    01195             ySum += sample->translation.y * sample->getProbability();
    01196             cosSum += sample->getCos() * sample->getProbability();
    01197             sinSum += sample->getSin() * sample->getProbability();
    01198             probabilitySum += sample->getProbability();
    01199           }
    01200       }
    01201   }
    01202
    01203   // now calculate the pose as the weighted averages of x,y,r
    01204   if(probabilitySum>0)
    01205   {
    01206     double weightedX = xSum / probabilitySum;
    01207     double weightedY = ySum / probabilitySum;
    01208     double weightedRot = atan2(sinSum,cosSum);
    01209       result.setPose(Pose2D(weightedRot,Vector2<double>(weightedX,weightedY)) );
    01210       result.setValidity(probabilitySum / SAMPLES_MAX);
    01211     // clip the pose to the field
    01212     if (parameters.clipRobotPose)
    01213     {
    01214       double diff = field.clip(result.translation);
    01215       if(diff > 1){
    01216         // double vold = validity;
    01217         result.setValidity(result.getValidity() / sqrt(diff));  // gt04: Why do we use the sqrt?
    01218       }
    01219     }
    01220   }
    01221
    01222   return result;
    01223 }
    01224
    01225 /**
    01226 * The function determines the most probable pose from the sample distribution.
    01227 * reimplementation of the gt04 calcPose method
    01228 * @param pose The pose is returned to this variable.
    01229 * @param validity The validity of the pose is returned to this variable.
    01230 */
    01231 void GT2005SelfLocator::calcPose(Pose2D& pose,double& validity)
    01232 {
    01233   // the 3d pose-space grid
    01234   Cell poseSpace[POSE_SPACE_GRID][POSE_SPACE_GRID][POSE_SPACE_GRID];
    01235
    01236   double width = field.x.getSize(),
    01237          height = field.y.getSize();
    01238
    01239   int i;
    01240   // attach the samples to the corresponding cells of the poseSpace
    01241   for(i = 0; i < SAMPLES_MAX; i++)
    01242   {
    01243     GT2005SelfLocatorSample& sample = sampleSet[i];
    01244
    01245     if(sample.isValid())
    01246     {
    01247     // r: Index on the rotation axis of the cube. as angles are between
    01248     //    [-pi..0] [0..pi] we put the 0 of the rotation axis on 0.5*POSE_SPACE_GRID
    01249     //    r = [(angle / 2*pi) + 0.5] * POSE_SPACE_GRID
    01250       int r = static_cast<int>((sample.getAngle() / pi2 + 0.5) * POSE_SPACE_GRID);
    01251     //  x,y: Index on the translation axes. similar to the rotation, the 0 position
    01252     //       is at 0.5*POSE_SPACE_GRID.
    01253     int y = static_cast<int>((sample.translation.y / height + 0.5) * POSE_SPACE_GRID);
    01254       int x = static_cast<int>((sample.translation.x / width + 0.5) * POSE_SPACE_GRID);
    01255
    01256     // translation axes are non-cyclic, so we set [x|y] < 0 to 0 and
    01257     // [x|y] >= POSE_SPACE_GRID to POSE_SPACE_GRID-1
    01258       if(x < 0)
    01259         x = 0;
    01260       else if(x >= POSE_SPACE_GRID)
    01261         x = POSE_SPACE_GRID-1;
    01262
    01263       if(y < 0)
    01264         y = 0;
    01265       else if(y >= POSE_SPACE_GRID)
    01266         y = POSE_SPACE_GRID-1;
    01267
    01268     // the rotation axis is cyclic so if we run out of the interval at
    01269     // the one side, we are at the other...
    01270       if(r < 0)
    01271         r = POSE_SPACE_GRID-1;
    01272       else if(r >= POSE_SPACE_GRID)
    01273         r = 0;
    01274
    01275     // fill the queue of samples for this cell by
    01276     // inserting before first sample
    01277       Cell& cell = poseSpace[r][y][x];
    01278     //as cell.first is preinitialized with 0, the end
    01279     //of the queue is always marked with 0
    01280       sample.next = cell.first;
    01281       cell.first = &sample;
    01282       cell.count++;
    01283     }
    01284   }
    01285
    01286   // determine the 2x2x2 sub-cube that contains most samples
    01287
    01288   // the position and sample-count of the winning sub-cube
    01289   int rMax[NUM_OF_CALCULATED_POSES],
    01290       yMax[NUM_OF_CALCULATED_POSES],
    01291       xMax[NUM_OF_CALCULATED_POSES],
    01292       countMax[NUM_OF_CALCULATED_POSES];
    01293
    01294   for (i=0;i<NUM_OF_CALCULATED_POSES;i++)
    01295   {
    01296     rMax[i] = 0;
    01297     yMax[i] = 0;
    01298     xMax[i] = 0;
    01299     countMax[i] = 0;
    01300   }
    01301
    01302   // iterate wih "r"over the rotation axis...
    01303   for(int r = 0; r < POSE_SPACE_GRID; r++)
    01304   {
    01305   // as this axis is cyclic, the next rotation index is:
    01306     int rNext = (r + 1) % POSE_SPACE_GRID;
    01307
    01308   // iterate with "x" and "y" over the translation axes
    01309     for(int y = 0; y < POSE_SPACE_GRID - 1; y++)
    01310       for(int x = 0; x < POSE_SPACE_GRID - 1; x++)
    01311       {
    01312     // get the number of all samples in the subcube
    01313     // around the current cell [r][y][x]
    01314         int count = poseSpace[r][y][x].count +
    01315                     poseSpace[r][y][x+1].count +
    01316                     poseSpace[r][y+1][x].count +
    01317                     poseSpace[r][y+1][x+1].count +
    01318                     poseSpace[rNext][y][x].count +
    01319                     poseSpace[rNext][y][x+1].count +
    01320                     poseSpace[rNext][y+1][x].count +
    01321                     poseSpace[rNext][y+1][x+1].count;
    01322     // if we've got a new winning cube, we have to remember
    01323         if(count > countMax[0])
    01324         {
    01325
    01326           if (NUM_OF_CALCULATED_POSES>1)
    01327           {
    01328             for (i=NUM_OF_CALCULATED_POSES-1;i>0;i--)
    01329             {
    01330               rMax[i] = rMax[i-1];
    01331               xMax[i] = xMax[i-1];
    01332               yMax[i] = yMax[i-1];
    01333               countMax[i] = countMax[i-1];
    01334             }
    01335           }
    01336           countMax[0] = count;
    01337           rMax[0] = r;
    01338           yMax[0] = y;
    01339           xMax[0] = x;
    01340         }
    01341       }
    01342   }
    01343
    01344   if(countMax[0] > 0)
    01345   {
    01346     for (i=0;i<NUM_OF_CALCULATED_POSES;i++)
    01347     {
    01348       if (countMax[i]>0)
    01349       {
    01350         robotPoseCollection.poses[i] = calcPoseFromSubCube(poseSpace, xMax[i],yMax[i],rMax[i]);
    01351         CIRCLE(selfLocatorField,
    01352                robotPoseCollection.poses[i].translation.x,
    01353                robotPoseCollection.poses[i].translation.y,
    01354                50,50,Drawings::ps_solid,Drawings::yellowOrange);
    01355       } else
    01356       {
    01357         robotPoseCollection.poses[i].setValidity(-1);
    01358       }
    01359     }
    01360     pose = robotPoseCollection.poses[0].getPose();
    01361     validity = robotPoseCollection.poses[0].getValidity();
    01362
    01363     //calc validity by covariance
    01364     DEBUG_RESPONSE("gt05-sl:validity by covariance",
    01365       validity = calcDistributionValidityByStandardDeviation();
    01366     );
    01367     //or calc validity by Entropy
    01368     DEBUG_RESPONSE("gt05-sl:validity by entropy",
    01369       validity = calcDistributionValidityByEntropy();
    01370     );
    01371
    01372
    01373     //robotPoseCollection.poses = candidates;
    01374     robotPoseCollection.numberOfPoses = NUM_OF_CALCULATED_POSES;
    01375   } else {
    01376     validity = 0;
    01377   }
    01378
    01379   // Vector2<double> diff = gtCamWorldState.getRobotPose().translation - pose.translation;
    01380   // OUTPUT(idText,text,"gt2005 "<< diff.abs() << " " << validity);
    01381 }
    01382
    01383 bool GT2005SelfLocator::handleMessage(InMessage& message)
    01384 {/*
    01385   switch(message.getMessageID())
    01386   {
    01387     case idMSHSelfLocator:
    01388     {
    01389       GenericDebugData d;
    01390       message.bin >> d;
    01391       GT2005SelfLocatorSample::paramProbUpLimit   = d.data[0];
    01392       GT2005SelfLocatorSample::paramProbDownLimit = d.data[1];
    01393       paramObservationUpdateZAngleErrorWeight  = d.data[2];
    01394       paramObservationUpdateYAngleErrorWeight  = d.data[3];  // ie gauss sharpness for y-angle errors
    01395       paramObservationUpdateZAngleErrorCrossings = d.data[4];
    01396       paramObservationUpdateYAngleErrorCrossings = d.data[5];
    01397     }
    01398     break;
    01399   }*/
    01400   return false;
    01401 }
    01402
    01403 void GT2005SelfLocator::resetSamples()
    01404 {
    01405   for(int i = 0; i < SAMPLES_MAX; i++)
    01406   {
    01407     sampleSet[i].reset();
    01408     (Pose2D&) sampleSet[i] = field.randomPose();
    01409   }
    01410 }
    01411
    01412 void GT2005SelfLocator::resetSamples(const Pose2D &pose)
    01413 {
    01414   for(int i = 0; i < SAMPLES_MAX; i++)
    01415   {
    01416     sampleSet[i].reset();
    01417     (Pose2D&) sampleSet[i] = pose;
    01418   }
    01419 }
    01420
    01421 void GT2005SelfLocator::resetSamplesWhenPenalized()
    01422 {
    01423   // erase samples
    01424   int i;
    01425   Pose2D t;
    01426   double randomFactor = 3;
    01427
    01428   // place 25% samples at left center spot
    01429   t.translation.x = 0;
    01430   t.translation.y = yPosLeftSideline;
    01431   t.rotation = -pi_2;
    01432   for(i = 0; i < SAMPLES_MAX / 4; ++i)
    01433   {
    01434     sampleSet[i].reset();
    01435     (Pose2D&) sampleSet[i] = Pose2D::random(Range<double>(t.translation.x - randomFactor * 50,
    01436                                                 t.translation.x + randomFactor * 50),
    01437                                   Range<double>(t.translation.y - randomFactor * 50,
    01438                                                 t.translation.y + randomFactor * 50),
    01439                                   Range<double>(t.getAngle() - randomFactor * 0.1,
    01440                                                 t.getAngle() + randomFactor * 0.1));
    01441   }
    01442   // place 25% samples at rightt center spot
    01443   t.translation.y = yPosRightSideline;
    01444   t.rotation = pi_2;
    01445   for(; i < SAMPLES_MAX / 2; ++i)
    01446   {
    01447     sampleSet[i].reset();
    01448     (Pose2D&) sampleSet[i] = Pose2D::random(Range<double>(t.translation.x - randomFactor * 50,
    01449                                                 t.translation.x + randomFactor * 50),
    01450                                   Range<double>(t.translation.y - randomFactor * 50,
    01451                                                 t.translation.y + randomFactor * 50),
    01452                                   Range<double>(t.getAngle() - randomFactor * 0.1,
    01453                                                 t.getAngle() + randomFactor * 0.1));
    01454   }
    01455
    01456   for(; i < SAMPLES_MAX; ++i)
    01457   {
    01458     sampleSet[i].reset();
    01459     (Pose2D&) sampleSet[i] = field.randomPose();
    01460   }
    01461 }
    01462
    01463
    01464 ///////////////////////////////////////////////////////////////////////
    01465 // Debug drawing
    01466
    01467 void GT2005SelfLocator::draw() const
    01468 {
    01469   NCOMPLEX_DRAWING ( "gt05-sl:sampleSet",
    01470     // draw sample set
    01471     for(int i = 0; i < SAMPLES_MAX; ++i)
    01472       draw(sampleSet[i], getSampleColor(sampleSet[i].probability));
    01473   );
    01474
    01475   NCOMPLEX_DRAWING( "gt05-sl:testSample",
    01476     drawTestSample(*testSample, getSampleColor(testSample->probability));
    01477   );
    01478
    01479   NCOMPLEX_DRAWING ( "gt05-sl:lastSeenCenterCircle",
    01480   {
    01481     //debug stuff
    01482     Pose2D pointFromPose = robotPose + lastSeenCenterCircle;
    01483     NCIRCLE( "gt05-sl:lastSeenCenterCircle",
    01484       pointFromPose.translation.x, pointFromPose.translation.y,
    01485       60,30,Drawings::ps_solid,Drawings::white);
    01486     Pose2D p1 = pointFromPose + Pose2D(0,60);
    01487     Pose2D p2 = pointFromPose + Pose2D(0,-60);
    01488     NLINE("gt05-sl:lastSeenCenterCircle",
    01489         p1.translation.x,
    01490         p1.translation.y,
    01491         p2.translation.x,
    01492         p2.translation.y,
    01493         1,
    01494         Drawings::ps_solid,
    01495         Drawings::white);
    01496   });
    01497
    01498   NCOMPLEX_DRAWING( "gt05-sl:lastCrossing",
    01499   {
    01500     int color = Drawings::black;
    01501     switch(lastSeenCrossingClass)
    01502     {
    01503       case GT2005LineCrossingsTable::virtualCrossing:
    01504         color = Drawings::blue;
    01505         break;
    01506       case GT2005LineCrossingsTable::lCrossing:
    01507         color = Drawings::yellow;
    01508         break;
    01509       case GT2005LineCrossingsTable::tCrossing:
    01510         color = Drawings::red;
    01511         break;
    01512       case GT2005LineCrossingsTable::xCrossing:
    01513         color = Drawings::pink;
    01514         break;
    01515     }
    01516     NCIRCLE("gt05-sl:lastCrossing",lastModelCrossing.x,lastModelCrossing.y,40,40,Drawings::ps_solid,color);
    01517     NCIRCLE("gt05-sl:lastCrossing",lastSeenCrossing.x,lastSeenCrossing.y,30,30,Drawings::ps_solid,Drawings::red);
    01518     NLINE("gt05-sl:lastCrossing",lastSeenCrossing.x,lastSeenCrossing.y,lastModelCrossing.x,lastModelCrossing.y,10,Drawings::ps_solid,Drawings::blue);
    01519   });
    01520
    01521   DEBUG_RESPONSE("gt05-sl:display field tables",
    01522     Field field;
    01523     field.draw(Drawings::white, LinesPercept::field);
    01524     field.draw(Drawings::red, LinesPercept::border);
    01525     field.draw(Drawings::yellow, LinesPercept::yellowGoal);
    01526     field.draw(Drawings::skyblue, LinesPercept::skyblueGoal);
    01527   );
    01528
    01529   DEBUG_RESPONSE("gt05-sl:display field lines observation table, 0 deg",
    01530     observationTable[LinesPercept::field].draw(0);
    01531   );
    01532   DEBUG_RESPONSE("gt05-sl:display field lines observation table, 90 deg",
    01533     observationTable[LinesPercept::field].draw(pi_2);
    01534   );
    01535   DEBUG_RESPONSE("gt05-sl:display field lines observation table, 180 deg",
    01536     observationTable[LinesPercept::field].draw(pi);
    01537   );
    01538   DEBUG_RESPONSE("gt05-sl:display field lines observation table, -90 deg",
    01539     observationTable[LinesPercept::field].draw(-pi_2);
    01540   );
    01541
    01542   DEBUG_RESPONSE("gt05-sl:display yellow goal lines observation table, 0 deg",
    01543     observationTable[LinesPercept::yellowGoal].draw(0);
    01544   );
    01545   DEBUG_RESPONSE("gt05-sl:display yellow goal lines observation table, 90 deg",
    01546     observationTable[LinesPercept::yellowGoal].draw(pi_2);
    01547   );
    01548   DEBUG_RESPONSE("gt05-sl:display yellow goal lines observation table, 180 deg",
    01549     observationTable[LinesPercept::yellowGoal].draw(pi);
    01550   );
    01551   DEBUG_RESPONSE("gt05-sl:display yellow goal lines observation table, -90 deg",
    01552     observationTable[LinesPercept::yellowGoal].draw(-pi_2);
    01553   );
    01554
    01555   DEBUG_RESPONSE("gt05-sl:display carpet border observation table, 0 deg",
    01556     observationTable[LinesPercept::border].draw(0);
    01557   );
    01558   DEBUG_RESPONSE("gt05-sl:display carpet border observation table, 90 deg",
    01559     observationTable[LinesPercept::border].draw(pi_2);
    01560   );
    01561   DEBUG_RESPONSE("gt05-sl:display carpet border observation table, 180 deg",
    01562     observationTable[LinesPercept::border].draw(pi);
    01563   );
    01564   DEBUG_RESPONSE("gt05-sl:display carpet border observation table, -90 deg",
    01565     observationTable[LinesPercept::border].draw(-pi_2);
    01566   );
    01567
    01568 }
    01569
    01570 Drawings::Color GT2005SelfLocator::getSampleColor(double probability) const
    01571 {
    01572   if (probability == 2 || probability <= QUASI_ZERO)
    01573     return Drawings::black;
    01574   else if (probability < 0.1)
    01575     return Drawings::blue;
    01576   else if (probability < 0.2)
    01577     return Drawings::yellow;
    01578   else if (probability < 0.5)
    01579     return Drawings::orange;
    01580   else if (probability < 0.8)
    01581     return Drawings::red;
    01582   else if (probability < 0.95)
    01583     return Drawings::pink;
    01584   else
    01585     return Drawings::white;
    01586 }
    01587
    01588 void GT2005SelfLocator::draw(const Pose2D& pose,Drawings::Color color) const
    01589 {
    01590   Pose2D p = pose + Pose2D(-100,0);
    01591   NLINE("gt05-sl:sampleSet",
    01592        pose.translation.x,
    01593        pose.translation.y,
    01594        p.translation.x,
    01595        p.translation.y,
    01596        1,
    01597        Drawings::ps_solid,
    01598        color);
    01599   p = pose + Pose2D(-40,-40);
    01600   NLINE("gt05-sl:sampleSet",
    01601        pose.translation.x,
    01602        pose.translation.y,
    01603        p.translation.x,
    01604        p.translation.y,
    01605        1,
    01606        Drawings::ps_solid,
    01607        color);
    01608   p = pose + Pose2D(-40,40);
    01609   NLINE("gt05-sl:sampleSet",
    01610        pose.translation.x,
    01611        pose.translation.y,
    01612        p.translation.x,
    01613        p.translation.y,
    01614        1,
    01615        Drawings::ps_solid,
    01616        color);
    01617 }
    01618
    01619 void GT2005SelfLocator::drawTestSample(const Pose2D& pose,Drawings::Color color) const
    01620 {
    01621   Pose2D p = pose + Pose2D(-100,0);
    01622   NLINE("gt05-sl:testSample",
    01623        pose.translation.x,
    01624        pose.translation.y,
    01625        p.translation.x,
    01626        p.translation.y,
    01627        1,
    01628        Drawings::ps_solid,
    01629        color);
    01630   p = pose + Pose2D(-40,-40);
    01631   NLINE("gt05-sl:testSample",
    01632        pose.translation.x,
    01633        pose.translation.y,
    01634        p.translation.x,
    01635        p.translation.y,
    01636        1,
    01637        Drawings::ps_solid,
    01638        color);
    01639   p = pose + Pose2D(-40,40);
    01640   NLINE("gt05-sl:testSample",
    01641        pose.translation.x,
    01642        pose.translation.y,
    01643        p.translation.x,
    01644        p.translation.y,
    01645        1,
    01646        Drawings::ps_solid,
    01647        color);
    01648 }