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

# Modules/SelfLocator/GT2005StableSelfLocator/GT2005StableSelfLocator.cpp

Go to the documentation of this file.

    00001 #include "GT2005StableSelfLocator.h"
    00002
    00003 #include "Tools/Math/Common.h"
    00004
    00005 /*
    00006   todo
    00007     template generation by line types is not yet done properly
    00008     needs change in class structure first
    00009     implement encapsuled motion and observation model
    00010
    00011     negative information update not working properly, now commented-out
    00012
    00013     changing number of percept types: change probability calculation in sample
    00014                                       change average calculation in the self locator
    00015
    00016     DEBUG_RESPONSE output, comments adapt to gt2004
    00017     declare some functions inline
    00018
    00019     diffs to gt04
    00020       -execute split up in new mehtods
    00021       -sample.setPerceptProbability replaces setProbability
    00022         -enum PerceptType to avoid strange LineType enum usage for setProbability
    00023       -usage of line crossings percepts
    00024       -negative information (not yet working, commented-out)
    00025       -middle circle as flag (not yet working)
    00026       -head height extracted from camera matrix
    00027 */
    00028
    00029 double GT2005StableSelfLocator::translationNoise = 200,
    00030        GT2005StableSelfLocator::rotationNoise = 0.5,
    00031        GT2005StableSelfLocator::movedDistWeight = 0.002,
    00032        GT2005StableSelfLocator::movedAngleWeight = 0.2,
    00033        GT2005StableSelfLocator::majorDirTransWeight = 0.1,
    00034        GT2005StableSelfLocator::minorDirTransWeight = 0.02,
    00035
    00036        GT2005StableSelfLocator::headHeight = 150,
    00037
    00038
    00039 // observation update parameters
    00040 // trust = weight / (variance)
    00041 // motiondependent variance = variance * (speed*dependency + 1) as variance estimated for a standing robot
    00042
    00043 // at the moment we take sqrt variance instead of variance..
    00044
    00045        GT2005StableSelfLocator::quasiZero = 0.000001,
    00046        GT2005StableSelfLocator::maximumTrust = 100,
    00047
    00048        GT2005StableSelfLocator::linePointZAngleVariance = 1.949,
    00049        GT2005StableSelfLocator::linePointZAngleMotionDependentVariance = 1.949,
    00050        GT2005StableSelfLocator::linePointYAngleVariance = 9.745,
    00051        GT2005StableSelfLocator::linePointYAngleMotionDependentVariance = 9.745,
    00052        GT2005StableSelfLocator::linePointZAngleMotionDependency = 3/400,
    00053        GT2005StableSelfLocator::linePointYAngleMotionDependency = 3/400,
    00054
    00055        GT2005StableSelfLocator::crossingZAngleVariance = 1.949,
    00056        GT2005StableSelfLocator::crossingZAngleMotionDependentVariance = 1.949,
    00057        GT2005StableSelfLocator::crossingYAngleVariance = 9.745,
    00058        GT2005StableSelfLocator::crossingYAngleMotionDependentVariance = 9.745,
    00059        GT2005StableSelfLocator::crossingZAngleMotionDependency = 1/400,
    00060        GT2005StableSelfLocator::crossingYAngleMotionDependency = 1/400,
    00061        GT2005StableSelfLocator::centerCircleZAngleVariance = 1.949,
    00062        GT2005StableSelfLocator::centerCircleZAngleMotionDependentVariance = 1.949,
    00063        GT2005StableSelfLocator::centerCircleYAngleVariance = 9.745,
    00064        GT2005StableSelfLocator::centerCircleYAngleMotionDependentVariance = 9.745,
    00065        GT2005StableSelfLocator::centerCircleOrientationAngleVariance = 9.745,
    00066        GT2005StableSelfLocator::centerCircleOrientationAngleMotionDependentVariance = 9.745,
    00067        GT2005StableSelfLocator::centerCircleZAngleMotionDependency = 1/400,
    00068        GT2005StableSelfLocator::centerCircleYAngleMotionDependency = 1/400,
    00069        GT2005StableSelfLocator::centerCircleOrientationAngleMotionDependency = 1/400,
    00070
    00071        GT2005StableSelfLocator::flagZAngleVariance = 1.949,
    00072        GT2005StableSelfLocator::flagZAngleMotionDependentVariance = 1.949,
    00073        GT2005StableSelfLocator::flagYAngleVariance = 9.745,
    00074        GT2005StableSelfLocator::flagYAngleMotionDependentVariance = 9.745,
    00075        GT2005StableSelfLocator::flagZAngleMotionDependency = 1/400,
    00076        GT2005StableSelfLocator::flagYAngleMotionDependency = 1/400,
    00077
    00078        GT2005StableSelfLocator::goalZAngleVariance = 1.949,
    00079        GT2005StableSelfLocator::goalZAngleMotionDependentVariance = 1.949,
    00080        GT2005StableSelfLocator::goalYAngleVariance = 9.745,
    00081        GT2005StableSelfLocator::goalYAngleMotionDependentVariance = 9.745,
    00082        GT2005StableSelfLocator::goalZAngleMotionDependency = 1/400,
    00083        GT2005StableSelfLocator::goalYAngleMotionDependency = 1/400,
    00084
    00085        GT2005StableSelfLocator::linePointWeight = 10,//19.49,
    00086        GT2005StableSelfLocator::linePointZAngleTrust =
    00087        GT2005StableSelfLocator::linePointWeight / ( GT2005StableSelfLocator::linePointZAngleVariance + GT2005StableSelfLocator::quasiZero ),
    00088        GT2005StableSelfLocator::linePointYAngleTrust =
    00089        GT2005StableSelfLocator::linePointWeight / ( GT2005StableSelfLocator::linePointYAngleVariance + GT2005StableSelfLocator::quasiZero ),
    00090
    00091        GT2005StableSelfLocator::crossingWeight = 7,// 9.745,
    00092        GT2005StableSelfLocator::crossingZAngleTrust =
    00093        GT2005StableSelfLocator::crossingWeight / ( GT2005StableSelfLocator::crossingZAngleVariance + GT2005StableSelfLocator::quasiZero ),
    00094        GT2005StableSelfLocator::crossingYAngleTrust =
    00095        GT2005StableSelfLocator::crossingWeight / ( GT2005StableSelfLocator::crossingYAngleVariance + GT2005StableSelfLocator::quasiZero ),
    00096
    00097        GT2005StableSelfLocator::centerCircleWeight = 0,// 9.745,
    00098        GT2005StableSelfLocator::centerCircleZAngleTrust =
    00099        GT2005StableSelfLocator::centerCircleWeight / ( GT2005StableSelfLocator::centerCircleZAngleVariance + GT2005StableSelfLocator::quasiZero ),
    00100        GT2005StableSelfLocator::centerCircleYAngleTrust =
    00101        GT2005StableSelfLocator::centerCircleWeight / ( GT2005StableSelfLocator::centerCircleYAngleVariance + GT2005StableSelfLocator::quasiZero ),
    00102        GT2005StableSelfLocator::centerCircleOrientationAngleTrust =
    00103        GT2005StableSelfLocator::centerCircleWeight / ( GT2005StableSelfLocator::centerCircleOrientationAngleVariance + GT2005StableSelfLocator::quasiZero ),
    00104
    00105        GT2005StableSelfLocator::flagWeight = 13,//13.643,
    00106        GT2005StableSelfLocator::flagZAngleTrust =
    00107        GT2005StableSelfLocator::flagWeight / ( GT2005StableSelfLocator::flagZAngleVariance + GT2005StableSelfLocator::quasiZero ),
    00108        GT2005StableSelfLocator::flagYAngleTrust =
    00109        GT2005StableSelfLocator::flagWeight / ( GT2005StableSelfLocator::flagYAngleVariance + GT2005StableSelfLocator::quasiZero ),
    00110
    00111
    00112        GT2005StableSelfLocator::goalWeight = 13,//13.643,
    00113        GT2005StableSelfLocator::goalZAngleTrust =
    00114        GT2005StableSelfLocator::goalWeight / ( GT2005StableSelfLocator::goalZAngleVariance + GT2005StableSelfLocator::quasiZero ),
    00115        GT2005StableSelfLocator::goalYAngleTrust =
    00116        GT2005StableSelfLocator::goalWeight / ( GT2005StableSelfLocator::goalYAngleVariance + GT2005StableSelfLocator::quasiZero );
    00117
    00118 static inline double fmax(double a, double b)
    00119 {
    00120   return a > b ? a : b;
    00121 }
    00122
    00123
    00124
    00125 /*---------------------------- class GT2005StableSelfLocatorII methods ------------------------------*/
    00126
    00127
    00128 GT2005StableSelfLocator::GT2005StableSelfLocator(const SelfLocatorInterfaces& interfaces)
    00129 : SelfLocator(interfaces)
    00130 {
    00131   numberOfTypes = 0;
    00132   timeStamp = 0;
    00133   testSample = 0;
    00134   speed = 0;
    00135   Vector2<double> nullvec(0,0);
    00136   lastModelCrossing = nullvec;
    00137   lastSeenCrossing = nullvec;
    00138
    00139
    00140   for(int i = 0; i < SAMPLES_MAX; i++)
    00141     (Pose2D&) sampleSet[i] = field.randomPose();
    00142
    00143   timeStamp = 0;
    00144   sampleTemplateGenerator = GT2005StableSampleTemplateGenerator();
    00145   lineCrossingsTable = GT2005StableLineCrossingsTable();
    00146   odometryPose = field.randomPose();
    00147   odometryPoseResetted = false;
    00148
    00149   winningValidity = 0; secondaryValidity = 0;
    00150
    00151   OUTPUT(idText, text, "GT2005Stable SelfLocator initialized");
    00152
    00153   OUTPUT(idText,text,"line Z trust: " << linePointZAngleTrust);
    00154   OUTPUT(idText,text,"line Y trust: " << linePointYAngleTrust);
    00155   OUTPUT(idText,text,"crossings Z trust: " << crossingZAngleTrust);
    00156   OUTPUT(idText,text,"crossings Y trust: " << crossingYAngleTrust);
    00157   OUTPUT(idText,text,"flags Z trust: " << flagZAngleTrust);
    00158   OUTPUT(idText,text,"flags Y trust: " << flagYAngleTrust);
    00159   OUTPUT(idText,text,"goal Z trust: " << goalZAngleTrust);
    00160   OUTPUT(idText,text,"goal Y trust: " << goalYAngleTrust);
    00161
    00162   #ifdef LARGE_FIELD
    00163     OUTPUT(idText, text, "old field");
    00164   #endif
    00165 #ifdef HUGE_FIELD
    00166     OUTPUT(idText, text, "new field");
    00167 #endif
    00168 }
    00169
    00170
    00171 void GT2005StableSelfLocator::motionUpdate(const Pose2D& odometry, const Pose2D& camera, bool noise)
    00172 {
    00173   double transNoise = noise ? translationNoise : 0;
    00174   double rotNoise   = noise ? rotationNoise : 0;
    00175   double transX     = odometry.translation.x;
    00176   double transY     = odometry.translation.y;
    00177   double dist     = odometry.translation.abs();
    00178   double angle    = fabs(odometry.getAngle());
    00179
    00180   /* precalculate rotational error that has to be adapted to all samples */
    00181   double rotError   = fmax(dist*movedDistWeight,
    00182                            angle*movedAngleWeight);
    00183
    00184   /* precalculate translational error that has to be adapted to all samples */
    00185   double transXError = fmax( fabs(transX*majorDirTransWeight),
    00186                              fabs(transY*minorDirTransWeight));
    00187   double transYError = fmax( fabs(transY*majorDirTransWeight),
    00188                              fabs(transX*minorDirTransWeight));
    00189
    00190   double improbability;
    00191
    00192   /* for each sample:
    00193     -move by odometry
    00194     -apply motion dependent error
    00195     -apply probability dependent error
    00196     */
    00197   int i;
    00198   for (i = 0; i < SAMPLES_MAX; i++)
    00199   {
    00200     GT2005StableSelfLocatorSample& s = sampleSet[i];
    00201     s += odometry;
    00202
    00203     if (s.probability>1) improbability = 0;           // for uninitialized or faulty samples we can't calc noise
    00204     else improbability = (1-s.probability)*(1-s.probability);
    00205
    00206     // improbability = 1;
    00207
    00208     rotError = (random()*2-1) * fmax(rotError,rotNoise*improbability);
    00209
    00210     /* the translational error vector*/
    00211     double sampleTransXError = max(transXError,transNoise*improbability);
    00212     double sampleTransYError = max(transYError,transNoise*improbability);
    00213     Vector2<double> transErrorVec( (random()*2-1)*sampleTransXError,
    00214                                    (random()*2-1)*sampleTransYError );
    00215
    00216     /* update the sample */
    00217     s += Pose2D(rotError, transErrorVec);
    00218     s.camera = s + camera;
    00219   }
    00220
    00221 }
    00222
    00223
    00224 GT2005StableLineCrossingsTable::CrossingClass GT2005StableSelfLocator::getCrossingClassification(const LinesPercept::LineCrossingPoint& point)
    00225 {
    00226   if (point.outOfImage)
    00227     return GT2005StableLineCrossingsTable::unknownCrossing;
    00228
    00229
    00230   int numberOfWhite = (point.side1 == LinesPercept::lineOnThisSide ? 1 : 0) +
    00231                       (point.side2 == LinesPercept::lineOnThisSide ? 1 : 0) +
    00232                       (point.side3 == LinesPercept::lineOnThisSide ? 1 : 0) +
    00233                       (point.side4 == LinesPercept::lineOnThisSide ? 1 : 0);
    00234
    00235   int numberOfGreen = (point.side1 == LinesPercept::noLineOnThisSide ? 1 : 0) +
    00236                       (point.side2 == LinesPercept::noLineOnThisSide ? 1 : 0) +
    00237                       (point.side3 == LinesPercept::noLineOnThisSide ? 1 : 0) +
    00238                       (point.side4 == LinesPercept::noLineOnThisSide ? 1 : 0);
    00239
    00240   if (numberOfWhite == 2 && numberOfGreen == 2)
    00241   {
    00242     if ( (point.side1==LinesPercept::lineOnThisSide && point.side3==LinesPercept::lineOnThisSide &&
    00243           point.side2==LinesPercept::noLineOnThisSide && point.side4==LinesPercept::noLineOnThisSide) ||
    00244          (point.side1==LinesPercept::noLineOnThisSide && point.side3==LinesPercept::noLineOnThisSide &&
    00245           point.side2==LinesPercept::lineOnThisSide && point.side4==LinesPercept::lineOnThisSide) )
    00246       return GT2005StableLineCrossingsTable::virtualCrossing;
    00247     else
    00248       return GT2005StableLineCrossingsTable::lCrossing;
    00249   } else
    00250     if (numberOfWhite == 3)
    00251       return GT2005StableLineCrossingsTable::tCrossing;
    00252     else
    00253       return GT2005StableLineCrossingsTable::unknownCrossing;
    00254 }
    00255
    00256
    00257 // for the moment this is just inserted into the line crossings percept value
    00258 void GT2005StableSelfLocator::updateByCenterCircle(const LinesPercept::CenterCircle& centerCircle)
    00259 {
    00260   Vector2<int> point = centerCircle.location;
    00261   double pointDist = point.abs();
    00262
    00263          // seen angle to the linepoint in z-direction (height) assuming the robots height is paramHeight
    00264   double observedZAngle = atan2(headHeight,pointDist),
    00265          // seen angle to the linepoint in y-direction
    00266          observedYAngle = atan2((double)point.y,(double)point.x);
    00267
    00268
    00269   // pose describing where the linepoint is seen from the robot and under which angle
    00270   Pose2D point2(centerCircle.orientation,(double)point.x,(double)point.y);
    00271   Pose2D pointFromPose = robotPose + point2;
    00272
    00273   //debug stuff
    00274   lastSeenCenterCircle = pointFromPose;
    00275   // OUTPUT(idText,text,"seen angle: " << centerCircle.orientation << " field angle: " << pointFromPose.rotation);
    00276
    00277   // update all samples..
    00278   for(int i = 0; i < SAMPLES_MAX; ++i)
    00279   {
    00280     GT2005StableSelfLocatorSample& s = sampleSet[i];
    00281     // the absolute position of the point if it was seen from the samples position
    00282     Pose2D pointFromSample = s + point2;
    00283     Vector2<double> vMin(0,0);
    00284
    00285     Vector2<double> v = (Pose2D(vMin) - s).translation;
    00286
    00287     // the z and y directed angle derived from the closest point
    00288     double zModel = atan2(headHeight,v.abs()),
    00289            yModel = atan2(v.y,v.x);
    00290
    00291     double orientationDiff1 = fabs(fromDegrees(90) - pointFromSample.rotation);
    00292     double orientationDiff2 = fabs(fromDegrees(270) - pointFromSample.rotation);
    00293     double orientationDiff;
    00294
    00295     if (centerCircle.orientationKnown)
    00296     {
    00297       orientationDiff = orientationDiff1 < orientationDiff2 ? orientationDiff1 : orientationDiff2;
    00298     } else
    00299     {
    00300       orientationDiff = 0;
    00301     }
    00302
    00303     double val = sigmoid((zModel - observedZAngle) * centerCircleZAngleTrust) *
    00304                  sigmoid((yModel - observedYAngle) * centerCircleYAngleTrust) *
    00305                  sigmoid(orientationDiff * centerCircleOrientationAngleTrust);
    00306     //
    00307
    00308     s.setPerceptProbability(GT2005StableSelfLocatorSample::lineCrossing , val);
    00309   }
    00310 }
    00311
    00312 void GT2005StableSelfLocator::updateByCrossing(const LinesPercept::LineCrossingPoint& crossingPoint)
    00313 {
    00314   // COMPLEX_DRAWING(selfLocator,draw(point,type));
    00315   Vector2<int> point = crossingPoint.locationOnField;
    00316   double pointDist = point.abs();
    00317   GT2005StableLineCrossingsTable::CrossingClass crossingClass = getCrossingClassification(crossingPoint);
    00318
    00319          // seen angle to the linepoint in z-direction (height) assuming the robots height is paramHeight
    00320   double observedZAngle = atan2(headHeight,pointDist),
    00321          // seen angle to the linepoint in y-direction
    00322          observedYAngle = atan2((double)point.y,(double)point.x);
    00323
    00324
    00325   // pose describing where the linepoint is seen from the robot and under which angle
    00326   Pose2D point2(point);
    00327   Pose2D pointFromPose = robotPose + point2;
    00328
    00329   // crossing filtering
    00330   Vector2<double> testCrossing = lineCrossingsTable.getClassifiedClosestPoint(pointFromPose.translation,GT2005StableLineCrossingsTable::falseCrossing);
    00331   Vector2<double> testDiff = testCrossing - pointFromPose.translation;
    00332   if (testDiff.abs()<300)
    00333   {
    00334     CIRCLE(selfLocatorField,pointFromPose.translation.x,pointFromPose.translation.y,40,40,Drawings::ps_solid,Drawings::yellow);
    00335     return;
    00336   }
    00337
    00338   //debug stuff
    00339         lastSeenCrossing = pointFromPose.translation;
    00340         Vector2<double> vm = lineCrossingsTable.getClassifiedClosestPoint(pointFromPose.translation,crossingClass);
    00341         lastModelCrossing = vm;
    00342         lastSeenCrossingClass = crossingClass;
    00343
    00344   // update all samples..
    00345   for(int i = 0; i < SAMPLES_MAX; ++i)
    00346   {
    00347     GT2005StableSelfLocatorSample& s = sampleSet[i];
    00348     // the absolute position of the point if it was seen from the samples position
    00349     Pose2D pointFromSample = s + point2;
    00350     // stores the vector pointing to the nearest point of the given type
    00351     Vector2<double> vMin;
    00352     vMin = lineCrossingsTable.getClassifiedClosestPoint(pointFromSample.translation,crossingClass);
    00353
    00354     // if we got a closest point..
    00355     if(vMin.x != 1000000)
    00356     {
    00357       // Vector2<double> diff = vMin - pointFromSample.translation;
    00358       Vector2<double> v = (Pose2D(vMin) - s).translation;
    00359       // the z and y directed angle derived from the closest point
    00360       double zModel = atan2(headHeight,v.abs()),
    00361              yModel = atan2(v.y,v.x);
    00362
    00363       // testing this way this equals the similarity function of flags and goals
    00364       /*
    00365       double normedSimilarity = fabs((zModel - observedZAngle)*0.8) + fabs((yModel - observedYAngle)*0.2);
    00366       s.setPerceptProbability(isY ? (LinesPercept::LineType)GT2005StableSelfLocatorSample::perceptTypeYFieldLine : type,
    00367                               sigmoid(normedSimilarity*7));
    00368                               */
    00369
    00370       double val = sigmoid((zModel - observedZAngle) * crossingZAngleTrust) *
    00371                    sigmoid((yModel - observedYAngle) * crossingYAngleTrust);
    00372       // if (val>0.3) OUTPUT(idText,text,"crossings value" << val);
    00373       s.setPerceptProbability(GT2005StableSelfLocatorSample::lineCrossing , val);
    00374
    00375     }
    00376     else
    00377       s.setPerceptProbability(GT2005StableSelfLocatorSample::lineCrossing,0.000001);
    00378   }
    00379
    00380 }
    00381
    00382 // TODO: document, rewrite
    00383 void GT2005StableSelfLocator::updateByPoint(const LinesPercept::LinePoint& point,GT2005StableSelfLocatorSample::PerceptType type)
    00384 {
    00385   // COMPLEX_DRAWING(selfLocator,draw(point,type));
    00386
    00387   GT2005StableSelfLocatorSample::PerceptType index = type;
    00388
    00389   // as tables are built for the red team we have to swap indices for the goals
    00390   // if(teamColorBlue && (type >= LinesPercept::yellowGoal)
    00391   //   index = LinesPercept::yellowGoal + LinesPercept::skyblueGoal - type;
    00392   if (teamColorBlue)
    00393   {
    00394     if (type==GT2005StableSelfLocatorSample::skyblueGoal)
    00395       index = GT2005StableSelfLocatorSample::yellowGoal;
    00396     else if (type==GT2005StableSelfLocatorSample::yellowGoal)
    00397       index = GT2005StableSelfLocatorSample::skyblueGoal;
    00398   }
    00399
    00400   double pointDist = point.abs();
    00401
    00402          // seen angle to the linepoint in z-direction (height) assuming the robots height is paramHeight
    00403   double observedZAngle = atan2(headHeight,pointDist),
    00404          // seen angle to the linepoint in y-direction
    00405          observedYAngle = atan2((double)point.y,(double)point.x);
    00406
    00407
    00408   // pose describing where the linepoint is seen from the robot and under which angle
    00409   Pose2D point2(point.angle, Vector2<double>(point.x, point.y));
    00410
    00411   // update all samples..
    00412   for(int i = 0; i < SAMPLES_MAX; ++i)
    00413   {
    00414     GT2005StableSelfLocatorSample& s = sampleSet[i];
    00415     // the absolute position of the point if it was seen from the samples position
    00416     Pose2D pointFromSample = s + point2;
    00417
    00418
    00419     // stores the vector pointing to the nearest point of the given type
    00420     Vector2<double> vMin;
    00421
    00422     // is this a line running in y direction?
    00423     bool isY = (index == GT2005StableSelfLocatorSample::xFieldLine || index==GT2005StableSelfLocatorSample::yFieldLine) &&
    00424               (fabs(pointFromSample.getAngle()) < pi / 4 ||
    00425                fabs(pointFromSample.getAngle()) > 3 * pi / 4);
    00426     if(isY)
    00427       // y-directed lines are stored here
    00428       vMin = observationTable[LinesPercept::numberOfLineTypes].getClosestPoint(pointFromSample.translation);
    00429     else
    00430       // else we find the closest point in the corresponding observationTable
    00431       switch(index)
    00432       {
    00433         case GT2005StableSelfLocatorSample::xFieldLine:
    00434         case GT2005StableSelfLocatorSample::yFieldLine:
    00435           vMin = observationTable[LinesPercept::field].getClosestPoint(pointFromSample.translation);
    00436           break;
    00437         //case GT2005StableSelfLocatorSample::border:
    00438         //  vMin = observationTable[LinesPercept::border].getClosestPoint(pointFromSample.translation);
    00439         //  break;
    00440         case GT2005StableSelfLocatorSample::yellowGoal:
    00441           vMin = observationTable[LinesPercept::yellowGoal].getClosestPoint(pointFromSample.translation);
    00442           break;
    00443         case GT2005StableSelfLocatorSample::skyblueGoal:
    00444           vMin = observationTable[LinesPercept::skyblueGoal].getClosestPoint(pointFromSample.translation);
    00445           break;
    00446         case GT2005StableSelfLocatorSample::lineCrossing:
    00447           OUTPUT(idText,text,"please use updateByCrossing for line crossings");
    00448           return; // vMin = lineCrossingsTable.getClosestPoint(pointFromSample.translation,GT2005StableLineCrossingsTable::lineCrossing);
    00449           break;
    00450       }
    00451
    00452     // if we got a closest point..
    00453     if(vMin.x != 1000000)
    00454     {
    00455       // Vector2<double> diff = vMin - pointFromSample.translation;
    00456       Vector2<double> v = (Pose2D(vMin) - s).translation;
    00457       // the z and y directed angle derived from the closest point
    00458       double zModel = atan2(headHeight,v.abs()),
    00459              yModel = atan2(v.y,v.x);
    00460
    00461       // testing this way this equals the similarity function of flags and goals
    00462       /*
    00463       double normedSimilarity = fabs((zModel - observedZAngle)*0.8) + fabs((yModel - observedYAngle)*0.2);
    00464       s.setPerceptProbability(isY ? (LinesPercept::LineType)GT2005StableSelfLocatorSample::perceptTypeYFieldLine : type,
    00465                               sigmoid(normedSimilarity*7));
    00466                               */
    00467
    00468
    00469       s.setPerceptProbability(isY ? GT2005StableSelfLocatorSample::yFieldLine : type,
    00470                               sigmoid((zModel - observedZAngle) * linePointZAngleTrust) *
    00471                               sigmoid((yModel - observedYAngle) * linePointYAngleTrust));
    00472
    00473     }
    00474     else
    00475       s.setPerceptProbability(type,0.000001);
    00476   }
    00477 }
    00478
    00479
    00480 void GT2005StableSelfLocator::lineObservationUpdate(const LinesPercept& linesPercept)
    00481 {
    00482     const int maxPercepts = 5; // maximum number of percepts used per linetype
    00483     int i;
    00484
    00485     // updating by center circle
    00486     /*
    00487     if (linesPercept.centerCircleFound){
    00488       updateByCenterCircle(linesPercept.centerCircle);
    00489     }
    00490     */
    00491
    00492     // debug drawings
    00493     CIRCLE(selfLocatorField,lastSeenCenterCircle.translation.x,lastSeenCenterCircle.translation.y,60,30,Drawings::ps_solid,Drawings::white);
    00494     COMPLEX_DRAWING(selfLocatorField,draw(lastSeenCenterCircle,Drawings::white));
    00495
    00496     int color = Drawings::black;
    00497     switch(lastSeenCrossingClass)
    00498     {
    00499       case GT2005StableLineCrossingsTable::unknownCrossing:
    00500         color = Drawings::black;
    00501         break;
    00502       case GT2005StableLineCrossingsTable::virtualCrossing:
    00503         color = Drawings::blue;
    00504         break;
    00505       case GT2005StableLineCrossingsTable::lCrossing:
    00506         color = Drawings::yellow;
    00507         break;
    00508       case GT2005StableLineCrossingsTable::tCrossing:
    00509         color = Drawings::red;
    00510         break;
    00511     }
    00512     CIRCLE(selfLocatorField,lastModelCrossing.x,lastModelCrossing.y,40,40,Drawings::ps_solid,color);
    00513     CIRCLE(selfLocatorField,lastSeenCrossing.x,lastSeenCrossing.y,30,30,Drawings::ps_solid,Drawings::red);
    00514     LINE(selfLocatorField,lastSeenCrossing.x,lastSeenCrossing.y,lastModelCrossing.x,lastModelCrossing.y,10,Drawings::ps_solid,Drawings::blue);
    00515
    00516
    00517     // this wil be replaced by a nicer solution in the LinesPercept
    00518     // OUTPUT(idText,text,"crossings " << linesPercept.numberOfLineCrossings);
    00519     for(i=0;i<linesPercept.numberOfLineCrossings && i<3;i++)
    00520     {
    00521       updateByCrossing(linesPercept.lineCrossings[i]);
    00522       observationUpdateDone = true;
    00523     }
    00524
    00525
    00526     ///COPIED FROM UNSTABLE VERSION!!!///
    00527     for(i = 0; i < 2; ++i)
    00528       for(int j = 0; j < linesPercept.points[i].numberOfPoints && j < maxPercepts; ++j)
    00529       {
    00530         switch(i)
    00531         {
    00532           case LinesPercept::field:
    00533             updateByPoint(linesPercept.points[i].pointsOnField[rand() % linesPercept.points[i].numberOfPoints],GT2005StableSelfLocatorSample::xFieldLine);
    00534             break;
    00535           //case LinesPercept::border:
    00536           //  updateByPoint(linesPercept.points[i].pointsOnField[rand() % linesPercept.points[i].numberOfPoints],GT2005StableSelfLocatorSample::border);
    00537           //  break;
    00538
    00539         }
    00540         observationUpdateDone = true;
    00541       }
    00542
    00543
    00544     //for(i = 0; i < 2; ++i)
    00545     //  for(int j = 0; j < linesPercept.numberOfPoints[i] && j < maxPercepts; ++j)
    00546     //  {
    00547     //    switch(i)
    00548     //    {
    00549     //      case LinesPercept::field:
    00550     //        updateByPoint(linesPercept.points[i][rand() % linesPercept.numberOfPoints[i]],GT2005StableSelfLocatorSample::xFieldLine);
    00551     //        break;
    00552     //      case LinesPercept::border:
    00553     //        updateByPoint(linesPercept.points[i][rand() % linesPercept.numberOfPoints[i]],GT2005StableSelfLocatorSample::border);
    00554     //        break;
    00555
    00556     //    }
    00557     //    observationUpdateDone = true;
    00558     //  }
    00559
    00560       // for template generation - will find another solution
    00561     //if(linesPercept.numberOfPoints[LinesPercept::yellowGoal])
    00562     //  types[numberOfTypes++] = LinesPercept::yellowGoal;
    00563     //if(linesPercept.numberOfPoints[LinesPercept::skyblueGoal])
    00564     //  types[numberOfTypes++] = LinesPercept::skyblueGoal;
    00565     if(linesPercept.points[LinesPercept::yellowGoal].numberOfPoints)
    00566       types[numberOfTypes++] = LinesPercept::yellowGoal;
    00567     if(linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints)
    00568       types[numberOfTypes++] = LinesPercept::skyblueGoal;
    00569
    00570
    00571     if(linesPercept.points[LinesPercept::yellowGoal].numberOfPoints ||
    00572       linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints)
    00573     //if(linesPercept.numberOfPoints[LinesPercept::yellowGoal] ||
    00574     //   linesPercept.numberOfPoints[LinesPercept::skyblueGoal])
    00575       for(int j = 0; j < linesPercept.points[LinesPercept::yellowGoal].numberOfPoints +
    00576                          linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints && j < maxPercepts; ++j)
    00577       //for(int j = 0; j < linesPercept.numberOfPoints[LinesPercept::yellowGoal] +
    00578       //                   linesPercept.numberOfPoints[LinesPercept::skyblueGoal] && j < maxPercepts; ++j)
    00579       { // This works if only one goal or both were seen
    00580         LinesPercept::LineType select = (random() + 1e-6) * linesPercept.points[LinesPercept::yellowGoal].numberOfPoints >
    00581                                         linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints
    00582                                         ? LinesPercept::yellowGoal
    00583                                         : LinesPercept::skyblueGoal;
    00584
    00585         switch(select)
    00586         {
    00587           case LinesPercept::yellowGoal:
    00588             updateByPoint(linesPercept.points[select].pointsOnField[rand() % linesPercept.points[select].numberOfPoints],GT2005StableSelfLocatorSample::yellowGoal);
    00589             break;
    00590           case LinesPercept::skyblueGoal:
    00591             updateByPoint(linesPercept.points[select].pointsOnField[rand() % linesPercept.points[select].numberOfPoints],GT2005StableSelfLocatorSample::skyblueGoal);
    00592             break;
    00593         }
    00594
    00595
    00596         //LinesPercept::LineType select = (random() + 1e-6) * linesPercept.numberOfPoints[LinesPercept::yellowGoal] >
    00597         //                                linesPercept.numberOfPoints[LinesPercept::skyblueGoal]
    00598         //                                ? LinesPercept::yellowGoal
    00599         //                                : LinesPercept::skyblueGoal;
    00600
    00601         //switch(select)
    00602         //{
    00603         //  case LinesPercept::yellowGoal:
    00604         //    updateByPoint(linesPercept.points[select][rand() % linesPercept.numberOfPoints[select]],GT2005StableSelfLocatorSample::yellowGoal);
    00605         //    break;
    00606         //  case LinesPercept::skyblueGoal:
    00607         //    updateByPoint(linesPercept.points[select][rand() % linesPercept.numberOfPoints[select]],GT2005StableSelfLocatorSample::skyblueGoal);
    00608         //    break;
    00609         //}
    00610         observationUpdateDone = true;
    00611       }
    00612 }
    00613
    00614
    00615
    00616 void GT2005StableSelfLocator::updateByNotSeenFlag(Flag::FlagType type){
    00617   switch(type)
    00618   {
    00619     case Flag::pinkAboveSkyblue:
    00620       break;
    00621     case Flag::pinkAboveYellow:
    00622       break;
    00623     case Flag::skyblueAbovePink:
    00624       break;
    00625     case Flag::yellowAbovePink:
    00626       break;
    00627   }
    00628 }
    00629
    00630 //void GT2005StableSelfLocator::updateByNotSeenGoal(colorClass goalColor){
    00631 //  Vector2<int> limit1(0,0);
    00632 //  Vector2<int> limit2(0,0);
    00633 //
    00634 //  // this is implemented for the blue team
    00635 //  if (!teamColorBlue)
    00636 //  {
    00637 //    goalColor = goalColor==skyblue ? yellow : skyblue;
    00638 //  }
    00639 //
    00640 //  switch(goalColor)
    00641 //  {
    00642 //    case skyblue:
    00643 //      limit1.x = xPosOwnGoalpost;
    00644 //      limit2.x = xPosOwnGoalpost;
    00645 //      limit1.y = yPosLeftGoal;
    00646 //      limit2.y = yPosRightGoal;
    00647 //      break;
    00648 //    case yellow:
    00649 //      limit1.x = xPosOpponentGoalpost;
    00650 //      limit2.x = xPosOpponentGoalpost;
    00651 //      limit1.y = yPosLeftGoal;
    00652 //      limit2.y = yPosRightGoal;
    00653 //      break;
    00654 //    default:
    00655 //      return;
    00656 //  }
    00657 //
    00658 //  int i;
    00659 //  Vector2<int> imagePos1, imagePos2;
    00660 //  for(i=0;i<SAMPLES_MAX;i++)
    00661 //  {
    00662 //    GT2005StableSelfLocatorSample& s = sampleSet[i];
    00663 //
    00664 //    Pose2D limit1FromSample(limit1.x,limit1.y);
    00665 //    limit1FromSample -= s.camera;
    00666 //
    00667 //    Pose2D limit2FromSample(limit2.x,limit2.y);
    00668 //    limit2FromSample -= s.camera;
    00669 //
    00670 //    Vector2<int> limit1Pos((int)limit1FromSample.translation.x,(int)limit1FromSample.translation.y);
    00671 //    Vector2<int> limit2Pos((int)limit2FromSample.translation.x,(int)limit2FromSample.translation.y);
    00672 //
    00673 //    if ((limit1Pos.abs()<2000 || limit2Pos.abs()<2000) )
    00674 //    {
    00675 //      Geometry::calculatePointInImage(limit1Pos,cameraMatrix,cameraInfo,imagePos1);
    00676 //      Geometry::calculatePointInImage(limit2Pos,cameraMatrix,cameraInfo,imagePos2);
    00677 //
    00678 //      // scan line between imagePos1 and imagePos2 for obstacles
    00679 //
    00680 //      // not seen goal at least partially inside virtual image?
    00681 //      if (imagePos1.x>0 && imagePos2.x>0 &&
    00682 //          imagePos1.x<cameraInfo.resolutionWidth && imagePos2.x<cameraInfo.resolutionWidth &&
    00683 //          imagePos1.y>0 && imagePos2.y>0 &&
    00684 //          imagePos1.y<cameraInfo.resolutionHeight && imagePos2.y<cameraInfo.resolutionHeight)
    00685 //      {
    00686 //        // decrease probability
    00687 //        // hack: only one goal is regarded so we dont have to care about which goal to take
    00688 //        CIRCLE(selfLocatorField,s.translation.x,s.translation.y,40,10,Drawings::ps_solid,Drawings::yellow);
    00689 //        s.setPerceptProbability(GT2005StableSelfLocatorSample::skyblueGoal,0.5);
    00690 //      }
    00691 //    }
    00692 //  }
    00693 //
    00694 //}
    00695
    00696 // TODO: maybe use vertical angles too?
    00697 // make similarity factor configurable
    00698 void GT2005StableSelfLocator::updateByFlag(const Vector2<double>& flagFieldPosition,
    00699                                      FlagSides sideOfFlag,
    00700                                      double measuredBearing)
    00701 {
    00702   observationUpdateDone = true;
    00703   int i;
    00704   for(i = 0; i < SAMPLES_MAX; ++i)
    00705   {
    00706     GT2005StableSelfLocatorSample& s = sampleSet[i];
    00707
    00708      // flagPos: position of the flag relative to sample s
    00709     Pose2D flagPos(flagFieldPosition.x,flagFieldPosition.y);
    00710     flagPos -= s.camera;
    00711
    00712      // horizontal angle between flag and camera coordinate system
    00713     double assumedBearing = atan2(flagPos.translation.y,flagPos.translation.x) + sideOfFlag * asin(flagRadius / flagPos.translation.abs());
    00714
    00715      // similarity in numbers of pi - handy as we need a similarity between 0 and 1
    00716     double similarity = fabs((assumedBearing - measuredBearing)) / pi;
    00717
    00718     // use symmetry for full circle
    00719     if(similarity > 1)
    00720       similarity = 2.0 - similarity;
    00721
    00722     // now we have a similarity measure between 0..1
    00723     // make similarity * 7 configurable...
    00724     if (similarity>0.01){
    00725       // COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::red));
    00726       if (testSample == &s){
    00727         COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::blue));
    00728       }
    00729     }
    00730     s.setPerceptProbability(GT2005StableSelfLocatorSample::flag,sigmoid(similarity * flagYAngleTrust));
    00731   }
    00732 }
    00733
    00734
    00735 // look at updateByFlag
    00736 // goalPost is a Vector pointing on the used goalpost
    00737 // TODO: maybe introduce new PerceptType GoalPost instead of using "Flag"
    00738 void GT2005StableSelfLocator::updateByGoalPost(const Vector2<double>& goalPost,
    00739                                          double measuredBearing)
    00740 {
    00741   observationUpdateDone = true;
    00742   int i;
    00743   for(i = 0; i < SAMPLES_MAX; ++i)
    00744   {
    00745     GT2005StableSelfLocatorSample& s = sampleSet[i];
    00746     Pose2D p(goalPost.x,goalPost.y);
    00747     p -= s.camera;
    00748     double assumedBearing = atan2(p.translation.y,p.translation.x);
    00749     double similarity = fabs((assumedBearing - measuredBearing)) / pi;
    00750     if(similarity > 1)
    00751       similarity = 2 - similarity;
    00752
    00753   // cast to LineType is needed for compatibility with the rest of GT04, maybe change
    00754   // method parameters later
    00755     s.setPerceptProbability(GT2005StableSelfLocatorSample::flag,sigmoid(similarity * goalYAngleTrust));
    00756   }
    00757 }
    00758
    00759
    00760
    00761 /*
    00762 *  update observation information by all flags and goalposts seen
    00763 */
    00764 void GT2005StableSelfLocator::landmarksObservationUpdate(const LandmarksPercept& landmarksPercept)
    00765 {
    00766    int i;
    00767    for(i = 0; i < landmarksPercept.numberOfFlags; i++)
    00768     {
    00769       // DEBUG_RESPONSE("selfLocator:showObservationUpdateDetails", OUTPUT(idText, text, "SLou: flag found"));
    00770       const Flag& flag = landmarksPercept.flags[i];
    00771
    00772     /* if the leftmost position of the flag is not on the robots image border.. */
    00773       if(!flag.isOnBorder(flag.x.max))
    00774         updateByFlag(flag.position,
    00775                      LEFT_SIDE_OF_FLAG,
    00776                      flag.x.max);
    00777     /* if the rightmost position of the flag is not on the robots image border.. */
    00778       if(!flag.isOnBorder(flag.x.min))
    00779         updateByFlag(flag.position,
    00780                      RIGHT_SIDE_OF_FLAG,
    00781                      flag.x.min);
    00782
    00783     /* for flag on border no update is done! */
    00784     }
    00785
    00786     /* same thing for goals */
    00787     bool skyblueGoalSeen = false;
    00788     bool yellowGoalSeen = false;
    00789     for (i = 0; i < landmarksPercept.numberOfGoals; i++)
    00790     {
    00791       // DEBUG_RESPONSE("selfLocator:showObservationUpdateDetails", OUTPUT(idText, text, "SLou: goal found"));
    00792       const Goal& goal = landmarksPercept.goals[i];
    00793       if (goal.color == skyblue)
    00794         skyblueGoalSeen = true;
    00795       else if (goal.color == yellow)
    00796         yellowGoalSeen = true;
    00797
    00798       if(!goal.isOnBorder(goal.x.max))
    00799         updateByGoalPost(goal.leftPost,
    00800                          goal.x.max);
    00801       if(!goal.isOnBorder(goal.x.min))
    00802         updateByGoalPost(goal.rightPost,
    00803                          goal.x.min);
    00804       /*
    00805       if(!goal.isOnBorder(goal.x.min) && !goal.isOnBorder(goal.x.max))
    00806         OUTPUT(idText,text,"distance: " << goal.distance << "angle: " << goal.angle << "rot: " << goal.rotation);
    00807         */
    00808     }
    00809
    00810
    00811     // update by not seen goals here..
    00812     /*
    00813     if (!skyblueGoalSeen)
    00814       updateByNotSeenGoal(skyblue);
    00815     if (!yellowGoalSeen)
    00816       updateByNotSeenGoal(yellow);
    00817       */
    00818 }
    00819
    00820
    00821
    00822 void GT2005StableSelfLocator::updateVariancesBySpeed(double speed)
    00823 {
    00824   linePointZAngleMotionDependentVariance = linePointZAngleVariance * (1 + speed * linePointZAngleMotionDependency);
    00825   linePointZAngleTrust = linePointWeight / (linePointZAngleMotionDependentVariance + quasiZero );
    00826 }
    00827
    00828 void GT2005StableSelfLocator::execute()
    00829 {
    00830   teamColorBlue = (getPlayer().getTeamColor() == Player::blue);
    00831
    00832 /*
    00833   headHeightBuffer.add(cameraMatrix.translation.z);
    00834   headHeight = headHeightBuffer.getNumberOfEntries()>0 ?
    00835   headHeightBuffer.getSum() / headHeightBuffer.getNumberOfEntries() : headHeight;
    00836   */
    00837   // OUTPUT(idText,text,paramHeadHeight);
    00838   // OUTPUT(idText,text,sampleSet.getNumberOfSamples());
    00839
    00840   robotPose.setFrameNumber(landmarksPercept.frameNumber);
    00841   robotPose.setTimestamp(SystemCall::getCurrentSystemTime());
    00842
    00843   //distanceToBorderEstimator.execute();
    00844
    00845   Pose2D odometryDelta = odometryData - lastOdometry;
    00846   lastOdometry = odometryData;
    00847
    00848   // this is about to go to the motion model
    00849   if(SystemCall::getTimeSince(timeStamp) > 500)
    00850   {
    00851     speed = (odometryData - lastOdometry2).translation.abs() / SystemCall::getTimeSince(timeStamp) * 1000.0;
    00852     lastOdometry2 = odometryData;
    00853     timeStamp = SystemCall::getCurrentSystemTime();
    00854   }
    00855
    00856   // updateVariancesBySpeed(speed);
    00857
    00858   // motion updating
    00859   motionUpdate(odometryDelta,Pose2D(landmarksPercept.cameraOffset.x,
    00860                landmarksPercept.cameraOffset.y),
    00861                observationUpdateDone);
    00862
    00863   observationUpdateDone = false;
    00864   numberOfTypes=0;
    00865
    00866   // observation updating
    00867   if (cameraMatrix.isValid)
    00868   {
    00869     lineObservationUpdate(linesPercept);
    00870     landmarksObservationUpdate(landmarksPercept);
    00871
    00872     if (observationUpdateDone)
    00873     {
    00874       sampleTemplateGenerator.generateTemplates(landmarksPercept,linesPercept,odometryDelta,numberOfTypes,types);
    00875       resample();
    00876     }
    00877   }
    00878   // taken from gt04..
    00879
    00880   Pose2D pose;
    00881   double validity;
    00882   calcPose(pose,validity);
    00883   // robotPose.setPose(pose);
    00884   // robotPose.setValidity(validity);
    00885
    00886   // OUTPUT(idText,text,validity);
    00887
    00888   odometryPoseResetted = false;
    00889   robotPose.setPose(pose);
    00890   robotPose.setValidity(validity);
    00891
    00892   sampleSet.link(selfLocatorSamples);
    00893
    00894
    00895   DEBUG_DRAWING_FINISHED(selfLocatorField);
    00896   DEBUG_DRAWING_FINISHED(selfLocator);
    00897   landmarksState.update(landmarksPercept);
    00898 }
    00899
    00900
    00901 // contains the part of the gt04 resampling method that calculates
    00902 // the average or each percept type probability and all samples
    00903 // averagePerceptTypeProb[i] = average for all samples if there are any, 0.5 else
    00904 // returns the product over all average percepttype probabilities
    00905
    00906 double GT2005StableSelfLocator::calcAveragePerceptTypeProbabilities()
    00907 {
    00908   int count[GT2005StableSelfLocatorSample::numberOfPerceptTypes]; // number of samples per perceptType
    00909   double probability = 1;                 //probability of all average possibilities = a1*a2*a3...
    00910
    00911   // initialize count and averagePerceptTypeProb
    00912   for (int k=0; k<GT2005StableSelfLocatorSample::numberOfPerceptTypes; k++){
    00913     count[k] = 0;
    00914     averagePerceptTypeProb[k] = 0;
    00915   }
    00916
    00917   int i;
    00918   for (i=0;i<SAMPLES_MAX;i++)
    00919   {
    00920     GT2005StableSelfLocatorSample& sample = sampleSet[i];
    00921     for (int k=0; k<GT2005StableSelfLocatorSample::numberOfPerceptTypes; k++){
    00922       if(sample.perceptProbabilities[k]<=1)
    00923       {
    00924         count[k]++;
    00925         averagePerceptTypeProb[k] += sample.perceptProbabilities[k];
    00926       }
    00927     }
    00928   }
    00929
    00930   for (i=0;i<GT2005StableSelfLocatorSample::numberOfPerceptTypes;i++)
    00931   {
    00932     if (count[i]>0) averagePerceptTypeProb[i] /= count[i];
    00933     else averagePerceptTypeProb[i] = 0.5;
    00934     // OUTPUT(idText,text,"avg type "<< i << "  "<<averagePerceptTypeProb[i]);
    00935     if (/*i!=GT2005StableSelfLocatorSample::lineCrossing &&*/ i!=GT2005StableSelfLocatorSample::skyblueGoal) probability *= averagePerceptTypeProb[i];
    00936   }
    00937   return probability;
    00938 }
    00939
    00940
    00941 /**
    00942 * redistibution of the sampleSet according to the probabilities of the samples
    00943 * reimplementation of the gt04 resampling method.
    00944 * TODO: template generation
    00945 */
    00946 void GT2005StableSelfLocator::resample()
    00947 {
    00948   // this fills averagePerceptTypeProb[] and returns the whole averageProbability
    00949   double averageProbability =
    00950     calcAveragePerceptTypeProbabilities();
    00951   // OUTPUT(idText,text,averageProbability);
    00952
    00953
    00954   // calculate probability for all samples from their perceptType probabilities
    00955   int i;
    00956   for(i = 0; i < SAMPLES_MAX; ++i){
    00957     sampleSet[i].updateProbability(averagePerceptTypeProb);
    00958     // sampleSet[i].updateProbability();
    00959   }
    00960
    00961   // if (testSample!=0) OUTPUT(idText,text,"last Samples prob: "<< testSample->probability << " average = " << averageProbability);
    00962   // swap sample arrays
    00963   GT2005StableSelfLocatorSample* oldSet = sampleSet.swap();
    00964
    00965   /* calculate the sum over the probabilities of all samples
    00966    to determine the number of samples per 1.0 probability (prob2Index)
    00967    this is needed to calculate how often a sample is multiplied
    00968    -> a sample with probability 1.0 would be inserted prob2Index times
    00969    into the new distribution  */
    00970    double probSum = 0;
    00971    for(i = 0; i < SAMPLES_MAX; ++i)
    00972      probSum += oldSet[i].getProbability();
    00973   double prob2Index = (double) SAMPLES_MAX / probSum;
    00974
    00975   double indexSum = 0;  // number of indices we covered in the new
    00976                         // sample set.
    00977   int j = 0;
    00978
    00979   // here starts resampling step, we copy the samples according
    00980   // to their probability, keeping the probabilities within
    00981   // narrow bounds
    00982   for(i = 0; i < SAMPLES_MAX; ++i)
    00983   {
    00984     // calculate how far we will proceed in the new sampleset with this sample
    00985     indexSum += oldSet[i].getProbability() * prob2Index;
    00986     while(j < SAMPLES_MAX && j < indexSum - 0.5)
    00987       sampleSet[j++] = oldSet[i];
    00988   }
    00989
    00990   // finally determine which samples we will replace randomly by
    00991   // template samples
    00992   for(i = 0; i < SAMPLES_MAX; i++)
    00993   {
    00994     GT2005StableSelfLocatorSample& s = sampleSet[i];
    00995     // if quality is too low, select a new random pose for the sample
    00996     /* template generation not yet implemented*/
    00997     // OUTPUT(idText,text,"number of templates" << sampleTemplateGenerator.getNumberOfTemplates());
    00998     if(s.getProbability()<=1 && sampleTemplateGenerator.getNumberOfTemplates() && random() * averageProbability > s.getProbability())
    00999     {
    01000       s = sampleTemplateGenerator.getTemplate();
    01001       testSample = &s;
    01002       COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::yellow));
    01003     }
    01004
    01005   /*
    01006     if(random() * averageProbability > s.getProbability()){
    01007       s = GT2005StableSelfLocatorSample(field.randomPose());
    01008       COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::red));
    01009     }*/
    01010     else
    01011     {
    01012       COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::black));
    01013     }
    01014   }
    01015 }
    01016
    01017
    01018
    01019 double GT2005StableSelfLocator::calcDistributionValidity()
    01020 {
    01021   double varianceCovarianceMatrix[3][3];
    01022   double averageXDeviation = 0;
    01023   double averageYDeviation = 0;
    01024   double averageRotDeviation = 0;
    01025
    01026   int i;
    01027   for (i=0;i<SAMPLES_MAX;i++)
    01028   {
    01029     averageXDeviation += fabs( sampleSet[i].translation.x - robotPose.translation.x);
    01030     averageYDeviation += fabs( sampleSet[i].translation.y - robotPose.translation.y);
    01031     double rotDiff = fabs(sampleSet[i].rotation - robotPose.rotation);
    01032     if (rotDiff > pi)
    01033       rotDiff -= pi;
    01034     averageRotDeviation += fabs(rotDiff);
    01035   }
    01036
    01037   averageXDeviation /= SAMPLES_MAX;
    01038   averageYDeviation /= SAMPLES_MAX;
    01039   averageRotDeviation /= SAMPLES_MAX;
    01040
    01041   varianceCovarianceMatrix[0][0] = averageXDeviation * averageXDeviation;
    01042   varianceCovarianceMatrix[1][1] = averageYDeviation * averageYDeviation;
    01043   varianceCovarianceMatrix[2][2] = averageRotDeviation * averageRotDeviation;
    01044
    01045   varianceCovarianceMatrix[0][1] = averageXDeviation * averageYDeviation;
    01046   varianceCovarianceMatrix[0][2] = averageXDeviation * averageRotDeviation;
    01047
    01048   varianceCovarianceMatrix[1][0] = varianceCovarianceMatrix[0][1];
    01049   varianceCovarianceMatrix[1][2] = averageYDeviation * averageRotDeviation;
    01050
    01051   varianceCovarianceMatrix[2][0] = varianceCovarianceMatrix[0][2];
    01052   varianceCovarianceMatrix[2][1] = varianceCovarianceMatrix[1][2];
    01053
    01054   // for now, dont use the covariances
    01055   // OUTPUT(idText,text,"....>" << sigmoid(sqrt(varianceCovarianceMatrix[0][0] + varianceCovarianceMatrix[1][1] + varianceCovarianceMatrix[2][2])/300 ));
    01056   // return sigmoid( (varianceCovarianceMatrix[0][0] + varianceCovarianceMatrix[1][1] + varianceCovarianceMatrix[2][2]) / (3*300));
    01057   return sigmoid( sqrt(varianceCovarianceMatrix[0][0] + varianceCovarianceMatrix[1][1] + varianceCovarianceMatrix[2][2]) / 200);
    01058 }
    01059
    01060
    01061 RobotPose GT2005StableSelfLocator::calcPoseFromSubCube(Cell poseSpace[POSE_SPACE_GRID][POSE_SPACE_GRID][POSE_SPACE_GRID], int xMax, int yMax, int rMax)
    01062 {
    01063     // determine the average pose of all samples in the winner sub-cube
    01064     double xSum = 0,
    01065            ySum = 0,
    01066            cosSum = 0,
    01067            sinSum = 0,
    01068            probabilitySum = 0;
    01069
    01070     RobotPose result;
    01071
    01072   // iterate over the rotation axis of the sub-cube
    01073     for(int r = 0; r < 2; r++)
    01074     {
    01075     // just as before: r axis is cyclic
    01076       int rNext = (rMax + r) % POSE_SPACE_GRID;
    01077
    01078       // iterate over the traslation axes...
    01079       for(int y = 0; y < 2; y++)
    01080         for(int x = 0; x < 2; x++)
    01081         {
    01082           // iterate over the list of samples inside the cell
    01083           // cell.first is 0 if the queue is empty
    01084           // else p->next is 0 at the end of the queue
    01085           for(GT2005StableSelfLocatorSample* sample = poseSpace[rNext][yMax + y][xMax + x].first; sample; sample = sample->next)
    01086             if(sample->isValid())
    01087             {
    01088               // sum the x and y positions of the samples weighted by probability
    01089               // sum the sine and cosine of the samples rotation weighted by probability
    01090               // calculate the whole sum of probabilities to calculate the weighted averages
    01091               xSum += sample->translation.x * sample->getProbability();
    01092               ySum += sample->translation.y * sample->getProbability();
    01093               cosSum += sample->getCos() * sample->getProbability();
    01094               sinSum += sample->getSin() * sample->getProbability();
    01095               probabilitySum += sample->getProbability();
    01096             }
    01097         }
    01098     }
    01099
    01100   // now calculate the pose as the weighted averages of x,y,r
    01101     if(probabilitySum>0)
    01102     {
    01103     double weightedX = xSum / probabilitySum;
    01104     double weightedY = ySum / probabilitySum;
    01105     double weightedRot = atan2(sinSum,cosSum);
    01106       result.setPose(Pose2D(weightedRot,Vector2<double>(weightedX,weightedY)) );
    01107       result.setValidity(probabilitySum / SAMPLES_MAX);
    01108     // clip the pose to the field
    01109       double diff = field.clip(result.translation);
    01110       if(diff > 1){
    01111         // double vold = validity;
    01112         result.setValidity(result.getValidity() / sqrt(diff));  // gt04: Why do we use the sqrt?
    01113       }
    01114     }
    01115
    01116     return result;
    01117 }
    01118
    01119 /**
    01120 * The function determines the most probable pose from the sample distribution.
    01121 * reimplementation of the gt04 calcPose method
    01122 * @param pose The pose is returned to this variable.
    01123 * @param validity The validity of the pose is returned to this variable.
    01124 */
    01125 void GT2005StableSelfLocator::calcPose(Pose2D& pose,double& validity)
    01126 {
    01127   // the 3d pose-space grid
    01128   Cell poseSpace[POSE_SPACE_GRID][POSE_SPACE_GRID][POSE_SPACE_GRID];
    01129
    01130   double width = field.x.getSize(),
    01131          height = field.y.getSize();
    01132
    01133   int i;
    01134   // attach the samples to the corresponding cells of the poseSpace
    01135   for(i = 0; i < SAMPLES_MAX; i++)
    01136   {
    01137     GT2005StableSelfLocatorSample& sample = sampleSet[i];
    01138     // COMPLEX_DRAWING(selfLocatorField,draw(sample,Drawings::black));
    01139
    01140     if(sample.isValid())
    01141     {
    01142     // r: Index on the rotation axis of the cube. as angles are between
    01143     //    [-pi..0] [0..pi] we put the 0 of the rotation axis on 0.5*POSE_SPACE_GRID
    01144     //    r = [(angle / 2*pi) + 0.5] * POSE_SPACE_GRID
    01145       int r = static_cast<int>((sample.getAngle() / pi2 + 0.5) * POSE_SPACE_GRID);
    01146     //  x,y: Index on the translation axes. similar to the rotation, the 0 position
    01147     //       is at 0.5*POSE_SPACE_GRID.
    01148     int y = static_cast<int>((sample.translation.y / height + 0.5) * POSE_SPACE_GRID);
    01149       int x = static_cast<int>((sample.translation.x / width + 0.5) * POSE_SPACE_GRID);
    01150
    01151     // translation axes are non-cyclic, so we set [x|y] < 0 to 0 and
    01152     // [x|y] >= POSE_SPACE_GRID to POSE_SPACE_GRID-1
    01153       if(x < 0)
    01154         x = 0;
    01155       else if(x >= POSE_SPACE_GRID)
    01156         x = POSE_SPACE_GRID-1;
    01157
    01158       if(y < 0)
    01159         y = 0;
    01160       else if(y >= POSE_SPACE_GRID)
    01161         y = POSE_SPACE_GRID-1;
    01162
    01163     // the rotation axis is cyclic so if we run out of the interval at
    01164     // the one side, we are at the other...
    01165       if(r < 0)
    01166         r = POSE_SPACE_GRID-1;
    01167       else if(r >= POSE_SPACE_GRID)
    01168         r = 0;
    01169
    01170     // fill the queue of samples for this cell by
    01171     // inserting before first sample
    01172       Cell& cell = poseSpace[r][y][x];
    01173     //as cell.first is preinitialized with 0, the end
    01174     //of the queue is always marked with 0
    01175       sample.next = cell.first;
    01176       cell.first = &sample;
    01177       cell.count++;
    01178     }
    01179   }
    01180
    01181   // determine the 2x2x2 sub-cube that contains most samples
    01182
    01183   // the position and sample-count of the winning sub-cube
    01184   int rMax[NUM_OF_CALCULATED_POSES],
    01185       yMax[NUM_OF_CALCULATED_POSES],
    01186       xMax[NUM_OF_CALCULATED_POSES],
    01187       countMax[NUM_OF_CALCULATED_POSES];
    01188
    01189   for (i=0;i<NUM_OF_CALCULATED_POSES;i++)
    01190   {
    01191     rMax[i] = 0;
    01192     yMax[i] = 0;
    01193     xMax[i] = 0;
    01194     countMax[i] = 0;
    01195   }
    01196
    01197   // iterate wih "r"over the rotation axis...
    01198   for(int r = 0; r < POSE_SPACE_GRID; r++)
    01199   {
    01200   // as this axis is cyclic, the next rotation index is:
    01201     int rNext = (r + 1) % POSE_SPACE_GRID;
    01202
    01203   // iterate with "x" and "y" over the translation axes
    01204     for(int y = 0; y < POSE_SPACE_GRID - 1; y++)
    01205       for(int x = 0; x < POSE_SPACE_GRID - 1; x++)
    01206       {
    01207     // get the number of all samples in the subcube
    01208     // around the current cell [r][y][x]
    01209         int count = poseSpace[r][y][x].count +
    01210                     poseSpace[r][y][x+1].count +
    01211                     poseSpace[r][y+1][x].count +
    01212                     poseSpace[r][y+1][x+1].count +
    01213                     poseSpace[rNext][y][x].count +
    01214                     poseSpace[rNext][y][x+1].count +
    01215                     poseSpace[rNext][y+1][x].count +
    01216                     poseSpace[rNext][y+1][x+1].count;
    01217     // if we've got a new winning cube, we have to remember
    01218         if(count > countMax[0])
    01219         {
    01220
    01221           if (NUM_OF_CALCULATED_POSES>1)
    01222           {
    01223             for (i=NUM_OF_CALCULATED_POSES-1;i>0;i--)
    01224             {
    01225               rMax[i] = rMax[i-1];
    01226               xMax[i] = xMax[i-1];
    01227               yMax[i] = yMax[i-1];
    01228               countMax[i] = countMax[i-1];
    01229             }
    01230           }
    01231           countMax[0] = count;
    01232           rMax[0] = r;
    01233           yMax[0] = y;
    01234           xMax[0] = x;
    01235         }
    01236       }
    01237   }
    01238
    01239   if(countMax[0] > 0)
    01240   {
    01241     for (i=0;i<NUM_OF_CALCULATED_POSES;i++)
    01242     {
    01243       if (countMax[i]>0)
    01244       {
    01245         /*candidates*/robotPoseCollection.poses[i] = calcPoseFromSubCube(poseSpace,xMax[i],yMax[i],rMax[i]);
    01246         CIRCLE(selfLocatorField,candidates[i].translation.x,candidates[i].translation.y,50,50,Drawings::ps_solid,Drawings::yellowOrange);
    01247       } else
    01248       {
    01249         /*candidates*/robotPoseCollection.poses[i].setValidity(-1);
    01250       }
    01251     }
    01252     pose = /*candidates*/robotPoseCollection.poses[0].getPose();
    01253     // validity = results[0].getValidity();
    01254     validity = calcDistributionValidity();
    01255
    01256     //robotPoseCollection.poses = candidates;
    01257     robotPoseCollection.numberOfPoses = NUM_OF_CALCULATED_POSES;
    01258   } else {
    01259     validity = 0;
    01260   }
    01261
    01262   // Vector2<double> diff = gtCamWorldState.getRobotPose().translation - pose.translation;
    01263   // OUTPUT(idText,text,"GT2005Stable "<< diff.abs() << " " << validity);
    01264 }
    01265
    01266 bool GT2005StableSelfLocator::handleMessage(InMessage& message)
    01267 {/*
    01268   switch(message.getMessageID())
    01269   {
    01270     case idGT2005SelfLocator:
    01271     {
    01272       GenericDebugData d;
    01273       message.bin >> d;
    01274       GT2005StableSelfLocatorSample::paramProbUpLimit   = d.data[0];
    01275       GT2005StableSelfLocatorSample::paramProbDownLimit = d.data[1];
    01276       paramObservationUpdateZAngleErrorWeight  = d.data[2];
    01277       paramObservationUpdateYAngleErrorWeight  = d.data[3];  // ie gauss sharpness for y-angle errors
    01278       paramObservationUpdateZAngleErrorCrossings = d.data[4];
    01279       paramObservationUpdateYAngleErrorCrossings = d.data[5];
    01280     }
    01281     break;
    01282   }*/
    01283   return false;
    01284 }
    01285
    01286
    01287
    01288
    01289
    01290 ///////////////////////////////////////////////////////////////////////
    01291 // Debug drawing 1:1 taken from gt04
    01292
    01293 void GT2005StableSelfLocator::draw(const Pose2D& pose,Drawings::Color color) const
    01294 {
    01295   Pose2D p = pose + Pose2D(-100,0);
    01296   LINE(selfLocatorField,
    01297        pose.translation.x,
    01298        pose.translation.y,
    01299        p.translation.x,
    01300        p.translation.y,
    01301        1,
    01302        Drawings::ps_solid,
    01303        color);
    01304   p = pose + Pose2D(-40,-40);
    01305   LINE(selfLocatorField,
    01306        pose.translation.x,
    01307        pose.translation.y,
    01308        p.translation.x,
    01309        p.translation.y,
    01310        1,
    01311        Drawings::ps_solid,
    01312        color);
    01313   p = pose + Pose2D(-40,40);
    01314   LINE(selfLocatorField,
    01315        pose.translation.x,
    01316        pose.translation.y,
    01317        p.translation.x,
    01318        p.translation.y,
    01319        1,
    01320        Drawings::ps_solid,
    01321        color);
    01322 }
    01323
    01324
    01325 void GT2005StableSelfLocator::draw(const Vector2<int>& point,LinesPercept::LineType type) const
    01326 {
    01327   static const Drawings::Color colors[] =
    01328   {
    01329     Drawings::red,
    01330     Drawings::green,
    01331     Drawings::yellow,
    01332     Drawings::skyblue
    01333   };
    01334
    01335   CameraInfo cameraInfo(getRobotConfiguration().getRobotDesign());
    01336
    01337   Vector2<int> p;
    01338   Geometry::calculatePointInImage(
    01339     point,
    01340     cameraMatrix, cameraInfo,
    01341     p);
    01342   CIRCLE(selfLocator,p.x,p.y,3,1,Drawings::ps_solid,colors[type]);
    01343 }
    01344
    01345
    01346 /*
    01347 GT2005StableSelfLocator::~GT2005StableSelfLocator(void)
    01348 {
    01349 }*/