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

# Modules/SelfLocator/SlamSelfLocator/SlamSelfLocator.cpp

Go to the documentation of this file.

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