Modules/BallLocator/GT2005BallLocator.cpp

Go to the documentation of this file.

/**
 * @file GT2005BallLocator.cpp
 * Contains a BallLocator implementation using Particle Filters
 *
 * @author <a href="mailto:christinez@gmx.de">Christine Zarges</a>
 * @author <a href="mailto:Thorsten.Kerkhof@gmx.de">Thorsten Kerkhof</a>
 */
//---------------------------------------------------------------------------------------
#include "GT2005BallLocator.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/FieldDimensions.h"
#include "Tools/Debugging/DebugDrawings.h"
//---------------------------------------------------------------------------------------
GT2005BallLocator::GT2005BallLocator(const BallLocatorInterfaces& interfaces)
 : BallLocator(interfaces)
{
  frameNumber = 0;
  lastOdometryData = odometryData;
  currentSystemTime = SystemCall::getCurrentSystemTime();
  lastSystemTime = SystemCall::getCurrentSystemTime();
  ballSeenTime = 0;
  dFieldDiagonalLength = 1.2*sqrt((2.0*xPosOpponentGoal)*(2.0*xPosOpponentGoal) +
                                  (2.0*yPosLeftFlags)*(2.0*yPosLeftFlags));
  
  framesNoBallSeen = 500; //initialized very high...
  lastPositionSeen.x = 0;
  lastPositionSeen.y = 0;

  prevCameraMatrix = cameraMatrix;

  stateIsSet = false;

  /* Use in the ball model the probabilities the ball locator sets
   * (default is a calculation in the ball model itself):
   */
  ballModel.seen.useGivenProbabilities = true;
  ballModel.propagated.useGivenProbabilities = true;

  initializeParticles();
}
//---------------------------------------------------------------------------------------
double GT2005BallLocator::calculateRobotSpeedValue()
{
  double robotSpeed = (motionInfo.executedMotionRequest.walkRequest.walkParams.translation).abs();
  double value(0);
  if(robotSpeed < 200)
    value = - 3 * robotSpeed / 20 + 110;
  else if(robotSpeed < 260)
    value = 80;
  else value = 30 * (robotSpeed - 260) / 40 + 80;
  return 80 / value;
}
//------------------------------------------------------------------------------
void GT2005BallLocator::execute()
{
  modify();

  //Get the current system time and translate it into seconds:
  currentSystemTime = SystemCall::getCurrentSystemTime();
  timeChange = ((double)(currentSystemTime) - (double)(lastSystemTime)) / 1000;

  oldX = ballX;
  oldY = ballY;

  //If the first time the game controller says "set" initialize the particles:
  if ((gameControlData.data.state == GameControlData::set) && (!stateIsSet))
  {
    initializeParticles();
    stateIsSet = true;
  }
  else //!(for the first time game state is "set")
  {
    if (gameControlData.data.state != GameControlData::set)
      stateIsSet = false;

    timeUpdate();

    if (ballPercept.multiplePercepts.numberOfElements == 0)
    {
      //Perhaps without multiple ballPercepts:
      Vector2<double> ballOffset;
      ballOffset = ballPercept.offsetOnField;
      if (ballPercept.ballWasSeen && !(isNonSensePos(ballOffset)))
      {
        double distanceValue = calculateDistanceValue(ballOffset);
        double panningVelocityValue = calculatePanningVelocityValue();
        double robotSpeedValue = calculateRobotSpeedValue();
        measurementUpdate(ballOffset, ballPercept.reliability, distanceValue, panningVelocityValue,
          robotSpeedValue);

        //Update last seen position:
        framesNoBallSeen = 0;
        lastPositionSeen.x = ballOffset.x;
        lastPositionSeen.y = ballOffset.y;
        ballSeenTime = currentSystemTime;
      }
      else //!(ballPercept.ballWasSeen && !(isNonSensePos(ballOffset))
        framesNoBallSeen++;
    }
    else
    {
      Vector2<double> ballOffset;

      //With multiple ball percepts:
      for (int i = 0; i < ballPercept.multiplePercepts.numberOfElements; i++)
      {
        ballOffset = ballPercept.multiplePercepts[i].offsetOnField;
        double distanceValue = ballPercept.multiplePercepts[i].distanceError;
        double panningVelocityValue = ballPercept.multiplePercepts.panningVelocityError;
        double robotSpeedValue = calculateRobotSpeedValue();
        if (!(isNonSensePos(ballOffset)))
          measurementUpdate(ballPercept.multiplePercepts[i].offsetOnField,
                            ballPercept.multiplePercepts[i].reliability,
                            distanceValue,
                            panningVelocityValue,
                            robotSpeedValue);
      }

      //Save the position taken from the percept:
      bool mainPerceptNonsense = false;

      //only the main percept will be stored, if this is non sense,
      //we don't save any percept in this frame
      ballOffset = ballPercept.multiplePercepts[0].offsetOnField;
      if (!(isNonSensePos(ballOffset)))
      {
        //Update last seen position:
        framesNoBallSeen = 0;
        lastPositionSeen.x = ballOffset.x;
        lastPositionSeen.y = ballOffset.y;
        ballSeenTime = currentSystemTime;
      }
      else
        mainPerceptNonsense = true;

      if (mainPerceptNonsense)
        framesNoBallSeen++;
    }

    /* If the ball was dribbled in the last frame the ball position is very
     * similar to the robot pose:
     */
    if (ballHandling.behaviorBallState == BallHandling::dribbleTheBall)
    {
      measurementUpdate(Vector2<double>(50, 0), 0.6, 1.0, 1.0, 1.0);
    }
  }

  ballLocatorSamples.calculateEstimatedBall(ballX, ballY, ballP,
                                            ballVX, ballVY, ballVP);

  setBallSymbols();

  lastSystemTime = currentSystemTime;
  lastOdometryData = odometryData;

  draw(Drawings::dark_gray, //particle color
       Drawings::orange);   //estimated ball color

  noNonsenseParticles();

  frameNumber++;
  prevCameraMatrix = cameraMatrix;
}
//---------------------------------------------------------------------------------------
void GT2005BallLocator::initializeParticles()
{
  /* Particles will be thrown on the field in a close distance to the
   * KickOffPosition:
   */
  for (int i = 0; i < ballLocatorSamples.getNumberOfSamples(); i++)
  {
    double randomValue1 = random();
    double randomValue2 = random();
    ballLocatorSamples[i].setParameters
      (random(2*50) - 50 - robotPose.translation.x, 
       random(2*50) - 50 - robotPose.translation.y,
       randomValue1 * 30, randomValue2 * 30,
       0.5, 0);
  }
  ballX = -robotPose.translation.x;
  ballY = -robotPose.translation.y;
  ballVX = 0;
  ballVY = 0;
  ballP = 0.5;
  ballVP = 0;
}
//---------------------------------------------------------------------------------------
double GT2005BallLocator::getTimeFactor()
{
  if (framesNoBallSeen < 1 * GT2005BallLocatorParameters::timeFactorThreshold)
    return 1;
  if (framesNoBallSeen < 2 * GT2005BallLocatorParameters::timeFactorThreshold)
    return 0.9;
  if (framesNoBallSeen < 3 * GT2005BallLocatorParameters::timeFactorThreshold)
    return 0.8;
  if (framesNoBallSeen < 4 * GT2005BallLocatorParameters::timeFactorThreshold)
    return 0.7;
  if (framesNoBallSeen < 5 * GT2005BallLocatorParameters::timeFactorThreshold)
    return 0.6;
  return 0.5;
}
//---------------------------------------------------------------------------------------
void GT2005BallLocator::timeUpdate()
{
  /* When a timeUpdate is done, the particles have to be "pushed"
   * on the field depending on the odometry data and the speed
   * of the particle.
   */
  Pose2D changeRobot = odometryData - lastOdometryData;

  Vector2<double> c1(cos(changeRobot.rotation),-sin(changeRobot.rotation));
  Vector2<double> c2(sin(changeRobot.rotation),cos(changeRobot.rotation));
  Matrix2x2<double> rotM(c1, c2);
  Vector2<double> n(oldX, oldY);
  n = rotM * n;
  n -= changeRobot.translation;
  oldX = n.x;
  oldY = n.y;

  /* Calculate the velocity out of x- and ychange and the time between the last
   * and the current frame.
   */
  int i;
  for (i = 0; i < ballLocatorSamples.getNumberOfSamples(); i++)
  {
    /* x- and y-Position are updated dependend on the velocity of the ball and
     * the movement of the robot.
     */
    ballLocatorSamples[i].pose = rotM * ballLocatorSamples[i].pose;
    ballLocatorSamples[i].pose.x +=
      ballLocatorSamples[i].vx * timeChange - changeRobot.translation.x;
    ballLocatorSamples[i].pose.y +=
      ballLocatorSamples[i].vy * timeChange - changeRobot.translation.y;

    //Velocity of the ball is decreased as the ball slows down:
    Vector2<double> v = ballLocatorSamples[i].getVelocity();
    v = rotM * v; //rotate velocity vector

    double l = sqrt(v.x * v.x + v.y * v.y);

    /* The breakFactor shows how fast the ball slows down,
     * the function is taken from physics.
     */
    double newLength
      = -(GT2005BallLocatorParameters::coefficientOfFriction * 9810 * timeChange) + l;
    double l2 = 0;
    if(l != 0)
      l2 = (1 / l) * newLength;

    ballLocatorSamples[i].setVelocity(l2 * v.x, l2 * v.y);

    /* Reliability of the particle is decreased depending on the system noise
     * and the time the ball is not seen.
     */
    ballLocatorSamples[i].probability *= (1 - GT2005BallLocatorParameters::systemNoise)
      * getTimeFactor(); //getTimeFactor depends on currentSystemTime
  }
}
//---------------------------------------------------------------------------------------
double GT2005BallLocator::calculateDistanceValue(Vector2<double> ballOffset)
{
  /* "distance to ball"-value:
   * b is equal to 1 if the calculated distance value is 50 (the minimum of the function)
   * the lowest value is 0.045 for a distance of approximately 4000 mm
   */
  double d2 = ballOffset.x*ballOffset.x + ballOffset.y*ballOffset.y;
  double d = sqrt(d2);

  //Function is result of BallLocatorEvolutionBehavior:
  double value;
  if (d < 250)
    value = (-d/12 + 74);
  if (d < 1500)
    value = 50;
  else
    value = (0.000160403*d2 - 0.440147*d + 349.31375);

  return 50 / value;
}
//---------------------------------------------------------------------------------------
double GT2005BallLocator::calculatePanningVelocityValue()
{
  //Probability depending on panning velocity and distance to ball:
  Vector3<double> vectorInDirectionOfViewCamera(1,0,0);
  Vector3<double> vectorInDirectionOfViewWorld,
                  vectorInDirectionOfViewWorldOld;
  vectorInDirectionOfViewWorld = cameraMatrix.rotation
    * vectorInDirectionOfViewCamera;
  vectorInDirectionOfViewWorldOld = prevCameraMatrix.rotation
    * vectorInDirectionOfViewCamera;
  long frameNumber = cameraMatrix.frameNumber, 
       prevFrameNumber = prevCameraMatrix.frameNumber;
  const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
  
  //In seconds:
  double timeDiff = (frameNumber - prevFrameNumber) * r.motionCycleTime;
  
  Vector2<double> currentOnGround(vectorInDirectionOfViewWorld.x,
                                  vectorInDirectionOfViewWorld.y), 
                  oldOnGround(vectorInDirectionOfViewWorldOld.x,
                              vectorInDirectionOfViewWorldOld.y);
  currentOnGround.normalize();
  oldOnGround.normalize();
  double panningVelocity = 0;
  if (timeDiff > 0)
  {
    double cosAng = currentOnGround*oldOnGround;
    if (cosAng > 1.0)
      cosAng = 1.0;
    else if (cosAng < -1.0)
      cosAng = -1.0;
    panningVelocity = fabs(normalize(acos(cosAng))/timeDiff);
  }
  
  /* "panningVelocity"-value:
   * a is equal to 1 if the panning velocity is equal to 0 (minimum of the function)
   * the lowest value is 0.52 for a panning velocity of approximately 5.5
   */

  //Function is result of BallLocatorEvolutionBehavior:
  double value;
  if(panningVelocity > 5.5)
    value = 57.198511908222125;
  else
    value = -0.9 * panningVelocity * panningVelocity
      + 9.90092024377335 * panningVelocity
      + 29.9684505674687;

  return 30 / value;
}
//---------------------------------------------------------------------------------------
void GT2005BallLocator::measurementUpdate(Vector2<double> ballOffset,
                                          double reliability,
                                          double distanceValue,
                                          double panningVelocityValue,
                                          double robotSpeedValue)
{
  /*
   * update positions
   */

  double imageBallProb = (distanceValue + panningVelocityValue + robotSpeedValue) * reliability / 3;

  /* This value counts the number of particles which are inside the circle with
   * the radius timesBallRadius * ballRadius.
   */
  int numPartInside = 0;

  int i;
  for (i = 0; i < ballLocatorSamples.getNumberOfSamples(); i++)
  {
    GT2005Particle &particle = ballLocatorSamples[i];

    double distance =
      Geometry::distanceTo(particle.getPosition(), ballOffset);
    
    if (distance < GT2005BallLocatorParameters::timesBallRadius * ballRadius)
    {
      double oldx = particle.pose.x;
      double oldy = particle.pose.y;

      numPartInside += 1;
      particle.pose.x = (GT2005BallLocatorParameters::timesPartProb * particle.probability
                         * particle.pose.x + distanceValue * ballOffset.x)
        / (GT2005BallLocatorParameters::timesPartProb * particle.probability + distanceValue);
      particle.pose.y = (GT2005BallLocatorParameters::timesPartProb * particle.probability
                         * particle.pose.y + panningVelocityValue * ballOffset.y)
        / (GT2005BallLocatorParameters::timesPartProb * particle.probability
           + panningVelocityValue);
      double probadd = distance/ballRadius + 1;
      probadd = GT2005BallLocatorParameters::nearPerceptValue1 / probadd;
      probadd += particle.probability * GT2005BallLocatorParameters::nearPerceptValue2;
      probadd *= imageBallProb * GT2005BallLocatorParameters::nearPerceptValue3;
      particle.probability = min(1.0, particle.probability + probadd);
      particle.vprob = imageBallProb * GT2005BallLocatorParameters::nearPerceptValue4;
      particle.seenvx = (ballOffset.x - oldx) / timeChange;
      particle.seenvy = (ballOffset.y - oldy) / timeChange;
    }
    else //!(distance < timesBallRadius * ballRadius)
    {
      particle.probability *= (1 - GT2005BallLocatorParameters::measurementNoise);
      particle.probability -= GT2005BallLocatorParameters::farPerceptValue1 * imageBallProb;
      if (particle.probability < 0)
        particle.probability = 0;
      particle.vprob = 0;
      Vector2<double> tmp = particle.getPosition();
      tmp.x += particle.provx * timeChange * 2 - particle.vx * timeChange;
      tmp.y += particle.provy * timeChange * 2 - particle.vy * timeChange;
      double distance2 =
        Geometry::distanceTo(tmp, ballOffset);
      if (distance2 <   GT2005BallLocatorParameters::timesBallRadius * ballRadius
                      * GT2005BallLocatorParameters::farPerceptValue2)
      {
        particle.vprob = 5;
        particle.seenvx = particle.provx;
        particle.seenvy = particle.provy;
      }
      else
      {
        particle.provx = (ballOffset.x - particle.pose.x) / timeChange;
        particle.provy = (ballOffset.y - particle.pose.y) / timeChange;
      }
    }
  }
  
  /* If the number of particles inside the radius timesBallRadius * ballRadius
   * is less than 5 percent of the number of particles we throw as many
   * particles as needed to get more than 5 percent on a new position. This is
   * done by giving them a probability of 0 so that the noNonesenseParticles-
   * function takes care of them.
   */
  while (numPartInside < (  GT2005BallLocatorParameters::percentNumPartInside
                          * ballLocatorSamples.getNumberOfSamples()))
  {
    int index = ballLocatorSamples.getIndexOfParticleWithLowestProb();
    ballLocatorSamples[index].probability = 0;
    numPartInside += 1;
  }

  /*
   * update velocities
   */
  double calcVX;
  double calcVY;
  
  for (i = 0; i < ballLocatorSamples.getNumberOfSamples(); i++)
  {
    GT2005Particle &particle = ballLocatorSamples[i];
    
    if (particle.probability == 0)
      continue;

    calcVX = particle.seenvx;
    calcVY = particle.seenvy;

    double randomValue1 =   (GT2005BallLocatorParameters::randomLimitV/2)
                          - (double)(random((int)GT2005BallLocatorParameters::randomLimitV));
    double randomValue2 =   (GT2005BallLocatorParameters::randomLimitV/2)
                          - (double)(random((int)GT2005BallLocatorParameters::randomLimitV));
    double partprob = GT2005BallLocatorParameters::timesPartProb * particle.probability;
    double improb = particle.vprob * GT2005BallLocatorParameters::velocityWeightXDirection;

    if (partprob + improb == 0)
    {
      particle.vx = 0;
      particle.vy = 0;
      continue;
    }
    else
    {
      particle.vx =
        (partprob * particle.vx + improb * calcVX
        + ((particle.probability + improb)/2) * randomValue1)
        / (partprob + improb);
      improb = particle.vprob;
      particle.vy =
        (partprob * particle.vy + improb * calcVY
        + ((particle.probability + improb)/2) * randomValue2)
        / (partprob + improb);
    }
    
    //Set velocity to 0 if it is NaN: (hopefully does not happen any more)
    if (!((particle.vx > -1000000) && (particle.vx < 1000000)))
    {
      particle.vx = 0;
      if (!((particle.vy > -1000000) && (particle.vy < 1000000)))
      {
        particle.vy = 0;
        continue;
      }
      continue;
    }
    if (!((particle.vy > -1000000) && (particle.vy < 1000000)))
    {
      particle.vy = 0;
      continue;
    }

    //Clipping velocities faster than 1500mm/s:
    if (particle.vx*particle.vx + particle.vy*particle.vy > 1500*1500)
    {
      if (particle.vx > 1500)
        particle.vx = 1500;
      else
        particle.vx /= 2;
      if (particle.vy > 1500)
        particle.vy = 1500;
      else
        particle.vy /= 2;
    }
  }
}
//---------------------------------------------------------------------------------------
void GT2005BallLocator::setBallSymbols()
{
  ballModel.propagated.lastSeenBallState = ballModel.seen;
  ballModel.propagated.setBallDataRelativeToRobot(robotPose,
                                                  ballX, ballY,
                                                  ballVX, ballVY);
  ballModel.propagated.positionProb = ballP;
  ballModel.propagated.velocityProb = ballVP;

  /* Values depending on the existence of a ball in the ball-percept:
   * When the ball was seen, we set the "ballWasSeen"-flag.
   * Every time a ball was seen, we set the "timeUntilSeenConsecutively"-flag
   * to the current system time, so that we always know the last timestamp
   * where we had a ball percept.
   * On the other hand we save the time, when we don't see the ball, so that we
   * have a timestamp for the last time without ball percept.
   */
  if (ballPercept.ballWasSeen)
  {
    ballModel.ballWasSeen = true;
    ballModel.numberOfImagesWithoutBallPercept = 0;
    ballModel.numberOfImagesWithBallPercept += 1;

    ballModel.seen.timeUntilSeenConsecutively = currentSystemTime;
    ballModel.seen.setBallDataRelativeToRobot(robotPose,
                                              lastPositionSeen.x,
                                              lastPositionSeen.y,
                                              ballVX,
                                              ballVY);
    ballModel.seen.timeWhenLastSeen = ballSeenTime;
    ballModel.seen.positionProb = ballPercept.reliability;
    ballModel.seen.velocityProb = ballVP;
  }
  else
  {
    //Update the seen position by odometry:
    Pose2D changeRobot = odometryData - lastOdometryData;

    Vector2<double> c1(cos(changeRobot.rotation),-sin(changeRobot.rotation));
    Vector2<double> c2(sin(changeRobot.rotation),cos(changeRobot.rotation));
    Matrix2x2<double> rotM(c1, c2);
    Vector2<double> n(oldX, oldY);
    n = rotM * n;
    n -= changeRobot.translation;

    Vector2<double> lastSeenPosition = ballModel.seen.positionRobot;
    lastSeenPosition = rotM * lastSeenPosition;
    lastSeenPosition.x -= changeRobot.translation.x;
    lastSeenPosition.y -= changeRobot.translation.y;

    ballModel.seen.setBallDataRelativeToRobot(robotPose,
                                              lastSeenPosition.x,
                                              lastSeenPosition.y,
                                              ballVX, ballVY);

    ballModel.seen.positionProb = max(0.0, ballModel.seen.positionProb * 0.8);
    ballModel.seen.velocityProb = 0;

    ballModel.ballWasSeen = false;
    ballModel.seen.timeWhenFirstSeenConsecutively = currentSystemTime;
    ballModel.numberOfImagesWithoutBallPercept += 1;
    ballModel.numberOfImagesWithBallPercept = 0;
  }

  Player::teamColor ownColor = getPlayer().getTeamColor();
  colorClass opponentGoalColor;
  if (ownColor == Player::red)
    opponentGoalColor = skyblue;
  else
    opponentGoalColor = yellow;
  ballModel.seen.ballInFrontOfOpponentGoal = false;
  ballModel.propagated.ballInFrontOfOpponentGoal = false;
  if (landmarksPercept.numberOfGoals > 0)
  {
    double angleToBall = ballPercept.sizeBasedOffsetOnField.angle();
    double ballDistance = ballPercept.sizeBasedOffsetOnField.abs();
    if(
      landmarksPercept.goals[0].color == opponentGoalColor &&
      ballPercept.ballWasSeen &&
      landmarksPercept.goals[0].x.min < angleToBall &&
      angleToBall < landmarksPercept.goals[0].x.max &&
      ballDistance < 1100
      )
    {
      ballModel.seen.ballInFrontOfOpponentGoal = true;
      ballModel.propagated.ballInFrontOfOpponentGoal = true;
    }
  }
}
//---------------------------------------------------------------------------------------
bool GT2005BallLocator::isNonSensePos(Vector2<double> position)
{
  double distance = position.x*position.x + position.y*position.y;
  if (distance > dFieldDiagonalLength*dFieldDiagonalLength)
  {
    nonsensePos++;
    return true;
  }
  else
    return false;
}
//---------------------------------------------------------------------------------------
void GT2005BallLocator::noNonsenseParticles()
{
  int i;
  for (i = 0; i < ballLocatorSamples.getNumberOfSamples(); i++)
  {
    GT2005Particle &particle = ballLocatorSamples[i];
    double distance = particle.pose.x*particle.pose.x
      + particle.pose.y*particle.pose.y;
    if (distance > (dFieldDiagonalLength*dFieldDiagonalLength + 100))
      particle.probability -= GT2005BallLocatorParameters::particleProbDecreaseStep;

    if (particle.probability < GT2005BallLocatorParameters::particleProbThreshold)
    {
      double fNBSP = getTimeFactor() / 2; //framesNoBallSeenProbability
      double randomValue = random();
      randomValue *= (fNBSP + ballP);
      double xPosRel;
      double yPosRel;
      if (randomValue <= fNBSP)
      {
        /* Throw gaussian around the lastPositionSeen.
         * (method of Marsaglia, first for x, then for y)
         */
        double v = 2;
        double u1 = 0;
        double u2 = 0;
        int count = 0;
        while (v >= 1)
        {
          u1 = random();
          u2 = random();
          v = (2*u1 - 1)*(2*u1 - 1) + (2*u2 - 1)*(2*u2 - 1);
          if ((v >= 1) && (count++ > 10))
            v = 0.9;
        }
        double xRand =   GT2005BallLocatorParameters::timesBallRadius * ballRadius
                       * sqrt(fNBSP) * ((2*u1 - 1) * sqrt(-2*log(v)/v));
        v = 2;
        count = 0;
        while (v >= 1)
        {
          u1 = random();
          u2 = random();
          v = (2*u1 - 1)*(2*u1 - 1) + (2*u2 - 1)*(2*u2 - 1);
          if ((v >= 1) && (count++ > 10))
            v = 0.9;
        }
        double yRand =   GT2005BallLocatorParameters::timesBallRadius * ballRadius
                       * sqrt(fNBSP) * ((2*u1 - 1) * sqrt(-2*log(v)/v));

        xPosRel = lastPositionSeen.x + xRand;
        yPosRel = lastPositionSeen.y + yRand;
      }
      else //!(randomValue < fNBSP)
      {
        /* Throw gaussian around the estimatedBallSeen.
         * (method of Marsaglia, first for x, then for y)
         */
        double v = 2;
        double u1 = 0;
        double u2 = 0;
        int count = 0;
        while (v >= 1)
        {
          u1 = random();
          u2 = random();
          v = (2*u1 - 1)*(2*u1 - 1) + (2*u2 - 1)*(2*u2 - 1);
          if ((v >= 1) && (count++ > 10))
            v = 0.9;
        }
        double xRand =   GT2005BallLocatorParameters::timesBallRadius
                       * ballRadius * sqrt(fNBSP) * ((2*u1 - 1) * sqrt(-2*log(v)/v));
        v = 2;
        count = 0;
        while (v >= 1)
        {
          u1 = random();
          u2 = random();
          v = (2*u1 - 1)*(2*u1 - 1) + (2*u2 - 1)*(2*u2 - 1);
          if ((v >= 1) && (count++ > 10))
            v = 0.9;
        }
        double yRand =   GT2005BallLocatorParameters::timesBallRadius * ballRadius
                       * sqrt(fNBSP) * ((2*u1 - 1) * sqrt(-2*log(v)/v));
        xPosRel = ballX + xRand;
        yPosRel = ballY + yRand;
      }
      //Clipping points outside the field:
      Vector2<double> posAbs = Geometry::relative2FieldCoord(robotPose, xPosRel, yPosRel);
      if (posAbs.x > xPosOpponentGroundline + 250)
      {
        Vector2<double> newPos = Geometry::fieldCoord2Relative(robotPose,
          Vector2<double>(xPosOpponentGroundline, posAbs.y));
        xPosRel = newPos.x;
        yPosRel = newPos.y;
        posAbs.x = xPosOpponentGroundline;
      }
      else if (posAbs.x < xPosOwnGroundline - 250)
      {
        Vector2<double> newPos = Geometry::fieldCoord2Relative(robotPose,
          Vector2<double>(xPosOwnGroundline, posAbs.y));
        xPosRel = newPos.x;
        yPosRel = newPos.y;
        posAbs.x = xPosOwnGroundline;
      }
      if (posAbs.y > yPosLeftSideline + 250)
      {
        Vector2<double> newPos = Geometry::fieldCoord2Relative(robotPose,
          Vector2<double>(posAbs.x, yPosLeftSideline));
        xPosRel = newPos.x;
        yPosRel = newPos.y;
      }
      else if (posAbs.y < yPosRightSideline - 250)
      {
        Vector2<double> newPos = Geometry::fieldCoord2Relative(robotPose,
          Vector2<double>(posAbs.x, yPosRightSideline));
        xPosRel = newPos.x;
        yPosRel = newPos.y;
      }

      double randomValue1 = random();
      double randomValue2 = random();

      //Throw new particle:
      particle.setParameters(xPosRel, yPosRel,
                             randomValue1 * 50, randomValue2 * 50,
                             GT2005BallLocatorParameters::throwInProb, 0);
    }
  }
}
//---------------------------------------------------------------------------------------
bool GT2005BallLocator::handleMessage(InMessage& message)
{
  switch(message.getMessageID())
  {
  case idGT2005BallLocator1:
    {
      GenericDebugData d;
      message.bin >> d;
      GT2005BallLocatorParameters::systemNoise = d.data[0] / 100;
      GT2005BallLocatorParameters::measurementNoise = d.data[1] / 100;
      GT2005BallLocatorParameters::coefficientOfFriction = d.data[2] / 1000;
      GT2005BallLocatorParameters::timesPartProb = d.data[3];
      GT2005BallLocatorParameters::particleProbThreshold = d.data[4] / 100;
      GT2005BallLocatorParameters::timesBallRadius = d.data[5];
      GT2005BallLocatorParameters::randomLimitV = d.data[6];
      GT2005BallLocatorParameters::timeFactorThreshold = d.data[7];
      GT2005BallLocatorParameters::throwInProb = d.data[8] / 100;
    }
    return true;
    break;
  case idGT2005BallLocator2:
    {
      GenericDebugData d;
      message.bin >> d;
      GT2005BallLocatorParameters::velocityWeightXDirection = d.data[0] / 100;
      GT2005BallLocatorParameters::percentNumPartInside = d.data[1] / 100;
      GT2005BallLocatorParameters::particleProbDecreaseStep = d.data[2] / 100;
      GT2005BallLocatorParameters::nearPerceptValue1 = d.data[3] / 100;
      GT2005BallLocatorParameters::nearPerceptValue2 = d.data[4] / 100;
      GT2005BallLocatorParameters::nearPerceptValue3 = d.data[5] / 100;
      GT2005BallLocatorParameters::nearPerceptValue4 = d.data[6] / 100;
      GT2005BallLocatorParameters::farPerceptValue1 = d.data[7] / 100;
      GT2005BallLocatorParameters::farPerceptValue2 = d.data[8] / 100;
    }
    return true;
    break;
  }
  return false;
}
//---------------------------------------------------------------------------------------
void GT2005BallLocator::modify()
{
  MODIFY("BallLocator:coefficientOfFriction",
         GT2005BallLocatorParameters::coefficientOfFriction);
  MODIFY("BallLocator:measurementNoise",
         GT2005BallLocatorParameters::measurementNoise);
  MODIFY("BallLocator:systemNoise",
         GT2005BallLocatorParameters::systemNoise);
  MODIFY("BallLocator:particleProbThreshold",
         GT2005BallLocatorParameters::particleProbThreshold);
  MODIFY("BallLocator:timesBallRadius",
         GT2005BallLocatorParameters::timesBallRadius);
  MODIFY("BallLocator:timeFactorThreshold", 
         GT2005BallLocatorParameters::timeFactorThreshold);
  MODIFY("BallLocator:throwInProb",
         GT2005BallLocatorParameters::throwInProb);
  MODIFY("BallLocator:timesPartProb",
         GT2005BallLocatorParameters::timesPartProb);
  MODIFY("BallLocator:randomLimitV",
         GT2005BallLocatorParameters::randomLimitV);
  MODIFY("BallLocator:velocityWeightXDirection",
         GT2005BallLocatorParameters::velocityWeightXDirection);
  MODIFY("BallLocator:percentNumPartInside",
         GT2005BallLocatorParameters::percentNumPartInside);
  MODIFY("BallLocator:particleProbDecreaseStep",
         GT2005BallLocatorParameters::particleProbDecreaseStep);
  MODIFY("BallLocator:nearPerceptValue1",
         GT2005BallLocatorParameters::nearPerceptValue1);
  MODIFY("BallLocator:nearPerceptValue2",
         GT2005BallLocatorParameters::nearPerceptValue2);
  MODIFY("BallLocator:nearPerceptValue3",
         GT2005BallLocatorParameters::nearPerceptValue3);
  MODIFY("BallLocator:nearPerceptValue4",
         GT2005BallLocatorParameters::nearPerceptValue4);
  MODIFY("BallLocator:farPerceptValue1",
         GT2005BallLocatorParameters::farPerceptValue1);
  MODIFY("BallLocator:farPerceptValue2",
         GT2005BallLocatorParameters::farPerceptValue2);
}
//---------------------------------------------------------------------------------------
void GT2005BallLocator::drawParticle(const GT2005Particle& particle,
                                     Drawings::Color color) const
{ 
  double vx = particle.getVelocity().x;
  double vy = particle.getVelocity().y;
  double v = vx*vx + vy*vy;
  
  if(v <= 50*50)
  {
    Vector2<double> position = Geometry::relative2FieldCoord(robotPose, 
      particle.getPosition().x,
      particle.getPosition().y);
    double thickness = (particle.probability + 0.1)*50;
    
    //RC2:
    NCIRCLE( "BallLocator:particle", position.x, position.y,
             thickness/2, thickness, Drawings::ps_solid, color);

    CIRCLE(ballLocatorField, position.x, position.y, thickness,
           2*thickness, Drawings::ps_solid, color);
  }
  else
  {
    Vector2<double> position = Geometry::relative2FieldCoord(robotPose, 
      particle.getPosition().x,
      particle.getPosition().y);
    Vector2<double> endPoint(particle.getPosition().x 
      + particle.getVelocity().x,
      particle.getPosition().y 
      + particle.getVelocity().y);
    endPoint = Geometry::relative2FieldCoord(robotPose, endPoint.x, endPoint.y);

    double thickness = (particle.probability + 0.1)*20;
    
    //RC2:
    NARROW( "BallLocator:particle", position.x, position.y,
            endPoint.x, endPoint.y, thickness/2, Drawings::ps_solid, color);

    ARROW(ballLocatorField, position.x, position.y,
          endPoint.x, endPoint.y, thickness, Drawings::ps_solid, color);
  }
}
//---------------------------------------------------------------------------------------
void GT2005BallLocator::draw(Drawings::Color particleColor, 
                             Drawings::Color estimatedBallColor) const
{
  NDECLARE_DEBUGDRAWING( "ImageProcessor:multipleBallPercepts",
                         "drawingOnField",
                         "Draws multiple ball percepts on the field.");
  NDECLARE_DEBUGDRAWING( "BallLocator:particle",
                         "drawingOnField",
                         "Draws the particle on the field.");
  NDECLARE_DEBUGDRAWING( "BallLocator:position:position",
                         "drawingOnField",
                         "Draws the estimated ball position.");
  NDECLARE_DEBUGDRAWING( "BallLocator:positionWithVelocity",
                         "drawingOnField",
                         "Draws the estimated ball position and the velocity.");
  NDECLARE_DEBUGDRAWING( "BallLocator:position:lineFromRobotToBall",
                         "drawingOnField",
                         "Draws a line from the robot pose to the estimated ball position.");

  ////Draw the grid of calculateEstimatedBallPosition:
  //double width = 2*yPosLeftSideline,
  //       height = 2*xPosOpponentGroundline;
  //for (int y = 0; y <= blMaxGrid; ++y)
  //{
  //  int start = static_cast<int>(xPosOwnGroundline
  //    + y*(height/blMaxGrid));
  //  LINE(ballLocatorField, start, yPosRightSideline,
  //       start, yPosLeftSideline, 2, Drawings::ps_solid, Drawings::white);
  //}
  //for (int x = 0; x <= blMaxGrid; ++x)
  //{
  //  int start = static_cast<int>(yPosRightSideline
  //    + x*(width/blMaxGrid));
  //  LINE(ballLocatorField, xPosOwnGroundline, start, xPosOpponentGroundline,
  //       start, 2, Drawings::ps_solid, Drawings::white);
  //}

  int i;

  //Call the function drawParticle for every particle with the given color:
  for (i = 0; i < ballLocatorSamples.getNumberOfSamples(); i++)
  {
    drawParticle(ballLocatorSamples[i], particleColor);
  }

  //The estimated ball is drawn:
  Vector2<double> fieldPosition = Geometry::relative2FieldCoord(robotPose, ballX, ballY);
  NCIRCLE( "BallLocator:position:position",
           fieldPosition.x, fieldPosition.y, 20, 40, Drawings::ps_solid, Drawings::orange);
  NCOLORED_LINE( "BallLocator:position:lineFromRobotToBall",
                 robotPose.translation.x, robotPose.translation.y,
                 fieldPosition.x, fieldPosition.y, 1, Drawings::ps_solid, 255, 170, 0);

  double vx = ballVX;
  double vy = ballVY;
  double v = vx*vx + vy*vy;
  
  if (v <= 50*50)
  {
    Vector2<double> position = Geometry::relative2FieldCoord(robotPose, ballX, ballY);
    double thickness = (ballP + 0.1)*50;
    
    //RC2:
    NCIRCLE( "BallLocator:positionWithVelocity",
             position.x, position.y, thickness/2, thickness,
             Drawings::ps_solid, Drawings::orange);

    CIRCLE(ballLocatorField, position.x, position.y,
           thickness, 2*thickness, Drawings::ps_solid, Drawings::orange);
  }
  else
  {
    Vector2<double> position = Geometry::relative2FieldCoord(robotPose, ballX, ballY);
    Vector2<double> endPoint(ballX + ballVX, ballY + ballVY);
    endPoint = Geometry::relative2FieldCoord(robotPose, endPoint.x, endPoint.y);

    double thickness = (ballP + 0.1)*20;
    
    //RC2:
    NARROW( "BallLocator:positionWithVelocity",
            position.x, position.y, endPoint.x, endPoint.y,
            thickness/2, Drawings::ps_solid, Drawings::orange);

    ARROW(ballLocatorField, position.x, position.y, endPoint.x, endPoint.y,
          thickness, Drawings::ps_solid, Drawings::orange);
  }

  DEBUG_DRAWING_FINISHED(ballLocatorField);

  Vector2<double> perceptOnField;
  for (int i = 0; i < ballPercept.multiplePercepts.numberOfElements; i++)
  {
    perceptOnField = Geometry::relative2FieldCoord
      (robotPose,
       ballPercept.multiplePercepts[i].offsetOnField.x,
       ballPercept.multiplePercepts[i].offsetOnField.y);

    NCIRCLE( "ImageProcessor:multipleBallPercepts",
             perceptOnField.x, perceptOnField.y,
             20, 40, Drawings::ps_solid, Drawings::yellow);

    CIRCLE(multipleBallPerceptsField,
           perceptOnField.x, perceptOnField.y,
           28, 40, Drawings::ps_dot, Drawings::yellow);
  }
  DEBUG_DRAWING_FINISHED(multipleBallPerceptsField);
}
//---------------------------------------------------------------------------------------

---