Modules/BallLocator/GT2004BallLocator.cpp

Go to the documentation of this file.

/**
* @file GT2004BallLocator.cpp
* Contains a BallLocator implementation using Kalman filtering
*
* @author <a href="mailto:stefanuhrig@gmx.net">Stefan Uhrig</a>
*/
//------------------------------------------------------------------------------
#include "GT2004BallLocator.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Actorics/RobotDimensions.h"
#include "Tools/FieldDimensions.h"
#include "Platform/SystemCall.h"
#include "Tools/Debugging/DebugDrawings.h"
#include "Tools/Debugging/GT2004BallLocatorDebugData.h"
#include "GT2004ProcessModels/FixedPositionModel.h"
#include "GT2004ProcessModels/ConstantSpeedModel.h"
//------------------------------------------------------------------------------
double GT2004BallLocator::dFieldDiagonalLength =
  1.2*sqrt((2.0*xPosOpponentGoal)*(2.0*xPosOpponentGoal) +
           (2.0*yPosLeftFlags)*(2.0*yPosLeftFlags));
//------------------------------------------------------------------------------
GT2004BallLocator::GT2004BallLocator(BallLocatorInterfaces& interfaces)
 : BallLocator(interfaces)
  
{
  // Initialise member variables
  bSendProcessModelStates = false;
  ballWasSeen = false;
  ballWasSeenInLastImage = false;
  x_abs = 0.0;
  y_abs = 0.0;
  x_rel = 0.0;
  y_rel = 0.0;
  vx_abs = 0.0;
  vy_abs = 0.0;
  vx_rel = 0.0;
  vy_rel = 0.0;

  // initializing the sliding average to 5 frames...
  //avx_rel.reset(5);
  //avy_rel.reset(5);
  
  // Add the models the filter is to work with
  // addModel(new KalmanFixedPositionModel);
  addModel(new KalmanConstantSpeedModel);

  // initializing some data for ball state information
  // used by setBallStateV2();
  freshDefend  = true;
  ballSensedPos.x = 0;
  ballSensedPos.y = 0;
  ballSensedTime = 0;
  ballSensedRelPos.x = 0;
  ballSensedRelPos.y = 0;
  last_seen_x_rel = 0;
  last_seen_y_rel = 0;
}
//------------------------------------------------------------------------------
GT2004BallLocator::~GT2004BallLocator()
{
  // Free the models
  destroyModels();
}
//------------------------------------------------------------------------------
void GT2004BallLocator::execute()
{
  Player::teamColor ownColor = getPlayer().getTeamColor();
  colorClass opponentGoalColor = ownColor == Player::red ? skyblue : yellow;
  ballModel.seen.ballInFrontOfOpponentGoal = false;
  if(landmarksPercept.numberOfGoals > 0)
  {
    double angleToBall = ballPercept.getAngleSizeBased();
    double ballDistance = ballPercept.getDistanceSizeBased();
    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;
    }
  }
  
  static bool prevCameraMatrixUninitialized = true;
  if (prevCameraMatrixUninitialized && cameraMatrix.frameNumber != 0)
  {
    prevCameraMatrix = cameraMatrix;
    prevCameraMatrixUninitialized = false;
  }

  determineNumberOfImagesWith_WithoutBall();
  
  // calculate error (why?)
  double ballMeasurementError = 159.1 * tan(openingAngleHeight_ERS210/2)/176*3;
  ballModel.seen.distanceError = 
    ballMeasurementError *
    ballPercept.getDistance() * ballPercept.getDistance() /
    (ballRadius * 159.1 /* focal length ers219 */);
  ballModel.seen.angleError = 0.05;

  compensateOdometry();

  // Distinguish between seen and unseen ball
  if (ballPercept.ballWasSeen)
  {
    handleSeenBall();
    ballWasSeen = true;
  }
  else
  {
    handleUnseenBall();
    ballWasSeen = false;
  }

  // Set frame number
  ballModel.frameNumber = cameraMatrix.frameNumber;

  // Set ball state (none, rollByRight, rollByLeft)
  setBallState();
//  setBallStateV2();

  // Send the results of the process models as debug messages
  if (bSendProcessModelStates)
    sendProcessModelStates();

  // Draw the ball position on the field
  drawBallPosition();
  
  prevCameraMatrix = cameraMatrix;

  // percept-part of balllocator
  if(ballPercept.ballWasSeen) // ball was seen
  {
    Vector2<double> ballOffset;
    ballPercept.getOffset(ballOffset);

    lastBallSeen = ballOffset;
  }
  else // ball was not seen
  {
    lastBallSeen = (lastRobotOdometry + Pose2D(lastBallSeen) - odometryData).translation;
  }
  
  ballModel.seen = 
    Geometry::relative2FieldCoord(robotPose, lastBallSeen.x, lastBallSeen.y); 

  lastRobotOdometry = odometryData;
}
//------------------------------------------------------------------------------
void GT2004BallLocator::handleSeenBall()
{
  int i;

  // If ball wasn't seen the last time, set the time the ball was seen first
  if (!ballWasSeen)
    ballModel.seen.timeWhenFirstSeenConsecutively 
    = SystemCall::getCurrentSystemTime();

  // Time in seconds
  double time = (double)SystemCall::getCurrentSystemTime() / 1000.0;

  // Get seen coordinates
  Vector2<double> ballOffset;
//  ballPercept.getOffsetSizeBased(ballOffset);
  ballPercept.getOffset(ballOffset);

  // Ignore impossible coordinates
  if (isNonSensePos(ballOffset.x, ballOffset.y))
  {
    handleUnseenBall();
    return;
  }

  // Relative coordinates to robot in meter
  x_rel = ballOffset.x / 1000.0;
  y_rel = ballOffset.y / 1000.0;

  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();
  double timeDiff = (frameNumber - prevFrameNumber) * r.motionCycleTime; // in seconds
  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);    
  }
  // Update process models
  double liklihoodSum = 0.0;
  try
  {
    for (i = 0; i < (int)kalmanModels.size(); ++i)
    {
      // check if the model is used
      if (!kalmanModels[i]->useModel())
        continue;
      
      // Send change of odometry to model
      kalmanModels[i]->adapt(lastOdometry, odometryData);

      // Do update and save result
      kalmanUpdateResults[i] = kalmanModels[i]->update(time, x_rel, y_rel, robotPose, panningVelocity);
      liklihoodSum += kalmanUpdateResults[i].liklihood;

      // Reset process model if result is not good
      if (kalmanModels[i]->isNonSensePos(kalmanUpdateResults[i].state.x,
                                         kalmanUpdateResults[i].state.y))
        kalmanModels[i]->reset();
    }
  }
  catch (...)
  {
    // Here we should never end, but better safe than sorry
    OUTPUT(idText, text,
           "Unknown Exception in GT2004BallLocator::handleSeenBall");
    setUnknownResult();
    return;
  }
  lastOdometry = odometryData;

  // Get weighted position and speed
  if (liklihoodSum != 0.0)
  {
    x_rel = 0.0;
    y_rel = 0.0;
    vx_rel = 0.0;
    vy_rel = 0.0;
    double probability;

    //std::vector<KalmanUpdateResult>::iterator pos;
    /*
    for (pos = kalmanUpdateResults.begin();
         pos != kalmanUpdateResults.end();
         ++pos)
    */
    for (i = 0; i < (int)kalmanModels.size(); ++i)
    {
      if (!kalmanModels[i]->useModel())
        continue;

      std::vector<KalmanUpdateResult>::iterator pos =
        kalmanUpdateResults.begin() + i;
      if (pos->liklihood != 0.0)
        probability = pos->liklihood = pos->liklihood / liklihoodSum;
      else
        probability = 0.0;
      x_rel += pos->state.x * probability;
      y_rel += pos->state.y * probability;
      vx_rel += pos->state.vx * probability;
      vy_rel += pos->state.vy * probability;
    }

    // Change meter to millimeter and meter per second to millimeter per second
    x_rel *= 1000.0;
    y_rel *= 1000.0;
    vx_rel *= 1000.0;
    vy_rel *= 1000.0;

    // Save last seen position
    last_seen_x_rel = x_rel;
    last_seen_y_rel = y_rel;

    // Translate relative coordinates to robot into field coordinates
    ballOffset = Geometry::relative2FieldCoord(robotPose, x_rel, y_rel);
    x_abs = ballOffset.x;
    y_abs = ballOffset.y;
    vx_abs =  vx_rel*cos(robotPose.rotation) + vy_rel*sin(robotPose.rotation); 
    vy_abs = -vx_rel*sin(robotPose.rotation) + vy_rel*cos(robotPose.rotation);
    
    // Set seen ball data
    ballModel.seen = ballOffset;
    
    // changed to relative values
    ballModel.seen.speed.x = vx_rel;
    ballModel.seen.speed.y = vy_rel;

    // Set the propagated data, too
    ballModel.propagated.timeOfObservation = SystemCall::getCurrentSystemTime();
    ballModel.propagated.x = ballOffset.x;
    ballModel.propagated.y = ballOffset.y;
    ballModel.propagated.positionWhenLastObserved = ballOffset;
    
    // changed to relative values
    ballModel.propagated.observedSpeed.x = vx_rel;
    ballModel.propagated.observedSpeed.y = vy_rel;
  }
  else
  {
    // Probably process models are not initialised yet

    // Change meter to millimeter
    x_rel *= 1000.0;
    y_rel *= 1000.0;

    // Translate relative coordinates to robot into field coordinates
    ballOffset = Geometry::relative2FieldCoord(robotPose, x_rel, y_rel);
    x_abs = ballOffset.x;
    y_abs = ballOffset.y;
    vx_rel = 0.0;
    vy_rel = 0.0;
    vx_abs = 0.0;
    vy_abs = 0.0;
    
    // Set seen (=measured) ball position, speed is unknown
    ballModel.seen = ballOffset;
    ballModel.seen.speed.x = 0.0;
    ballModel.seen.speed.y = 0.0;
  }

  // Set times
  ballModel.seen.timeWhenLastSeen = SystemCall::getCurrentSystemTime();
  ballModel.seen.timeUntilSeenConsecutively = SystemCall::getCurrentSystemTime();
}
//------------------------------------------------------------------------------
void GT2004BallLocator::handleUnseenBall()
{
  // Use predict function of the models to predict the position and
  // speed of the ball

  // Time in seconds
  double time = (double)SystemCall::getCurrentSystemTime() / 1000.0;
  
  int i;
  double liklihoodSum = 0.0;
  try
  {
    for (i = 0; i < (int)kalmanModels.size(); ++i)
    {
      // Check if model is used
      if (!kalmanModels[i]->useModel())
        continue;
      
      // Send change of odometry to model
      kalmanModels[i]->adapt(lastOdometry, odometryData);

      // Predict next state
      kalmanPredictResults[i] = kalmanModels[i]->predict(time);
      liklihoodSum += kalmanPredictResults[i].liklihood;

      // Reset process model if results are not good
      if (kalmanModels[i]->isNonSensePos(kalmanPredictResults[i].state.x,
                                         kalmanPredictResults[i].state.y))
        kalmanModels[i]->reset();
    }
  }
  catch (...)
  {
    // Here we should never end, but better safe than sorry
    OUTPUT(idText, text,
           "Unknown Exception in GT2004BallLocator::handleUnseenBall");
    setUnknownResult();
    return;
  }
  lastOdometry = odometryData;

  // Get weighted position and speed
  if (liklihoodSum != 0.0)
  {
    x_rel = 0.0;
    y_rel = 0.0;
    vx_rel = 0.0;
    vy_rel = 0.0;
    double probability;

    /*
    std::vector<KalmanPredictResult>::iterator pos;
    for (pos = kalmanPredictResults.begin();
         pos != kalmanPredictResults.end();
         ++pos)
    */
    for (i = 0; i < (int)kalmanModels.size(); ++i)
    {
      if (!kalmanModels[i]->useModel())
        continue;

      std::vector<KalmanPredictResult>::iterator pos =
        kalmanPredictResults.begin() + i;
      
      if (pos->liklihood != 0.0)
        probability = pos->liklihood = pos->liklihood / liklihoodSum;
      else
        probability = 0.0;
      x_rel += pos->state.x * probability;
      y_rel += pos->state.y * probability;
      vx_rel += pos->state.vx * probability;
      vy_rel += pos->state.vy * probability;
    }

    // Change meter to millimeter and meter per second to millimeter per second
    x_rel *= 1000.0;
    y_rel *= 1000.0;
    vx_rel *= 1000.0;
    vy_rel *= 1000.0;

    // Translate relative coordinates to robot into field coordinates
    Vector2<double> ballOffset =
      Geometry::relative2FieldCoord(robotPose, x_rel, y_rel);
    x_abs = ballOffset.x;
    y_abs = ballOffset.y;
    vx_abs =  vx_rel*cos(robotPose.rotation) + vy_rel*sin(robotPose.rotation); 
    vy_abs = -vx_rel*sin(robotPose.rotation) + vy_rel*cos(robotPose.rotation);

    // Set estimated position
    ballModel.propagated.x = ballOffset.x;
    ballModel.propagated.y = ballOffset.y;
  }
  else
  {
    // Probably process models are not initialised yet
    
    // Get time difference in seconds
    double dt = (SystemCall::getCurrentSystemTime() -
                 ballModel.propagated.timeOfObservation) / 1000.0;
    
    // Assume the ball is moving equally and set estimated position
    ballModel.propagated.x =
      ballModel.propagated.positionWhenLastObserved.x + 
      ballModel.propagated.observedSpeed.x*dt;
    ballModel.propagated.y =
      ballModel.propagated.positionWhenLastObserved.y + 
      ballModel.propagated.observedSpeed.y*dt;
  }

  // write last seen position to seen ball
  Vector2<double> ballOnField = 
    Geometry::relative2FieldCoord(robotPose, last_seen_x_rel, last_seen_y_rel);
  ballModel.seen.x = ballOnField.x;
  ballModel.seen.y = ballOnField.y;
}
//------------------------------------------------------------------------------
void GT2004BallLocator::compensateOdometry()
{
  Vector2<double> lastBallSeenRelative
    = (lastOdometry +
       Pose2D(last_seen_x_rel,last_seen_y_rel) -
       odometryData).translation;
  last_seen_x_rel = lastBallSeenRelative.x;
  last_seen_y_rel = lastBallSeenRelative.y;

  //~ lastOdometry = odometryData;
}        
//------------------------------------------------------------------------------
void GT2004BallLocator::setBallState()
{
  ballModel.ballState.reset();

  // Ball must be approaching faster than 50 millimeters per second
  if (vx_rel > -50.0)
    return;

  // Calculate y-position (side) of ball when the ball has the same height
  // as the robot und set appropriate ball state
  ballModel.ballState.projectedDistanceOnYAxis = y_rel - (vy_rel/vx_rel)*x_rel;

  ballModel.ballState.timeBallCrossesYAxis = (long)(ballModel.ballState.projectedDistanceOnYAxis / vx_rel) * 1000; // mm/s to mm/ms
 
  // Ball must be nearer than 1 meter
  double dist = sqrt(x_rel*x_rel + y_rel*y_rel);
  if (dist > 1000.0)
    return;
  
  if ((ballModel.ballState.projectedDistanceOnYAxis < -100.0) 
      && (ballModel.ballState.projectedDistanceOnYAxis > -500.0))
  {
    ballModel.ballState.ballRollsByRight = true;
  }
  else if ((ballModel.ballState.projectedDistanceOnYAxis > 100.0) 
      && (ballModel.ballState.projectedDistanceOnYAxis < 500.0))
  {
    ballModel.ballState.ballRollsByLeft = true;
  }
  else if ((ballModel.ballState.projectedDistanceOnYAxis >= -100.0) 
      && (ballModel.ballState.projectedDistanceOnYAxis <= 100.0))
  {
    ballModel.ballState.ballRollsTowardsRobot = true;
  }
}
//------------------------------------------------------------------------------
void GT2004BallLocator::setBallStateV2()
{
  // some constants

  static int const defendZone1Border = 550;
  static int const defendZone2Border = 1000; // should be at least greater defendZone1Border+100
  static int const defendZone3Border = 1000;

  static double const ballHasMovedThreshold = 20.0;

//  not used in the moment...
//  static double const ballIsFastThreshold = 500;

  static unsigned long const speedTrigger = 1000;

  /**
  *   Meanings of the constants:
  *
  *   If the ball is farer away than defendZone2Border from the robot
  *   then freshDefend is set to true. This is just a kind of hysteresis
  *   to reset the "sensing" for the ballStates.
  *
  *   To sense a ball state, the ball has to be detected first in a range
  *   smaller than defendZone2Border, and after that in a range smaller than
  *   defendZone1Border. The maximum time between the detections in zone2 and
  *   zone1 has to be smaller than speedTrigger (in ms). 
  *
  *   The ballHasMovedThreshold is used to determine, if the ball has moved
  *   or the robot, since only relative ball positions are used. The difference
  *   between the absolute ball positions in zone2 and zone1 has to be greater
  *   than this threshold to identify the ball as moving, rather than the robot.
  *
  *   The ballIsFastThreshold is used to determine if the ball rolls fast.
  *   Value is mm/s.
  */
  
  ballModel.ballState.reset();

  // getting some ball information...
  double ballRelDist = 
    Geometry::distanceTo(robotPose.getPose(),ballModel.seen);

  Vector2<double> ballRelPos;
  ballRelPos.x = ballRelDist * 
    cos(Geometry::angleTo(robotPose.getPose(),ballModel.seen));
  ballRelPos.y = ballRelDist * 
    sin(Geometry::angleTo(robotPose.getPose(),ballModel.seen));

  double ballRelAngle = toDegrees(Geometry::angleTo(robotPose.getPose(),
    ballModel.seen));

  if ((ballRelDist > (defendZone1Border+100)) && (ballRelDist < defendZone2Border))
  { 
    ballSensedPos = ballModel.seen;

    ballSensedRelPos = ballRelPos;
    if ((ballRelAngle > -90) && (ballRelAngle <= 90))
      ballSensedTime = SystemCall::getCurrentSystemTime();
  }

  if (ballRelDist > defendZone3Border) {
    freshDefend  = true;
  }

  double side;
  side = ballSensedRelPos.y + (ballSensedRelPos.x / (ballRelPos.x - ballSensedRelPos.x))*(ballRelPos.y - ballSensedRelPos.y);
  ballModel.ballState.projectedDistanceOnYAxis = side;
  ballModel.ballState.timeBallCrossesYAxis = (long)(ballModel.ballState.projectedDistanceOnYAxis / vx_rel) * 1000; // mm/s to mm/ms
  if (    (ballRelAngle > -90)
       && (ballRelAngle <  90)
       && (ballRelDist  <  defendZone1Border)
       && (freshDefend == true))
  {
    freshDefend = false;
    unsigned long travelTime = SystemCall::getCurrentSystemTime() - ballSensedTime;
    double travelDist = (ballSensedPos - ballModel.seen).abs();
    if (    (travelTime <  speedTrigger)
         && (ballSensedRelPos.x - ballRelPos.x  >= 1)
         && (travelDist > ballHasMovedThreshold)
       )
    {
      if ((side > -150) && (side < 150))
        ballModel.ballState.ballRollsTowardsRobot = true;
      else if ((side > -500) && (side <= -150))
        ballModel.ballState.ballRollsByRight = true;
      else if ((side >= 150) && (side < 500))
        ballModel.ballState.ballRollsByLeft = true;
    }
  }

  // determining ballModel.ballState.ballRollsFast is not done yet...

}
//------------------------------------------------------------------------------
void GT2004BallLocator::setUnknownResult()
{
  // Sets the result to the last known results
  ballModel.propagated.x = x_abs;
  ballModel.propagated.y = y_abs;
}
//------------------------------------------------------------------------------
bool GT2004BallLocator::isNonSensePos(double x, double y)
{
  double dist = sqrt(x*x + y*y);
  if (dist > dFieldDiagonalLength)
    return true;
  else
    return false;
}
//------------------------------------------------------------------------------
void GT2004BallLocator::addModel(KalmanProcessModelBase* pModel)
{
  // Add the model and make room in the result vectors
  kalmanModels.push_back(pModel);
  kalmanUpdateResults.push_back(KalmanUpdateResult());
  kalmanPredictResults.push_back(KalmanPredictResult());
}
//------------------------------------------------------------------------------
void GT2004BallLocator::destroyModels()
{
  // Free the models
  std::vector<KalmanProcessModelBase*>::iterator pos;
  for (pos = kalmanModels.begin(); pos != kalmanModels.end(); ++pos)
    delete *pos;
}
//------------------------------------------------------------------------------
bool GT2004BallLocator::handleMessage(InMessage& message)
{
  // Handle only GT2004Data messages
  if (message.getMessageID() != idGT2004BallLocatorData)
    return false;

  // Get type of action
  char action;
  message.bin >> action;

  switch (action)
  {
    case GT2004BallLocatorDebugData::idRequestParameters:
    {
      // Parameters of the process models were requested

      // Start message and send number of models
      getDebugOut().bin << (char)GT2004BallLocatorDebugData::idSendingParameters 
                        << kalmanModels.size();
      
      // Send the data of the models
      std::vector<KalmanProcessModelBase*>::iterator pos;
      for (pos = kalmanModels.begin(); pos != kalmanModels.end(); ++pos)
        (*pos)->toStream(getDebugOut().bin);
      
      // Finish message
      getDebugOut().finishMessage(idGT2004BallLocatorData);
    }
    return true;

    case GT2004BallLocatorDebugData::idSetParameters:
    {
      // The parameters of a model are to be set

      // Read index of the model
      int modelIndex;
      message.bin >> modelIndex;
      
      // Set data of the model
      kalmanModels[modelIndex]->fromStream(message.bin);
    }
    return true;
  }

  // Message is unknown, reset position in queue
  message.resetReadPosition();
  return false;
}
//------------------------------------------------------------------------------
void GT2004BallLocator::sendProcessModelStates()
{
  // Get debug stream
  OutMessage& stream = getDebugOut();

  // Write ID of message
  stream.bin << (char)GT2004BallLocatorDebugData::idFilterState;
  
  // Time stamp of the data
  stream.bin << SystemCall::getCurrentSystemTime();

  // Write robot position
  stream.bin << robotPose;

  // Write relative coordinates and speeds
  // (in millimeters and millimeters per second)
  stream.bin << x_rel << y_rel;
  stream.bin << vx_rel << vy_rel;

  // Write if ball was seen or not
  stream.bin << (char)ballWasSeen;

  // Write number of models
  stream.bin << kalmanUpdateResults.size();
  
  // Write result of models
  std::vector<KalmanUpdateResult>::const_iterator urpos;
  std::vector<KalmanPredictResult>::const_iterator prpos;
  std::vector<KalmanProcessModelBase*>::const_iterator mpos;
  for (mpos = kalmanModels.begin(),
         urpos = kalmanUpdateResults.begin(),
         prpos = kalmanPredictResults.begin();
       mpos != kalmanModels.end();
       ++mpos, ++urpos, ++prpos)
  {
    
    if (ballWasSeen)
    {
      // Write position in millimeters
      stream.bin << urpos->state.x*1000.0 << urpos->state.y*1000.0;
      // Write speed in millimeters per second
      stream.bin << urpos->state.vx*1000.0 << urpos->state.vy*1000.0;
      // Write probability (weight was converted to probability)
      stream.bin << urpos->liklihood;
    }
    else
    {
      // Write position in millimeters
      stream.bin << prpos->state.x*1000.0 << prpos->state.y*1000.0;
      // Write speed in millimeters per second
      stream.bin << prpos->state.vx*1000.0 << prpos->state.vy*1000.0;
      // Write probability (weight was converted to probability)
      stream.bin << prpos->liklihood;
    }

    // Write dimensioni of state
    int n = (*mpos)->getStateDim();
    stream.bin << n;
    
    // Write internal covarinace matrix of process model
    double* M = new double[n*n];
    (*mpos)->getP(M);
    int i;
    for (i = 0; i < n*n; ++i)
      stream.bin << M[i];
    delete [] M;
  }
  
  // Finish message
  stream.finishMessage(idGT2004BallLocatorData);
}
//------------------------------------------------------------------------------
void GT2004BallLocator::drawBallPosition()
{
  // Draw global positions
  if (ballWasSeen)
  {
    CIRCLE(ballLocatorField, x_abs, y_abs, 90, 3, 0, Drawings::orange);
    LINE(ballLocatorField, x_abs, y_abs, 
         x_abs + vx_abs*3, y_abs + vy_abs*3,
         3, 0, Drawings::orange);
  }
  else
  {
    CIRCLE(ballLocatorField, x_abs, y_abs, 90, 3, 0, Drawings::blue);
    LINE(ballLocatorField, x_abs, y_abs, 
         x_abs + vx_abs*3, y_abs + vy_abs*3,
         3, 0, Drawings::blue);
  }

  // Draw positions of models
  std::vector<KalmanUpdateResult>::const_iterator urpos;
  std::vector<KalmanPredictResult>::const_iterator prpos;
  std::vector<KalmanProcessModelBase*>::const_iterator mpos;
  for (mpos = kalmanModels.begin(),
         urpos = kalmanUpdateResults.begin(),
         prpos = kalmanPredictResults.begin();
       mpos != kalmanModels.end();
       ++mpos, ++urpos, ++prpos)
  {
    
    if (ballWasSeen)
    {
      // Calculate field coordinates
      Vector2<double> ballOffset =
        Geometry::relative2FieldCoord(robotPose, urpos->state.x*1000.0,
                                      urpos->state.y*1000.0);
      CIRCLE(ballLocatorField, ballOffset.x, ballOffset.y,
             60, 2, 0, Drawings::yellowOrange);
    }
    else
    {
      // Calculate field coordinates
      Vector2<double> ballOffset =
        Geometry::relative2FieldCoord(robotPose, prpos->state.x*1000.0,
                                      prpos->state.y*1000.0);
      CIRCLE(ballLocatorField, ballOffset.x, ballOffset.y,
             60, 2, 0, Drawings::skyblue);
    }
  }  
  
  DEBUG_DRAWING_FINISHED(ballLocatorField);
}
//------------------------------------------------------------------------------
void GT2004BallLocator::determineNumberOfImagesWith_WithoutBall()
{
  unsigned char diff = 1;
  switch (lastFrameNumber)
  {
  case  4: diff = 1; break;
  case  5: diff = 1; break;
  case  8: diff = 2; break;
  case  9: diff = 2; break;
  case 12: diff = 3; break;
  case 13: diff = 3; break;
  case 16: diff = 4; break;
  case 17: diff = 4; break;
  case 18: diff = 4; break;
  }


  if(ballWasSeenInLastImage)
  {
    if(!ballPercept.ballWasSeen)
    {
      ballModel.numberOfImagesWithoutBallPercept = 1;
    }
    else
    {
      ballModel.numberOfImagesWithBallPercept += diff;
    }
  }
  else // !lastBallWasSeen
  {
    if(ballPercept.ballWasSeen)
    {
      ballModel.numberOfImagesWithBallPercept = 1;
    }
    else
    {
      ballModel.numberOfImagesWithoutBallPercept += diff;
    }
  }
  ballModel.ballWasSeen = ballPercept.ballWasSeen;
  ballWasSeenInLastImage = ballPercept.ballWasSeen;
}
//------------------------------------------------------------------------------
/*
 * Change log :
 * $Log: GT2004BallLocator.cpp,v $
 * Revision 1.1  2004/07/14 20:56:34  spranger
 * moved KalmanCombo (incl percept stuff) to gt2004balllocator
 *
 * Revision 1.21  2004/06/28 02:00:35  spranger
 * bugfix
 *
 * Revision 1.20  2004/06/23 18:18:47  spranger
 * reintroduced ballModel.seen.ballInFrontOfOpponentGoal
 *
 * Revision 1.19  2004/06/23 11:37:43  dassler
 * New Ballsymbols introduced
 * ball.projected-distance-on-y-axis" description="Projected Distance of the ball to the y axis of the robot
 * ball.time-until-ball-crosses-y-axis" description="Time until the ball crosses the y axis of the robot
 *
 * Revision 1.18  2004/06/22 08:24:38  nistico
 * Reintroduced clipping between [-1; 1] before acos()
 *
 * Revision 1.17  2004/06/21 08:15:57  nistico
 * The dot product of normalized vectors (versors) is guaranteed to be within [-1, 1] ;
 * the angle was not normalized, however. (-pi, pi)
 *
 * Revision 1.16  2004/06/19 08:02:29  roefer
 * Initialize ball position
 * Clip parameter of acos to [-1 .. 1]
 *
 * Revision 1.15  2004/06/18 12:29:17  nistico
 * Added (redundant?) check for division_by_zero
 *
 * Revision 1.14  2004/06/18 10:24:51  nistico
 * Kalman measurement covariance matrix now adjusted also based on
 * head panning angular velocity
 *
 * Revision 1.13  2004/06/16 17:57:00  nistico
 * Speed covariance submatrix dynamically calculated
 * based on distance from ball
 * More detailed identification of exception
 * Bug fixes
 *
 * Revision 1.11  2004/06/15 21:40:14  uhrig
 * Added possibility to disable and enable process models in the Kalman filter.
 *
 * Revision 1.10  2004/06/15 17:48:32  juengel
 * Added method determineNumberOfImagesWith_WithoutBall().
 *
 * Revision 1.9  2004/06/14 10:44:28  risler
 * removed speed averaging
 * reactivated old working ball state
 *
 * Revision 1.8  2004/06/08 13:06:20  kerdels
 * slightly improved side-decision in ball-blocking, but there's still space for improvement ;-)
 *
 * Revision 1.7  2004/06/04 08:46:49  juengel
 * Position is determined based on the bearing to the ball.
 *
 * Revision 1.6  2004/05/27 18:49:17  kerdels
 * added a small 5 frames sliding average for the relative ballspeed,
 * added new ballState Representation and adjusted the apropriate files
 *
 * Revision 1.5  2004/05/27 09:53:33  loetzsch
 * removed "timeOfImageProcessing"
 *
 * Revision 1.4  2004/05/26 10:08:03  loetzsch
 * correct use of odometry
 *
 * Revision 1.2  2004/05/22 22:52:01  juengel
 * Renamed ballP_osition to ballModel.
 *
 * Revision 1.1.1.1  2004/05/22 17:15:19  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.8  2004/05/03 15:32:25  uhrig
 * Added additional ball state and disabled sending of Kalman filter process model states.
 *
 * Revision 1.7  2004/04/27 09:09:22  uhrig
 * Using own side ball detection again
 *
 * Revision 1.6  2004/04/22 14:28:02  roefer
 * .NET errors/warnings removed
 *
 * Revision 1.5  2004/04/20 14:12:16  uhrig
 *  - odometry processing corrected
 *  - fixed position model disabled
 *  - covariance matrices tuned
 *
 * Revision 1.4  2004/04/07 12:28:56  risler
 * ddd checkin after go04 - first part
 *
 * Revision 1.5  2004/03/31 00:09:46  risler
 * set last seen position when ball is not seen
 *
 * Revision 1.4  2004/03/30 15:48:04  risler
 * various bugfixes from stefan
 *
 * Revision 1.1.1.1  2004/03/29 08:28:51  Administrator
 * initial transfer from tamara
 *
 * Revision 1.3  2004/03/27 21:51:32  risler
 * process model message sending disabled
 *
 * Revision 1.2  2004/03/15 12:28:52  risler
 * change log added
 *
 *
 */

---