Modules/TeamBallLocator/GT2005TeamBallLocator.cpp

Go to the documentation of this file.

/**
* @file GT2005TeamBallLocator.cpp
* 
* Implementation of class GT2005TeamBallLocator
*
* @author <a href="mailto:christinez@gmx.de">Christine Zarges</a>
* @author <a href="mailto:Thorsten.Kerkhof@gmx.de">Thorsten Kerkhof</a>
*/
//---------------------------------------------------------------------------------------
#include "GT2005TeamBallLocator.h"
#include "Platform/SystemCall.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "Tools/Math/Matrix2x2.h"
#include "Modules/BallLocator/GT2005BallLocatorParameters.h"
//---------------------------------------------------------------------------------------
double GT2005TeamBallLocator::smallestProb = 0.05;
double GT2005TeamBallLocator::biggestProb = 0.10;
int GT2005TeamBallLocator::numberOfCells = 0;
double maxDistTimes = 5;
double decreaseProbValue = 0.9;
//---------------------------------------------------------------------------------------
GT2005TeamBallLocator::GT2005TeamBallLocator(const TeamBallLocatorInterfaces& interfaces)
: TeamBallLocator(interfaces)
{
  lastOdometryData = odometryData;
  timeFactorThreshold = 8;
 
  communicatedBallState.useGivenProbabilities = true;

  framesNothingReceived[0] = 0;
  framesNothingReceived[1] = 0;
  framesNothingReceived[2] = 0;
}
//---------------------------------------------------------------------------------------
void GT2005TeamBallLocator::execute()
{
  MODIFY("TeamBallLocator:parameters", maxDistTimes);
  MODIFY("TeamBallLocator:parameters", decreaseProbValue);

  lastOdometryData = odometryData;
  int i, j;

  //Send particles: (only if not penalized)
  GT2005ParticleContainerSend sendingContainer;
  if (gameControlData.getRobot().penalty == GameControlData::notPenalized)
  {
    calcRepresentativeParticleContainer(
      ballLocatorSamples, //Particles of the GT2005BallLocator
      sendingContainer);
    DEBUG_RESPONSE_NOT("TeamBallLocator:doNotsendParticles",
                       teamMessageCollection.send(sendingContainer););
  }
  
  //GT2005ParticleContainerReceived receivingContainer(4*numOfBLParticlesSend);
  //Receive particles:
  double currentSystemTime = (double) SystemCall::getCurrentSystemTime();
  bool receivedFromRobot[3] = {false, false, false};
  DEBUG_RESPONSE_NOT
  ( "TeamBallLocator:Put particles to point x=500 y=0",
    for (i = 0; i < teamMessageCollection.numberOfTeamMessages; i++)
      if (teamMessageCollection[i].isActual())
      {
        int times = 0;
        switch(teamMessageCollection[i].playerNumberOfSender)
        {
        case Player::one:   times = 0; break;
        case Player::two:   times = 1; break;
        case Player::three: times = 2; break;
        case Player::four:  times = 3; break;
        }
        int sender = times > getPlayer().getPlayerNumber() ? times - 1 : times;

        double difference =   currentSystemTime
                            - teamMessageCollection[i].getTimeStampInOwnTime();

        int numberOfSamples
          = teamMessageCollection[i].gt2005ParticleContainerSend.getNumberOfSamples();

        GT2005ParticleContainerReceived tmpReceivingContainer(numberOfSamples);
        bool receivedInformation = false;
        for (int k = 0; k < numberOfSamples; k++)
        {
          if (((double)(teamMessageCollection[i].gt2005ParticleContainerSend[k].probability))
              / 10000 < 0.005)
          {
            receivedInformation = true;
          }
        }

        if (receivedInformation)
          for (j = 0; j < numberOfSamples; j++)
          {
            receivingContainer.current[(int)(j + times*numberOfSamples)].setParameters
              (teamMessageCollection[i].gt2005ParticleContainerSend[j].x,
              teamMessageCollection[i].gt2005ParticleContainerSend[j].y,
              teamMessageCollection[i].gt2005ParticleContainerSend[j].vx,
              teamMessageCollection[i].gt2005ParticleContainerSend[j].vy,
              (double)(teamMessageCollection[i].gt2005ParticleContainerSend[j].probability) / 10000,
              (double)(teamMessageCollection[i].gt2005ParticleContainerSend[j].vprob) / 10000);
            timeUpdateReceived
              (&receivingContainer.current[(int)(j + times*numberOfSamples)], difference);
            receivedFromRobot[sender] = true;
            framesNothingReceived[sender] = 0;
          }
        else
          receivedFromRobot[sender] = false;
      }

    /* For particles belonging to team mates of which no particles were received
     * make a special timeUpdate (this peace of code is not nice, sorry...):
     */
    for (int sender = 0; sender < 3; sender++)
      if (!receivedFromRobot[sender])
      {
        /* To make spreading more efficient, resample particles to really use
         * the maximum number of particles.
         */
        //Get the number of unused particles and put them to the end:
        int numberOfUnusedParticles = 0;
        int senderRobotNumber = sender > getPlayer().getPlayerNumber() - 1 ? sender + 1 : sender;
        for (j = 0; j < numOfBLParticlesSend - numberOfUnusedParticles; j++)
        {
          if (receivingContainer.current[j + senderRobotNumber*numOfBLParticlesSend].probability == 0)
          {
            //last particle not already treated:
            GT2005Particle& p = receivingContainer.current[  numOfBLParticlesSend - 1
                                                           + senderRobotNumber*numOfBLParticlesSend
                                                           - numberOfUnusedParticles];
            receivingContainer.current[j + senderRobotNumber*numOfBLParticlesSend]
              .setParameters(p.pose.x, p.pose.y, p.vx, p.vy, p.probability, p.vprob);
            p.setParameters(0, 0, 0, 0, 0, 0);
            numberOfUnusedParticles++;
            j--;
          }
        }

        //If there is no used particle do nothing:
        if (numberOfUnusedParticles == numOfBLParticlesSend)
          continue;

        /* Resample the particles with highest probability until no unused
        * particle is left.
        */
        for (j = 0; j < numberOfUnusedParticles; j++)
        {
          //Find particle with highest probability:
          GT2005Particle& bigProbParticle
            = receivingContainer.current[senderRobotNumber*numOfBLParticlesSend];
          for (int k = 1; k < numOfBLParticlesSend; k++)
          {
            if (bigProbParticle.probability
              < receivingContainer.current[k + senderRobotNumber*numOfBLParticlesSend].probability)
            {
              bigProbParticle
                = receivingContainer.current[k + senderRobotNumber*numOfBLParticlesSend];
            }
          }
          //Resample this particle if the probability is not too low:
          if (bigProbParticle.probability > 0.04)
          {
            receivingContainer
              .current[numOfBLParticlesSend - 1 + senderRobotNumber*numOfBLParticlesSend - j]
                .setParameters(bigProbParticle.pose.x,
                              bigProbParticle.pose.y,
                              bigProbParticle.vx,
                              bigProbParticle.vy,
                              bigProbParticle.probability / 2,
                              bigProbParticle.vprob);
            bigProbParticle.probability /= 2;
          }
          else
            break;
        }
        framesNothingReceived[sender]++;
        for (j = 0; j < numOfBLParticlesSend; j++)
          timeUpdateNotReceived
            (&receivingContainer.current[j + senderRobotNumber*numOfBLParticlesSend],
            framesNothingReceived[sender]);
      }
  );

  DEBUG_RESPONSE
  ( "TeamBallLocator:Put particles to point x=500 y=0",
    for (int i = 0; i < receivingContainer.getNumberOfSamples(); i++)
      receivingContainer.current[i].setParameters(500, 0, 0, 0, 0.5, 0);
  );


  //Copy own particles into the receivingContainer:
  int times = 0;
  if (gameControlData.getRobot().penalty == GameControlData::notPenalized)
  {
    switch (getPlayer().getPlayerNumber())
    {
    case Player::one:   times = 0; break;
    case Player::two:   times = 1; break;
    case Player::three: times = 2; break;
    case Player::four:  times = 3; break;
    }
    for (j = 0; j < sendingContainer.getNumberOfSamples(); j++)
    {
      receivingContainer.current
        [(int)(j + times*sendingContainer.getNumberOfSamples())]
          .setParameters(
            sendingContainer[j].x,
            sendingContainer[j].y,
            sendingContainer[j].vx,
            sendingContainer[j].vy,
            (double)(sendingContainer[j].probability) / 10000,
            (double)(sendingContainer[j].vprob) / 10000);
    }
  }

  double ballX, ballY, ballP;
  receivingContainer.calculateEstimatedBallPosition(ballX, ballY, ballP);

  /* If the ball was seen by this robot or the communicated ball position
   * has a high validity (mostly when other team mates have seen the ball)
   * set timeWhenLastObserved at current system time.
   */
  if (ballP > 0.01)
  {
    communicatedBallState.timeWhenLastObserved = SystemCall::getCurrentSystemTime();
    communicatedBallState.setBallDataRelativeToField(robotPose,
                                                     ballX, ballY,
                                                     0, 0);
  }

  communicatedBallState.positionProb = ballP;
  communicatedBallState.velocityProb = 0;

  draw(receivingContainer,   //the container with the particles which should be drawed
       ballX,                //the x-position of the estimated ball
       ballY,                //the y-position of the estimated ball
       ballP,                //the probability of the estimated ball position
       Drawings::light_gray, //the color of the particles
       Drawings::pink);      //the color of the estimated ball
}
//---------------------------------------------------------------------------------------
void GT2005TeamBallLocator::calcRepresentativeParticleContainer(
  GT2005ParticleContainer& oldContainer,
  GT2005ParticleContainerSend& newContainer)
{
  int i;
  //root composite cell, the field is divided into 4 cells:
  CompositeCell compositeCell(Vector2<double>(xPosOpponentGroundline,
                                              yPosLeftSideline),
                              Vector2<double>(xPosOwnGroundline,
                                              yPosRightSideline),
                              0);
  numberOfCells = 4;
  //add all particles to the composite cell:
  if (robotPoseCollection[0].getValidity() == 0)
  {
    for (i = 0; i < oldContainer.getNumberOfSamples(); i++)
    {
      Vector2<double> position = Geometry::relative2FieldCoord(robotPose,
                                                               oldContainer[i].pose.x,
                                                               oldContainer[i].pose.y);
      GT2005Particle *particle = new GT2005Particle(position.x, position.y,
                                                    0, 0,
                                                    oldContainer[i].probability, 0);
      compositeCell.addParticle(particle);
    }
  }
  else
  {
    //get the validities of the robot poses and normalize them:
    Vector3<double> validities;
    if (robotPoseCollection.poses[0].getValidity() > 0)
      validities.x = robotPoseCollection.poses[0].getValidity();
    else
      validities.x = 0;
    if (robotPoseCollection.poses[1].getValidity() > 0)
      validities.y = robotPoseCollection.poses[1].getValidity();
    else
      validities.y = 0;
    if (robotPoseCollection.poses[2].getValidity() > 0)
      validities.z = robotPoseCollection.poses[2].getValidity();
    else
      validities.z = 0;
    validities.normalize();
    double robotPoseValidity = max(0.1, robotPose.getValidity());

    for (i = 0; i < oldContainer.getNumberOfSamples(); i++)
    {
      //throw particles for all robot poses:
      { //first robot pose
        Vector2<double> position = Geometry::relative2FieldCoord(
          robotPoseCollection.poses[0], oldContainer[i].pose.x, oldContainer[i].pose.y);
        GT2005Particle* particle
          = new GT2005Particle(position.x, position.y, 0, 0, 0, 0);
        particle->probability = max(0.01,   oldContainer[i].probability
                                          * validities.x * robotPoseValidity);
        DEBUG_RESPONSE
        ( "TeamBallLocator:output particle.probability",
          OUTPUT(idText, text, "particle x probability " << particle->probability);
        );
        compositeCell.addParticle(particle);
      }
      { //second robot pose
        Vector2<double> position = Geometry::relative2FieldCoord(
          robotPoseCollection.poses[1], oldContainer[i].pose.x, oldContainer[i].pose.y);
        GT2005Particle* particle
          = new GT2005Particle(position.x, position.y, 0, 0, 0, 0);
        particle->probability = max(0.01,   oldContainer[i].probability
                                          * validities.y * robotPoseValidity);
        DEBUG_RESPONSE
        ( "TeamBallLocator:output particle.probability",
          OUTPUT(idText, text, "particle y probability " << particle->probability);
        );
        compositeCell.addParticle(particle);
      }
      { //third robot pose
        Vector2<double> position = Geometry::relative2FieldCoord(
          robotPoseCollection.poses[2], oldContainer[i].pose.x, oldContainer[i].pose.y);
        GT2005Particle* particle
          = new GT2005Particle(position.x, position.y, 0, 0, 0, 0);
        particle->probability = max(0.01,   oldContainer[i].probability
                                          * validities.z * robotPoseValidity);
        DEBUG_RESPONSE
        ( "TeamBallLocator:output particle.probability",
          OUTPUT(idText, text, "particle z probability " << particle->probability);
        );
        compositeCell.addParticle(particle);
      }
    }
  }

  //in the queue all cells with probability greater than biggestProb will be saved:
  Queue queue;
  
  //the cells of the composite cell with too low probability are destroyed:
  if (compositeCell.cellDownLeft->getSumOfProb()
      < (smallestProb * compositeCell.getSumOfProb()))
  {
    compositeCell.cellDownLeft->violentBrutalDestruction();
    delete compositeCell.cellDownLeft;
    compositeCell.cellDownLeft = 0;
    numberOfCells--;
  }
  else
    queue.insertCell(compositeCell.cellDownLeft);

  if (compositeCell.cellDownRight->getSumOfProb()
      < (smallestProb * compositeCell.getSumOfProb()))
  {
    compositeCell.cellDownRight->violentBrutalDestruction();
    delete compositeCell.cellDownRight;
    compositeCell.cellDownRight = 0;
    numberOfCells--;
  }
  else
    queue.insertCell(compositeCell.cellDownRight);

  if (compositeCell.cellTopLeft->getSumOfProb()
      < (smallestProb * compositeCell.getSumOfProb()))
  {
    compositeCell.cellTopLeft->violentBrutalDestruction();
    delete compositeCell.cellTopLeft;
    compositeCell.cellTopLeft = 0;
    numberOfCells--;
  }
  else
    queue.insertCell(compositeCell.cellTopLeft);

  if (compositeCell.cellTopRight->getSumOfProb()
      < (smallestProb * compositeCell.getSumOfProb()))
  {
    compositeCell.cellTopRight->violentBrutalDestruction();
    delete compositeCell.cellTopRight;
    compositeCell.cellTopRight = 0;
    numberOfCells--;
  }
  else
    queue.insertCell(compositeCell.cellTopRight);

  //the cells with too much probability are splitted:
  while ((numberOfCells <= 9) && (!(queue.first == 0)))
  {
    AbstractCell* tmp = queue.first;
    queue.first = queue.first->next;

    //if the depth is too deep, do not sub-divide this cell further:
    if (tmp->depth >= 5)
      continue;
    
    CompositeCell* cc = 0;
    CompositeCell* parC = (CompositeCell*)(tmp->parent);
    parC->split((BasicCell*)(tmp), &cc);
    if (!(cc->cellDownLeft == 0) && (cc->cellDownLeft->getSumOfProb()
                                     > (biggestProb * cc->getSumOfProb())))
    {
      queue.insertCell(cc->cellDownLeft);
    }
    if (!(cc->cellDownRight == 0) && (cc->cellDownRight->getSumOfProb()
                                      > (biggestProb*cc->getSumOfProb())))
    {
      queue.insertCell(cc->cellDownRight);
    }
    if (!(cc->cellTopRight == 0) && (cc->cellTopRight->getSumOfProb()
                                     > (biggestProb * cc->getSumOfProb())))
    {
      queue.insertCell(cc->cellTopRight);
    }
    if (!(cc->cellTopLeft == 0) && (cc->cellTopLeft->getSumOfProb()
                                    > (biggestProb * cc->getSumOfProb())))
    {
      queue.insertCell(cc->cellTopLeft);
    }
  }

  GT2005Particle* first = 0;
  compositeCell.getParticles(&first);
  GT2005Particle* johnny = 0;
  i = 0;
  for (GT2005Particle* walker = first; !(walker == 0); walker = walker->next)
  {
    if (walker->probability > GT2005BallLocatorParameters::throwInProb)
    {
      newContainer[i++].setParameters(
        (short int)(walker->pose.x), (short int)(walker->pose.y),
        0, 0, (unsigned short int)(walker->probability * 10000), 0);
    }
    //delete the particle of the step before:
    if (!(johnny == 0))
      delete johnny;
    johnny = walker;
  }
  //delete the last particle:
  if (!(johnny == 0))
    delete johnny;

  //Destruct all cells:
  compositeCell.violentBrutalDestruction();
}
//---------------------------------------------------------------------------------------
void GT2005TeamBallLocator::timeUpdateNotReceived(GT2005Particle* particle,
                                                  int& framesNotSeen)
{
  /* Move the particle into a random direction. How far it is moved depends on the
   * number of frames the particle is old. (gaussian distribution, method of Marsaglia,
   * first for x, then for y)
   */
  double maxDist = framesNotSeen*maxDistTimes;
  double v = 2, u1 = 0, 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;
  }
  particle->pose.x += maxDist * (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;
  }
  particle->pose.y += maxDist * ((2*u1 - 1) * sqrt(-2*log(v)/v));

  //Decrease the probability (faster than in the normal timeUpdate)
  particle->probability -= decreaseProbValue;
  //if (particle->probability < 0.005)
  //  particle->probability = 0;
}
//---------------------------------------------------------------------------------------
void GT2005TeamBallLocator::timeUpdateReceived(GT2005Particle* particle, double& difference)
{
  particle->probability *= getTimeFactor(difference);
}
//---------------------------------------------------------------------------------------
double GT2005TeamBallLocator::getTimeFactor(double& difference)
{
  if (difference < 1 * timeFactorThreshold)
    return 1;
  if (difference < 2 * timeFactorThreshold)
    return 0.9;
  if (difference < 3 * timeFactorThreshold)
    return 0.8;
  if (difference < 4 * timeFactorThreshold)
    return 0.7;
  if (difference < 5 * timeFactorThreshold)
    return 0.6;
  return 0.5;
}
//---------------------------------------------------------------------------------------
bool GT2005TeamBallLocator::handleMessage(InMessage& message)
{
  switch(message.getMessageID())
  {
  case idGT2005TeamBallLocator:
    {
      GenericDebugData d;
      message.bin >> d;
      smallestProb = d.data[0] / 100;
      biggestProb = d.data[1] / 100;
      timeFactorThreshold = d.data[4];
    }
    break;
  }
  return false;
}
//---------------------------------------------------------------------------------------
void GT2005TeamBallLocator::drawParticle(const GT2005Particle& particle,
                                         Drawings::Color color) const
{
  if (particle.probability == 0)
    return;

  double thickness = (particle.probability + 0.1)*50;
  
  NCIRCLE("TeamBallLocator:particles", particle.pose.x, particle.pose.y,
          thickness, 2*thickness, Drawings::ps_solid, color);
  CIRCLE(teamBallLocatorFieldParticles, particle.pose.x, particle.pose.y,
         thickness, 2*thickness, Drawings::ps_solid, color);
}
//---------------------------------------------------------------------------------------
void GT2005TeamBallLocator::draw(const GT2005ParticleContainerReceived& container,
                                 const double& ballX,
                                 const double& ballY,
                                 const double& ballP,
                                 Drawings::Color particleColor, 
                                 Drawings::Color estimatedBallColor) const
{
  NDECLARE_DEBUGDRAWING("TeamBallLocator:position:position", "drawingOnField",
                        "The estimated ball position.");
  NDECLARE_DEBUGDRAWING("TeamBallLocator:position:line from robot to position",
                        "drawingOnField", "A line from the robot pose to the team ball.");
  NDECLARE_DEBUGDRAWING("TeamBallLocator:particles", "drawingOnField",
                        "The particles of the GT2005 team ball locator.");

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

  //The estimated ball is drawn:
  NCIRCLE("TeamBallLocator:position:position", ballX, ballY, 28, 40,
          Drawings::ps_solid, estimatedBallColor);
  NCOLORED_LINE("TeamBallLocator:position:line from robot to position",
                robotPose.translation.x, robotPose.translation.y,
                ballX, ballY, 2, Drawings::ps_solid, 255, 0, 255);
  CIRCLE(teamBallLocatorField, ballX, ballY, 28, 40, 
         Drawings::ps_solid, estimatedBallColor);
  LINE(teamBallLocatorField, robotPose.translation.x, robotPose.translation.y,
       ballX, ballY, 2, Drawings::ps_solid, estimatedBallColor);

  DEBUG_DRAWING_FINISHED(teamBallLocatorField);
  DEBUG_DRAWING_FINISHED(teamBallLocatorFieldParticles);
}
//---------------------------------------------------------------------------------------

---