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

# Modules/BallLocator/GT2005BallLocator.cpp

Go to the documentation of this file.

    00001 /**
    00002  * @file GT2005BallLocator.cpp
    00003  * Contains a BallLocator implementation using Particle Filters
    00004  *
    00005  * @author <a href="mailto:christinez@gmx.de">Christine Zarges</a>
    00006  * @author <a href="mailto:Thorsten.Kerkhof@gmx.de">Thorsten Kerkhof</a>
    00007  */
    00008 //---------------------------------------------------------------------------------------
    00009 #include "GT2005BallLocator.h"
    00010 #include "Tools/Debugging/Debugging.h"
    00011 #include "Tools/FieldDimensions.h"
    00012 #include "Tools/Debugging/DebugDrawings.h"
    00013 //---------------------------------------------------------------------------------------
    00014 GT2005BallLocator::GT2005BallLocator(const BallLocatorInterfaces& interfaces)
    00015  : BallLocator(interfaces)
    00016 {
    00017   frameNumber = 0;
    00018   lastOdometryData = odometryData;
    00019   currentSystemTime = SystemCall::getCurrentSystemTime();
    00020   lastSystemTime = SystemCall::getCurrentSystemTime();
    00021   ballSeenTime = 0;
    00022   dFieldDiagonalLength = 1.2*sqrt((2.0*xPosOpponentGoal)*(2.0*xPosOpponentGoal) +
    00023                                   (2.0*yPosLeftFlags)*(2.0*yPosLeftFlags));
    00024
    00025   framesNoBallSeen = 500; //initialized very high...
    00026   lastPositionSeen.x = 0;
    00027   lastPositionSeen.y = 0;
    00028
    00029   prevCameraMatrix = cameraMatrix;
    00030
    00031   stateIsSet = false;
    00032
    00033   /* Use in the ball model the probabilities the ball locator sets
    00034    * (default is a calculation in the ball model itself):
    00035    */
    00036   ballModel.seen.useGivenProbabilities = true;
    00037   ballModel.propagated.useGivenProbabilities = true;
    00038
    00039   initializeParticles();
    00040 }
    00041 //---------------------------------------------------------------------------------------
    00042 double GT2005BallLocator::calculateRobotSpeedValue()
    00043 {
    00044   double robotSpeed = (motionInfo.executedMotionRequest.walkRequest.walkParams.translation).abs();
    00045   double value(0);
    00046   if(robotSpeed < 200)
    00047     value = - 3 * robotSpeed / 20 + 110;
    00048   else if(robotSpeed < 260)
    00049     value = 80;
    00050   else value = 30 * (robotSpeed - 260) / 40 + 80;
    00051   return 80 / value;
    00052 }
    00053 //------------------------------------------------------------------------------
    00054 void GT2005BallLocator::execute()
    00055 {
    00056   modify();
    00057
    00058   //Get the current system time and translate it into seconds:
    00059   currentSystemTime = SystemCall::getCurrentSystemTime();
    00060   timeChange = ((double)(currentSystemTime) - (double)(lastSystemTime)) / 1000;
    00061
    00062   oldX = ballX;
    00063   oldY = ballY;
    00064
    00065   //If the first time the game controller says "set" initialize the particles:
    00066   if ((gameControlData.data.state == GameControlData::set) && (!stateIsSet))
    00067   {
    00068     initializeParticles();
    00069     stateIsSet = true;
    00070   }
    00071   else //!(for the first time game state is "set")
    00072   {
    00073     if (gameControlData.data.state != GameControlData::set)
    00074       stateIsSet = false;
    00075
    00076     timeUpdate();
    00077
    00078     if (ballPercept.multiplePercepts.numberOfElements == 0)
    00079     {
    00080       //Perhaps without multiple ballPercepts:
    00081       Vector2<double> ballOffset;
    00082       ballOffset = ballPercept.offsetOnField;
    00083       if (ballPercept.ballWasSeen && !(isNonSensePos(ballOffset)))
    00084       {
    00085         double distanceValue = calculateDistanceValue(ballOffset);
    00086         double panningVelocityValue = calculatePanningVelocityValue();
    00087         double robotSpeedValue = calculateRobotSpeedValue();
    00088         measurementUpdate(ballOffset, ballPercept.reliability, distanceValue, panningVelocityValue,
    00089           robotSpeedValue);
    00090
    00091         //Update last seen position:
    00092         framesNoBallSeen = 0;
    00093         lastPositionSeen.x = ballOffset.x;
    00094         lastPositionSeen.y = ballOffset.y;
    00095         ballSeenTime = currentSystemTime;
    00096       }
    00097       else //!(ballPercept.ballWasSeen && !(isNonSensePos(ballOffset))
    00098         framesNoBallSeen++;
    00099     }
    00100     else
    00101     {
    00102       Vector2<double> ballOffset;
    00103
    00104       //With multiple ball percepts:
    00105       for (int i = 0; i < ballPercept.multiplePercepts.numberOfElements; i++)
    00106       {
    00107         ballOffset = ballPercept.multiplePercepts[i].offsetOnField;
    00108         double distanceValue = ballPercept.multiplePercepts[i].distanceError;
    00109         double panningVelocityValue = ballPercept.multiplePercepts.panningVelocityError;
    00110         double robotSpeedValue = calculateRobotSpeedValue();
    00111         if (!(isNonSensePos(ballOffset)))
    00112           measurementUpdate(ballPercept.multiplePercepts[i].offsetOnField,
    00113                             ballPercept.multiplePercepts[i].reliability,
    00114                             distanceValue,
    00115                             panningVelocityValue,
    00116                             robotSpeedValue);
    00117       }
    00118
    00119       //Save the position taken from the percept:
    00120       bool mainPerceptNonsense = false;
    00121
    00122       //only the main percept will be stored, if this is non sense,
    00123       //we don't save any percept in this frame
    00124       ballOffset = ballPercept.multiplePercepts[0].offsetOnField;
    00125       if (!(isNonSensePos(ballOffset)))
    00126       {
    00127         //Update last seen position:
    00128         framesNoBallSeen = 0;
    00129         lastPositionSeen.x = ballOffset.x;
    00130         lastPositionSeen.y = ballOffset.y;
    00131         ballSeenTime = currentSystemTime;
    00132       }
    00133       else
    00134         mainPerceptNonsense = true;
    00135
    00136       if (mainPerceptNonsense)
    00137         framesNoBallSeen++;
    00138     }
    00139
    00140     /* If the ball was dribbled in the last frame the ball position is very
    00141      * similar to the robot pose:
    00142      */
    00143     if (ballHandling.behaviorBallState == BallHandling::dribbleTheBall)
    00144     {
    00145       measurementUpdate(Vector2<double>(50, 0), 0.6, 1.0, 1.0, 1.0);
    00146     }
    00147   }
    00148
    00149   ballLocatorSamples.calculateEstimatedBall(ballX, ballY, ballP,
    00150                                             ballVX, ballVY, ballVP);
    00151
    00152   setBallSymbols();
    00153
    00154   lastSystemTime = currentSystemTime;
    00155   lastOdometryData = odometryData;
    00156
    00157   draw(Drawings::dark_gray, //particle color
    00158        Drawings::orange);   //estimated ball color
    00159
    00160   noNonsenseParticles();
    00161
    00162   frameNumber++;
    00163   prevCameraMatrix = cameraMatrix;
    00164 }
    00165 //---------------------------------------------------------------------------------------
    00166 void GT2005BallLocator::initializeParticles()
    00167 {
    00168   /* Particles will be thrown on the field in a close distance to the
    00169    * KickOffPosition:
    00170    */
    00171   for (int i = 0; i < ballLocatorSamples.getNumberOfSamples(); i++)
    00172   {
    00173     double randomValue1 = random();
    00174     double randomValue2 = random();
    00175     ballLocatorSamples[i].setParameters
    00176       (random(2*50) - 50 - robotPose.translation.x,
    00177        random(2*50) - 50 - robotPose.translation.y,
    00178        randomValue1 * 30, randomValue2 * 30,
    00179        0.5, 0);
    00180   }
    00181   ballX = -robotPose.translation.x;
    00182   ballY = -robotPose.translation.y;
    00183   ballVX = 0;
    00184   ballVY = 0;
    00185   ballP = 0.5;
    00186   ballVP = 0;
    00187 }
    00188 //---------------------------------------------------------------------------------------
    00189 double GT2005BallLocator::getTimeFactor()
    00190 {
    00191   if (framesNoBallSeen < 1 * GT2005BallLocatorParameters::timeFactorThreshold)
    00192     return 1;
    00193   if (framesNoBallSeen < 2 * GT2005BallLocatorParameters::timeFactorThreshold)
    00194     return 0.9;
    00195   if (framesNoBallSeen < 3 * GT2005BallLocatorParameters::timeFactorThreshold)
    00196     return 0.8;
    00197   if (framesNoBallSeen < 4 * GT2005BallLocatorParameters::timeFactorThreshold)
    00198     return 0.7;
    00199   if (framesNoBallSeen < 5 * GT2005BallLocatorParameters::timeFactorThreshold)
    00200     return 0.6;
    00201   return 0.5;
    00202 }
    00203 //---------------------------------------------------------------------------------------
    00204 void GT2005BallLocator::timeUpdate()
    00205 {
    00206   /* When a timeUpdate is done, the particles have to be "pushed"
    00207    * on the field depending on the odometry data and the speed
    00208    * of the particle.
    00209    */
    00210   Pose2D changeRobot = odometryData - lastOdometryData;
    00211
    00212   Vector2<double> c1(cos(changeRobot.rotation),-sin(changeRobot.rotation));
    00213   Vector2<double> c2(sin(changeRobot.rotation),cos(changeRobot.rotation));
    00214   Matrix2x2<double> rotM(c1, c2);
    00215   Vector2<double> n(oldX, oldY);
    00216   n = rotM * n;
    00217   n -= changeRobot.translation;
    00218   oldX = n.x;
    00219   oldY = n.y;
    00220
    00221   /* Calculate the velocity out of x- and ychange and the time between the last
    00222    * and the current frame.
    00223    */
    00224   int i;
    00225   for (i = 0; i < ballLocatorSamples.getNumberOfSamples(); i++)
    00226   {
    00227     /* x- and y-Position are updated dependend on the velocity of the ball and
    00228      * the movement of the robot.
    00229      */
    00230     ballLocatorSamples[i].pose = rotM * ballLocatorSamples[i].pose;
    00231     ballLocatorSamples[i].pose.x +=
    00232       ballLocatorSamples[i].vx * timeChange - changeRobot.translation.x;
    00233     ballLocatorSamples[i].pose.y +=
    00234       ballLocatorSamples[i].vy * timeChange - changeRobot.translation.y;
    00235
    00236     //Velocity of the ball is decreased as the ball slows down:
    00237     Vector2<double> v = ballLocatorSamples[i].getVelocity();
    00238     v = rotM * v; //rotate velocity vector
    00239
    00240     double l = sqrt(v.x * v.x + v.y * v.y);
    00241
    00242     /* The breakFactor shows how fast the ball slows down,
    00243      * the function is taken from physics.
    00244      */
    00245     double newLength
    00246       = -(GT2005BallLocatorParameters::coefficientOfFriction * 9810 * timeChange) + l;
    00247     double l2 = 0;
    00248     if(l != 0)
    00249       l2 = (1 / l) * newLength;
    00250
    00251     ballLocatorSamples[i].setVelocity(l2 * v.x, l2 * v.y);
    00252
    00253     /* Reliability of the particle is decreased depending on the system noise
    00254      * and the time the ball is not seen.
    00255      */
    00256     ballLocatorSamples[i].probability *= (1 - GT2005BallLocatorParameters::systemNoise)
    00257       * getTimeFactor(); //getTimeFactor depends on currentSystemTime
    00258   }
    00259 }
    00260 //---------------------------------------------------------------------------------------
    00261 double GT2005BallLocator::calculateDistanceValue(Vector2<double> ballOffset)
    00262 {
    00263   /* "distance to ball"-value:
    00264    * b is equal to 1 if the calculated distance value is 50 (the minimum of the function)
    00265    * the lowest value is 0.045 for a distance of approximately 4000 mm
    00266    */
    00267   double d2 = ballOffset.x*ballOffset.x + ballOffset.y*ballOffset.y;
    00268   double d = sqrt(d2);
    00269
    00270   //Function is result of BallLocatorEvolutionBehavior:
    00271   double value;
    00272   if (d < 250)
    00273     value = (-d/12 + 74);
    00274   if (d < 1500)
    00275     value = 50;
    00276   else
    00277     value = (0.000160403*d2 - 0.440147*d + 349.31375);
    00278
    00279   return 50 / value;
    00280 }
    00281 //---------------------------------------------------------------------------------------
    00282 double GT2005BallLocator::calculatePanningVelocityValue()
    00283 {
    00284   //Probability depending on panning velocity and distance to ball:
    00285   Vector3<double> vectorInDirectionOfViewCamera(1,0,0);
    00286   Vector3<double> vectorInDirectionOfViewWorld,
    00287                   vectorInDirectionOfViewWorldOld;
    00288   vectorInDirectionOfViewWorld = cameraMatrix.rotation
    00289     * vectorInDirectionOfViewCamera;
    00290   vectorInDirectionOfViewWorldOld = prevCameraMatrix.rotation
    00291     * vectorInDirectionOfViewCamera;
    00292   long frameNumber = cameraMatrix.frameNumber,
    00293        prevFrameNumber = prevCameraMatrix.frameNumber;
    00294   const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    00295
    00296   //In seconds:
    00297   double timeDiff = (frameNumber - prevFrameNumber) * r.motionCycleTime;
    00298
    00299   Vector2<double> currentOnGround(vectorInDirectionOfViewWorld.x,
    00300                                   vectorInDirectionOfViewWorld.y),
    00301                   oldOnGround(vectorInDirectionOfViewWorldOld.x,
    00302                               vectorInDirectionOfViewWorldOld.y);
    00303   currentOnGround.normalize();
    00304   oldOnGround.normalize();
    00305   double panningVelocity = 0;
    00306   if (timeDiff > 0)
    00307   {
    00308     double cosAng = currentOnGround*oldOnGround;
    00309     if (cosAng > 1.0)
    00310       cosAng = 1.0;
    00311     else if (cosAng < -1.0)
    00312       cosAng = -1.0;
    00313     panningVelocity = fabs(normalize(acos(cosAng))/timeDiff);
    00314   }
    00315
    00316   /* "panningVelocity"-value:
    00317    * a is equal to 1 if the panning velocity is equal to 0 (minimum of the function)
    00318    * the lowest value is 0.52 for a panning velocity of approximately 5.5
    00319    */
    00320
    00321   //Function is result of BallLocatorEvolutionBehavior:
    00322   double value;
    00323   if(panningVelocity > 5.5)
    00324     value = 57.198511908222125;
    00325   else
    00326     value = -0.9 * panningVelocity * panningVelocity
    00327       + 9.90092024377335 * panningVelocity
    00328       + 29.9684505674687;
    00329
    00330   return 30 / value;
    00331 }
    00332 //---------------------------------------------------------------------------------------
    00333 void GT2005BallLocator::measurementUpdate(Vector2<double> ballOffset,
    00334                                           double reliability,
    00335                                           double distanceValue,
    00336                                           double panningVelocityValue,
    00337                                           double robotSpeedValue)
    00338 {
    00339   /*
    00340    * update positions
    00341    */
    00342
    00343   double imageBallProb = (distanceValue + panningVelocityValue + robotSpeedValue) * reliability / 3;
    00344
    00345   /* This value counts the number of particles which are inside the circle with
    00346    * the radius timesBallRadius * ballRadius.
    00347    */
    00348   int numPartInside = 0;
    00349
    00350   int i;
    00351   for (i = 0; i < ballLocatorSamples.getNumberOfSamples(); i++)
    00352   {
    00353     GT2005Particle &particle = ballLocatorSamples[i];
    00354
    00355     double distance =
    00356       Geometry::distanceTo(particle.getPosition(), ballOffset);
    00357
    00358     if (distance < GT2005BallLocatorParameters::timesBallRadius * ballRadius)
    00359     {
    00360       double oldx = particle.pose.x;
    00361       double oldy = particle.pose.y;
    00362
    00363       numPartInside += 1;
    00364       particle.pose.x = (GT2005BallLocatorParameters::timesPartProb * particle.probability
    00365                          * particle.pose.x + distanceValue * ballOffset.x)
    00366         / (GT2005BallLocatorParameters::timesPartProb * particle.probability + distanceValue);
    00367       particle.pose.y = (GT2005BallLocatorParameters::timesPartProb * particle.probability
    00368                          * particle.pose.y + panningVelocityValue * ballOffset.y)
    00369         / (GT2005BallLocatorParameters::timesPartProb * particle.probability
    00370            + panningVelocityValue);
    00371       double probadd = distance/ballRadius + 1;
    00372       probadd = GT2005BallLocatorParameters::nearPerceptValue1 / probadd;
    00373       probadd += particle.probability * GT2005BallLocatorParameters::nearPerceptValue2;
    00374       probadd *= imageBallProb * GT2005BallLocatorParameters::nearPerceptValue3;
    00375       particle.probability = min(1.0, particle.probability + probadd);
    00376       particle.vprob = imageBallProb * GT2005BallLocatorParameters::nearPerceptValue4;
    00377       particle.seenvx = (ballOffset.x - oldx) / timeChange;
    00378       particle.seenvy = (ballOffset.y - oldy) / timeChange;
    00379     }
    00380     else //!(distance < timesBallRadius * ballRadius)
    00381     {
    00382       particle.probability *= (1 - GT2005BallLocatorParameters::measurementNoise);
    00383       particle.probability -= GT2005BallLocatorParameters::farPerceptValue1 * imageBallProb;
    00384       if (particle.probability < 0)
    00385         particle.probability = 0;
    00386       particle.vprob = 0;
    00387       Vector2<double> tmp = particle.getPosition();
    00388       tmp.x += particle.provx * timeChange * 2 - particle.vx * timeChange;
    00389       tmp.y += particle.provy * timeChange * 2 - particle.vy * timeChange;
    00390       double distance2 =
    00391         Geometry::distanceTo(tmp, ballOffset);
    00392       if (distance2 <   GT2005BallLocatorParameters::timesBallRadius * ballRadius
    00393                       * GT2005BallLocatorParameters::farPerceptValue2)
    00394       {
    00395         particle.vprob = 5;
    00396         particle.seenvx = particle.provx;
    00397         particle.seenvy = particle.provy;
    00398       }
    00399       else
    00400       {
    00401         particle.provx = (ballOffset.x - particle.pose.x) / timeChange;
    00402         particle.provy = (ballOffset.y - particle.pose.y) / timeChange;
    00403       }
    00404     }
    00405   }
    00406
    00407   /* If the number of particles inside the radius timesBallRadius * ballRadius
    00408    * is less than 5 percent of the number of particles we throw as many
    00409    * particles as needed to get more than 5 percent on a new position. This is
    00410    * done by giving them a probability of 0 so that the noNonesenseParticles-
    00411    * function takes care of them.
    00412    */
    00413   while (numPartInside < (  GT2005BallLocatorParameters::percentNumPartInside
    00414                           * ballLocatorSamples.getNumberOfSamples()))
    00415   {
    00416     int index = ballLocatorSamples.getIndexOfParticleWithLowestProb();
    00417     ballLocatorSamples[index].probability = 0;
    00418     numPartInside += 1;
    00419   }
    00420
    00421   /*
    00422    * update velocities
    00423    */
    00424   double calcVX;
    00425   double calcVY;
    00426
    00427   for (i = 0; i < ballLocatorSamples.getNumberOfSamples(); i++)
    00428   {
    00429     GT2005Particle &particle = ballLocatorSamples[i];
    00430
    00431     if (particle.probability == 0)
    00432       continue;
    00433
    00434     calcVX = particle.seenvx;
    00435     calcVY = particle.seenvy;
    00436
    00437     double randomValue1 =   (GT2005BallLocatorParameters::randomLimitV/2)
    00438                           - (double)(random((int)GT2005BallLocatorParameters::randomLimitV));
    00439     double randomValue2 =   (GT2005BallLocatorParameters::randomLimitV/2)
    00440                           - (double)(random((int)GT2005BallLocatorParameters::randomLimitV));
    00441     double partprob = GT2005BallLocatorParameters::timesPartProb * particle.probability;
    00442     double improb = particle.vprob * GT2005BallLocatorParameters::velocityWeightXDirection;
    00443
    00444     if (partprob + improb == 0)
    00445     {
    00446       particle.vx = 0;
    00447       particle.vy = 0;
    00448       continue;
    00449     }
    00450     else
    00451     {
    00452       particle.vx =
    00453         (partprob * particle.vx + improb * calcVX
    00454         + ((particle.probability + improb)/2) * randomValue1)
    00455         / (partprob + improb);
    00456       improb = particle.vprob;
    00457       particle.vy =
    00458         (partprob * particle.vy + improb * calcVY
    00459         + ((particle.probability + improb)/2) * randomValue2)
    00460         / (partprob + improb);
    00461     }
    00462
    00463     //Set velocity to 0 if it is NaN: (hopefully does not happen any more)
    00464     if (!((particle.vx > -1000000) && (particle.vx < 1000000)))
    00465     {
    00466       particle.vx = 0;
    00467       if (!((particle.vy > -1000000) && (particle.vy < 1000000)))
    00468       {
    00469         particle.vy = 0;
    00470         continue;
    00471       }
    00472       continue;
    00473     }
    00474     if (!((particle.vy > -1000000) && (particle.vy < 1000000)))
    00475     {
    00476       particle.vy = 0;
    00477       continue;
    00478     }
    00479
    00480     //Clipping velocities faster than 1500mm/s:
    00481     if (particle.vx*particle.vx + particle.vy*particle.vy > 1500*1500)
    00482     {
    00483       if (particle.vx > 1500)
    00484         particle.vx = 1500;
    00485       else
    00486         particle.vx /= 2;
    00487       if (particle.vy > 1500)
    00488         particle.vy = 1500;
    00489       else
    00490         particle.vy /= 2;
    00491     }
    00492   }
    00493 }
    00494 //---------------------------------------------------------------------------------------
    00495 void GT2005BallLocator::setBallSymbols()
    00496 {
    00497   ballModel.propagated.lastSeenBallState = ballModel.seen;
    00498   ballModel.propagated.setBallDataRelativeToRobot(robotPose,
    00499                                                   ballX, ballY,
    00500                                                   ballVX, ballVY);
    00501   ballModel.propagated.positionProb = ballP;
    00502   ballModel.propagated.velocityProb = ballVP;
    00503
    00504   /* Values depending on the existence of a ball in the ball-percept:
    00505    * When the ball was seen, we set the "ballWasSeen"-flag.
    00506    * Every time a ball was seen, we set the "timeUntilSeenConsecutively"-flag
    00507    * to the current system time, so that we always know the last timestamp
    00508    * where we had a ball percept.
    00509    * On the other hand we save the time, when we don't see the ball, so that we
    00510    * have a timestamp for the last time without ball percept.
    00511    */
    00512   if (ballPercept.ballWasSeen)
    00513   {
    00514     ballModel.ballWasSeen = true;
    00515     ballModel.numberOfImagesWithoutBallPercept = 0;
    00516     ballModel.numberOfImagesWithBallPercept += 1;
    00517
    00518     ballModel.seen.timeUntilSeenConsecutively = currentSystemTime;
    00519     ballModel.seen.setBallDataRelativeToRobot(robotPose,
    00520                                               lastPositionSeen.x,
    00521                                               lastPositionSeen.y,
    00522                                               ballVX,
    00523                                               ballVY);
    00524     ballModel.seen.timeWhenLastSeen = ballSeenTime;
    00525     ballModel.seen.positionProb = ballPercept.reliability;
    00526     ballModel.seen.velocityProb = ballVP;
    00527   }
    00528   else
    00529   {
    00530     //Update the seen position by odometry:
    00531     Pose2D changeRobot = odometryData - lastOdometryData;
    00532
    00533     Vector2<double> c1(cos(changeRobot.rotation),-sin(changeRobot.rotation));
    00534     Vector2<double> c2(sin(changeRobot.rotation),cos(changeRobot.rotation));
    00535     Matrix2x2<double> rotM(c1, c2);
    00536     Vector2<double> n(oldX, oldY);
    00537     n = rotM * n;
    00538     n -= changeRobot.translation;
    00539
    00540     Vector2<double> lastSeenPosition = ballModel.seen.positionRobot;
    00541     lastSeenPosition = rotM * lastSeenPosition;
    00542     lastSeenPosition.x -= changeRobot.translation.x;
    00543     lastSeenPosition.y -= changeRobot.translation.y;
    00544
    00545     ballModel.seen.setBallDataRelativeToRobot(robotPose,
    00546                                               lastSeenPosition.x,
    00547                                               lastSeenPosition.y,
    00548                                               ballVX, ballVY);
    00549
    00550     ballModel.seen.positionProb = max(0.0, ballModel.seen.positionProb * 0.8);
    00551     ballModel.seen.velocityProb = 0;
    00552
    00553     ballModel.ballWasSeen = false;
    00554     ballModel.seen.timeWhenFirstSeenConsecutively = currentSystemTime;
    00555     ballModel.numberOfImagesWithoutBallPercept += 1;
    00556     ballModel.numberOfImagesWithBallPercept = 0;
    00557   }
    00558
    00559   Player::teamColor ownColor = getPlayer().getTeamColor();
    00560   colorClass opponentGoalColor;
    00561   if (ownColor == Player::red)
    00562     opponentGoalColor = skyblue;
    00563   else
    00564     opponentGoalColor = yellow;
    00565   ballModel.seen.ballInFrontOfOpponentGoal = false;
    00566   ballModel.propagated.ballInFrontOfOpponentGoal = false;
    00567   if (landmarksPercept.numberOfGoals > 0)
    00568   {
    00569     double angleToBall = ballPercept.sizeBasedOffsetOnField.angle();
    00570     double ballDistance = ballPercept.sizeBasedOffsetOnField.abs();
    00571     if(
    00572       landmarksPercept.goals[0].color == opponentGoalColor &&
    00573       ballPercept.ballWasSeen &&
    00574       landmarksPercept.goals[0].x.min < angleToBall &&
    00575       angleToBall < landmarksPercept.goals[0].x.max &&
    00576       ballDistance < 1100
    00577       )
    00578     {
    00579       ballModel.seen.ballInFrontOfOpponentGoal = true;
    00580       ballModel.propagated.ballInFrontOfOpponentGoal = true;
    00581     }
    00582   }
    00583 }
    00584 //---------------------------------------------------------------------------------------
    00585 bool GT2005BallLocator::isNonSensePos(Vector2<double> position)
    00586 {
    00587   double distance = position.x*position.x + position.y*position.y;
    00588   if (distance > dFieldDiagonalLength*dFieldDiagonalLength)
    00589   {
    00590     nonsensePos++;
    00591     return true;
    00592   }
    00593   else
    00594     return false;
    00595 }
    00596 //---------------------------------------------------------------------------------------
    00597 void GT2005BallLocator::noNonsenseParticles()
    00598 {
    00599   int i;
    00600   for (i = 0; i < ballLocatorSamples.getNumberOfSamples(); i++)
    00601   {
    00602     GT2005Particle &particle = ballLocatorSamples[i];
    00603     double distance = particle.pose.x*particle.pose.x
    00604       + particle.pose.y*particle.pose.y;
    00605     if (distance > (dFieldDiagonalLength*dFieldDiagonalLength + 100))
    00606       particle.probability -= GT2005BallLocatorParameters::particleProbDecreaseStep;
    00607
    00608     if (particle.probability < GT2005BallLocatorParameters::particleProbThreshold)
    00609     {
    00610       double fNBSP = getTimeFactor() / 2; //framesNoBallSeenProbability
    00611       double randomValue = random();
    00612       randomValue *= (fNBSP + ballP);
    00613       double xPosRel;
    00614       double yPosRel;
    00615       if (randomValue <= fNBSP)
    00616       {
    00617         /* Throw gaussian around the lastPositionSeen.
    00618          * (method of Marsaglia, first for x, then for y)
    00619          */
    00620         double v = 2;
    00621         double u1 = 0;
    00622         double u2 = 0;
    00623         int count = 0;
    00624         while (v >= 1)
    00625         {
    00626           u1 = random();
    00627           u2 = random();
    00628           v = (2*u1 - 1)*(2*u1 - 1) + (2*u2 - 1)*(2*u2 - 1);
    00629           if ((v >= 1) && (count++ > 10))
    00630             v = 0.9;
    00631         }
    00632         double xRand =   GT2005BallLocatorParameters::timesBallRadius * ballRadius
    00633                        * sqrt(fNBSP) * ((2*u1 - 1) * sqrt(-2*log(v)/v));
    00634         v = 2;
    00635         count = 0;
    00636         while (v >= 1)
    00637         {
    00638           u1 = random();
    00639           u2 = random();
    00640           v = (2*u1 - 1)*(2*u1 - 1) + (2*u2 - 1)*(2*u2 - 1);
    00641           if ((v >= 1) && (count++ > 10))
    00642             v = 0.9;
    00643         }
    00644         double yRand =   GT2005BallLocatorParameters::timesBallRadius * ballRadius
    00645                        * sqrt(fNBSP) * ((2*u1 - 1) * sqrt(-2*log(v)/v));
    00646
    00647         xPosRel = lastPositionSeen.x + xRand;
    00648         yPosRel = lastPositionSeen.y + yRand;
    00649       }
    00650       else //!(randomValue < fNBSP)
    00651       {
    00652         /* Throw gaussian around the estimatedBallSeen.
    00653          * (method of Marsaglia, first for x, then for y)
    00654          */
    00655         double v = 2;
    00656         double u1 = 0;
    00657         double u2 = 0;
    00658         int count = 0;
    00659         while (v >= 1)
    00660         {
    00661           u1 = random();
    00662           u2 = random();
    00663           v = (2*u1 - 1)*(2*u1 - 1) + (2*u2 - 1)*(2*u2 - 1);
    00664           if ((v >= 1) && (count++ > 10))
    00665             v = 0.9;
    00666         }
    00667         double xRand =   GT2005BallLocatorParameters::timesBallRadius
    00668                        * ballRadius * sqrt(fNBSP) * ((2*u1 - 1) * sqrt(-2*log(v)/v));
    00669         v = 2;
    00670         count = 0;
    00671         while (v >= 1)
    00672         {
    00673           u1 = random();
    00674           u2 = random();
    00675           v = (2*u1 - 1)*(2*u1 - 1) + (2*u2 - 1)*(2*u2 - 1);
    00676           if ((v >= 1) && (count++ > 10))
    00677             v = 0.9;
    00678         }
    00679         double yRand =   GT2005BallLocatorParameters::timesBallRadius * ballRadius
    00680                        * sqrt(fNBSP) * ((2*u1 - 1) * sqrt(-2*log(v)/v));
    00681         xPosRel = ballX + xRand;
    00682         yPosRel = ballY + yRand;
    00683       }
    00684       //Clipping points outside the field:
    00685       Vector2<double> posAbs = Geometry::relative2FieldCoord(robotPose, xPosRel, yPosRel);
    00686       if (posAbs.x > xPosOpponentGroundline + 250)
    00687       {
    00688         Vector2<double> newPos = Geometry::fieldCoord2Relative(robotPose,
    00689           Vector2<double>(xPosOpponentGroundline, posAbs.y));
    00690         xPosRel = newPos.x;
    00691         yPosRel = newPos.y;
    00692         posAbs.x = xPosOpponentGroundline;
    00693       }
    00694       else if (posAbs.x < xPosOwnGroundline - 250)
    00695       {
    00696         Vector2<double> newPos = Geometry::fieldCoord2Relative(robotPose,
    00697           Vector2<double>(xPosOwnGroundline, posAbs.y));
    00698         xPosRel = newPos.x;
    00699         yPosRel = newPos.y;
    00700         posAbs.x = xPosOwnGroundline;
    00701       }
    00702       if (posAbs.y > yPosLeftSideline + 250)
    00703       {
    00704         Vector2<double> newPos = Geometry::fieldCoord2Relative(robotPose,
    00705           Vector2<double>(posAbs.x, yPosLeftSideline));
    00706         xPosRel = newPos.x;
    00707         yPosRel = newPos.y;
    00708       }
    00709       else if (posAbs.y < yPosRightSideline - 250)
    00710       {
    00711         Vector2<double> newPos = Geometry::fieldCoord2Relative(robotPose,
    00712           Vector2<double>(posAbs.x, yPosRightSideline));
    00713         xPosRel = newPos.x;
    00714         yPosRel = newPos.y;
    00715       }
    00716
    00717       double randomValue1 = random();
    00718       double randomValue2 = random();
    00719
    00720       //Throw new particle:
    00721       particle.setParameters(xPosRel, yPosRel,
    00722                              randomValue1 * 50, randomValue2 * 50,
    00723                              GT2005BallLocatorParameters::throwInProb, 0);
    00724     }
    00725   }
    00726 }
    00727 //---------------------------------------------------------------------------------------
    00728 bool GT2005BallLocator::handleMessage(InMessage& message)
    00729 {
    00730   switch(message.getMessageID())
    00731   {
    00732   case idGT2005BallLocator1:
    00733     {
    00734       GenericDebugData d;
    00735       message.bin >> d;
    00736       GT2005BallLocatorParameters::systemNoise = d.data[0] / 100;
    00737       GT2005BallLocatorParameters::measurementNoise = d.data[1] / 100;
    00738       GT2005BallLocatorParameters::coefficientOfFriction = d.data[2] / 1000;
    00739       GT2005BallLocatorParameters::timesPartProb = d.data[3];
    00740       GT2005BallLocatorParameters::particleProbThreshold = d.data[4] / 100;
    00741       GT2005BallLocatorParameters::timesBallRadius = d.data[5];
    00742       GT2005BallLocatorParameters::randomLimitV = d.data[6];
    00743       GT2005BallLocatorParameters::timeFactorThreshold = d.data[7];
    00744       GT2005BallLocatorParameters::throwInProb = d.data[8] / 100;
    00745     }
    00746     return true;
    00747     break;
    00748   case idGT2005BallLocator2:
    00749     {
    00750       GenericDebugData d;
    00751       message.bin >> d;
    00752       GT2005BallLocatorParameters::velocityWeightXDirection = d.data[0] / 100;
    00753       GT2005BallLocatorParameters::percentNumPartInside = d.data[1] / 100;
    00754       GT2005BallLocatorParameters::particleProbDecreaseStep = d.data[2] / 100;
    00755       GT2005BallLocatorParameters::nearPerceptValue1 = d.data[3] / 100;
    00756       GT2005BallLocatorParameters::nearPerceptValue2 = d.data[4] / 100;
    00757       GT2005BallLocatorParameters::nearPerceptValue3 = d.data[5] / 100;
    00758       GT2005BallLocatorParameters::nearPerceptValue4 = d.data[6] / 100;
    00759       GT2005BallLocatorParameters::farPerceptValue1 = d.data[7] / 100;
    00760       GT2005BallLocatorParameters::farPerceptValue2 = d.data[8] / 100;
    00761     }
    00762     return true;
    00763     break;
    00764   }
    00765   return false;
    00766 }
    00767 //---------------------------------------------------------------------------------------
    00768 void GT2005BallLocator::modify()
    00769 {
    00770   MODIFY("BallLocator:coefficientOfFriction",
    00771          GT2005BallLocatorParameters::coefficientOfFriction);
    00772   MODIFY("BallLocator:measurementNoise",
    00773          GT2005BallLocatorParameters::measurementNoise);
    00774   MODIFY("BallLocator:systemNoise",
    00775          GT2005BallLocatorParameters::systemNoise);
    00776   MODIFY("BallLocator:particleProbThreshold",
    00777          GT2005BallLocatorParameters::particleProbThreshold);
    00778   MODIFY("BallLocator:timesBallRadius",
    00779          GT2005BallLocatorParameters::timesBallRadius);
    00780   MODIFY("BallLocator:timeFactorThreshold",
    00781          GT2005BallLocatorParameters::timeFactorThreshold);
    00782   MODIFY("BallLocator:throwInProb",
    00783          GT2005BallLocatorParameters::throwInProb);
    00784   MODIFY("BallLocator:timesPartProb",
    00785          GT2005BallLocatorParameters::timesPartProb);
    00786   MODIFY("BallLocator:randomLimitV",
    00787          GT2005BallLocatorParameters::randomLimitV);
    00788   MODIFY("BallLocator:velocityWeightXDirection",
    00789          GT2005BallLocatorParameters::velocityWeightXDirection);
    00790   MODIFY("BallLocator:percentNumPartInside",
    00791          GT2005BallLocatorParameters::percentNumPartInside);
    00792   MODIFY("BallLocator:particleProbDecreaseStep",
    00793          GT2005BallLocatorParameters::particleProbDecreaseStep);
    00794   MODIFY("BallLocator:nearPerceptValue1",
    00795          GT2005BallLocatorParameters::nearPerceptValue1);
    00796   MODIFY("BallLocator:nearPerceptValue2",
    00797          GT2005BallLocatorParameters::nearPerceptValue2);
    00798   MODIFY("BallLocator:nearPerceptValue3",
    00799          GT2005BallLocatorParameters::nearPerceptValue3);
    00800   MODIFY("BallLocator:nearPerceptValue4",
    00801          GT2005BallLocatorParameters::nearPerceptValue4);
    00802   MODIFY("BallLocator:farPerceptValue1",
    00803          GT2005BallLocatorParameters::farPerceptValue1);
    00804   MODIFY("BallLocator:farPerceptValue2",
    00805          GT2005BallLocatorParameters::farPerceptValue2);
    00806 }
    00807 //---------------------------------------------------------------------------------------
    00808 void GT2005BallLocator::drawParticle(const GT2005Particle& particle,
    00809                                      Drawings::Color color) const
    00810 {
    00811   double vx = particle.getVelocity().x;
    00812   double vy = particle.getVelocity().y;
    00813   double v = vx*vx + vy*vy;
    00814
    00815   if(v <= 50*50)
    00816   {
    00817     Vector2<double> position = Geometry::relative2FieldCoord(robotPose,
    00818       particle.getPosition().x,
    00819       particle.getPosition().y);
    00820     double thickness = (particle.probability + 0.1)*50;
    00821
    00822     //RC2:
    00823     NCIRCLE( "BallLocator:particle", position.x, position.y,
    00824              thickness/2, thickness, Drawings::ps_solid, color);
    00825
    00826     CIRCLE(ballLocatorField, position.x, position.y, thickness,
    00827            2*thickness, Drawings::ps_solid, color);
    00828   }
    00829   else
    00830   {
    00831     Vector2<double> position = Geometry::relative2FieldCoord(robotPose,
    00832       particle.getPosition().x,
    00833       particle.getPosition().y);
    00834     Vector2<double> endPoint(particle.getPosition().x
    00835       + particle.getVelocity().x,
    00836       particle.getPosition().y
    00837       + particle.getVelocity().y);
    00838     endPoint = Geometry::relative2FieldCoord(robotPose, endPoint.x, endPoint.y);
    00839
    00840     double thickness = (particle.probability + 0.1)*20;
    00841
    00842     //RC2:
    00843     NARROW( "BallLocator:particle", position.x, position.y,
    00844             endPoint.x, endPoint.y, thickness/2, Drawings::ps_solid, color);
    00845
    00846     ARROW(ballLocatorField, position.x, position.y,
    00847           endPoint.x, endPoint.y, thickness, Drawings::ps_solid, color);
    00848   }
    00849 }
    00850 //---------------------------------------------------------------------------------------
    00851 void GT2005BallLocator::draw(Drawings::Color particleColor,
    00852                              Drawings::Color estimatedBallColor) const
    00853 {
    00854   NDECLARE_DEBUGDRAWING( "ImageProcessor:multipleBallPercepts",
    00855                          "drawingOnField",
    00856                          "Draws multiple ball percepts on the field.");
    00857   NDECLARE_DEBUGDRAWING( "BallLocator:particle",
    00858                          "drawingOnField",
    00859                          "Draws the particle on the field.");
    00860   NDECLARE_DEBUGDRAWING( "BallLocator:position:position",
    00861                          "drawingOnField",
    00862                          "Draws the estimated ball position.");
    00863   NDECLARE_DEBUGDRAWING( "BallLocator:positionWithVelocity",
    00864                          "drawingOnField",
    00865                          "Draws the estimated ball position and the velocity.");
    00866   NDECLARE_DEBUGDRAWING( "BallLocator:position:lineFromRobotToBall",
    00867                          "drawingOnField",
    00868                          "Draws a line from the robot pose to the estimated ball position.");
    00869
    00870   ////Draw the grid of calculateEstimatedBallPosition:
    00871   //double width = 2*yPosLeftSideline,
    00872   //       height = 2*xPosOpponentGroundline;
    00873   //for (int y = 0; y <= blMaxGrid; ++y)
    00874   //{
    00875   //  int start = static_cast<int>(xPosOwnGroundline
    00876   //    + y*(height/blMaxGrid));
    00877   //  LINE(ballLocatorField, start, yPosRightSideline,
    00878   //       start, yPosLeftSideline, 2, Drawings::ps_solid, Drawings::white);
    00879   //}
    00880   //for (int x = 0; x <= blMaxGrid; ++x)
    00881   //{
    00882   //  int start = static_cast<int>(yPosRightSideline
    00883   //    + x*(width/blMaxGrid));
    00884   //  LINE(ballLocatorField, xPosOwnGroundline, start, xPosOpponentGroundline,
    00885   //       start, 2, Drawings::ps_solid, Drawings::white);
    00886   //}
    00887
    00888   int i;
    00889
    00890   //Call the function drawParticle for every particle with the given color:
    00891   for (i = 0; i < ballLocatorSamples.getNumberOfSamples(); i++)
    00892   {
    00893     drawParticle(ballLocatorSamples[i], particleColor);
    00894   }
    00895
    00896   //The estimated ball is drawn:
    00897   Vector2<double> fieldPosition = Geometry::relative2FieldCoord(robotPose, ballX, ballY);
    00898   NCIRCLE( "BallLocator:position:position",
    00899            fieldPosition.x, fieldPosition.y, 20, 40, Drawings::ps_solid, Drawings::orange);
    00900   NCOLORED_LINE( "BallLocator:position:lineFromRobotToBall",
    00901                  robotPose.translation.x, robotPose.translation.y,
    00902                  fieldPosition.x, fieldPosition.y, 1, Drawings::ps_solid, 255, 170, 0);
    00903
    00904   double vx = ballVX;
    00905   double vy = ballVY;
    00906   double v = vx*vx + vy*vy;
    00907
    00908   if (v <= 50*50)
    00909   {
    00910     Vector2<double> position = Geometry::relative2FieldCoord(robotPose, ballX, ballY);
    00911     double thickness = (ballP + 0.1)*50;
    00912
    00913     //RC2:
    00914     NCIRCLE( "BallLocator:positionWithVelocity",
    00915              position.x, position.y, thickness/2, thickness,
    00916              Drawings::ps_solid, Drawings::orange);
    00917
    00918     CIRCLE(ballLocatorField, position.x, position.y,
    00919            thickness, 2*thickness, Drawings::ps_solid, Drawings::orange);
    00920   }
    00921   else
    00922   {
    00923     Vector2<double> position = Geometry::relative2FieldCoord(robotPose, ballX, ballY);
    00924     Vector2<double> endPoint(ballX + ballVX, ballY + ballVY);
    00925     endPoint = Geometry::relative2FieldCoord(robotPose, endPoint.x, endPoint.y);
    00926
    00927     double thickness = (ballP + 0.1)*20;
    00928
    00929     //RC2:
    00930     NARROW( "BallLocator:positionWithVelocity",
    00931             position.x, position.y, endPoint.x, endPoint.y,
    00932             thickness/2, Drawings::ps_solid, Drawings::orange);
    00933
    00934     ARROW(ballLocatorField, position.x, position.y, endPoint.x, endPoint.y,
    00935           thickness, Drawings::ps_solid, Drawings::orange);
    00936   }
    00937
    00938   DEBUG_DRAWING_FINISHED(ballLocatorField);
    00939
    00940   Vector2<double> perceptOnField;
    00941   for (int i = 0; i < ballPercept.multiplePercepts.numberOfElements; i++)
    00942   {
    00943     perceptOnField = Geometry::relative2FieldCoord
    00944       (robotPose,
    00945        ballPercept.multiplePercepts[i].offsetOnField.x,
    00946        ballPercept.multiplePercepts[i].offsetOnField.y);
    00947
    00948     NCIRCLE( "ImageProcessor:multipleBallPercepts",
    00949              perceptOnField.x, perceptOnField.y,
    00950              20, 40, Drawings::ps_solid, Drawings::yellow);
    00951
    00952     CIRCLE(multipleBallPerceptsField,
    00953            perceptOnField.x, perceptOnField.y,
    00954            28, 40, Drawings::ps_dot, Drawings::yellow);
    00955   }
    00956   DEBUG_DRAWING_FINISHED(multipleBallPerceptsField);
    00957 }
    00958 //---------------------------------------------------------------------------------------