Representations/Perception/LandmarksPercept.cpp

Go to the documentation of this file.

/**
 * @file Representations/Perception/LandmarksPercept.cpp
 *
 * Contains the implementation of class LandmarksPercept.
 *
 * @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
 * @author <A href=mailto:asbre01@tzi.de>Andreas Sztybryc</A>
 */ 

#include "Tools/Streams/InOut.h"
#include "Tools/FieldDimensions.h"
#include "LandmarksPercept.h"


enum FlagType
{
  pinkAboveYellow, pinkAboveSkyblue, 
  yellowAbovePink, skyblueAbovePink
};
    
void Flag::setBottomColor()
{
  switch(type)
  {
    case pinkAboveYellow:
      bottomColor =  yellow;
      return;
    case pinkAboveSkyblue:
      bottomColor = skyblue;
      return;
    default:
      bottomColor = pink;
  }
}

void Flag::setTopColor()
{
  switch(type)
  {
    case yellowAbovePink:
      topColor =  yellow;
      return;
    case skyblueAbovePink:
      topColor = skyblue;
      return;
    default:
      topColor = pink;
  }
}

LandmarksPercept::LandmarksPercept()
{
  reset(0);  
}

void LandmarksPercept::reset(unsigned long frameNumber)
{  
  this->frameNumber = frameNumber;
  numberOfFlags = numberOfGoals = 0;
  viewAngle = 0;
  viewAngleValid = false;
}

void LandmarksPercept::addFlag(Flag::FlagType type, 
                               const Vector2<double>& position,
                               const ConditionalBoundary& boundary,
                               const Vector2<int>& topLeft,
                               const Vector2<int>& topRight,
                               const Vector2<int>& bottomLeft,
                               const Vector2<int>& bottomRight)
{
  if(numberOfFlags < 4)
  {
    flags[numberOfFlags].type = type;
    flags[numberOfFlags].position = position;
    flags[numberOfFlags].topLeft = topLeft;
    flags[numberOfFlags].topRight = topRight;
    flags[numberOfFlags].bottomLeft = bottomLeft;
    flags[numberOfFlags].bottomRight = bottomRight;
    flags[numberOfFlags].setBottomColor();
    flags[numberOfFlags].setTopColor();
    (ConditionalBoundary&) flags[numberOfFlags++] = boundary;
  }
}



void LandmarksPercept::addFlag
(
 Flag::FlagType type,
 bool ownTeamColorIsBlue,
 const ConditionalBoundary& boundary,
 const Vector2<int>& topLeft,
 const Vector2<int>& topRight,
 const Vector2<int>& bottomLeft,
 const Vector2<int>& bottomRight
)
{
  if(numberOfFlags < 4)
  {
    Vector2<double> flagPosition;

    int flip = ownTeamColorIsBlue ? -1 : 1;

    switch (type)
    {
    case Flag::pinkAboveYellow:
      flagPosition.x = xPosBackFlags * flip;
      flagPosition.y = yPosRightFlags * flip;
      break;
    case Flag::pinkAboveSkyblue:
      flagPosition.x = xPosFrontFlags * flip;
      flagPosition.y = yPosRightFlags * flip;
      break;
    case Flag::yellowAbovePink:
      flagPosition.x = xPosBackFlags * flip;
      flagPosition.y = yPosLeftFlags * flip;
      break;
    case Flag::skyblueAbovePink:
      flagPosition.x = xPosFrontFlags * flip;
      flagPosition.y = yPosLeftFlags * flip;
      break;
    }

    flags[numberOfFlags].type = type;
    flags[numberOfFlags].position = flagPosition;
    flags[numberOfFlags].topLeft = topLeft;
    flags[numberOfFlags].topRight = topRight;
    flags[numberOfFlags].bottomLeft = bottomLeft;
    flags[numberOfFlags].bottomRight = bottomRight;
    flags[numberOfFlags].setBottomColor();
    flags[numberOfFlags].setTopColor();
    (ConditionalBoundary&) flags[numberOfFlags++] = boundary;
  }
}

void LandmarksPercept::estimateOffsetForFlags
(
 const Vector2<double>& cameraOffset
 )
{
  for(int i = 0; i < numberOfFlags; ++i)
  {
    Flag& flag = flags[i];

    /** @todo improve, isOnBorder(flag.x.?) not checked */
    double distance;
    double direction = flag.x.getCenter();

    if(!flag.isOnBorder(flag.y.min) && !flag.isOnBorder(flag.y.max))
    {
      if(flag.y.min != flag.y.max)
      {
        /* calculation of distance is based on the fact that flag is perpendicular to the ground */
        distance = flagHeight / (tan(flag.y.max) - tan(flag.y.min)) + flagRadius;
        flag.distanceValidity = 0.8;
      }
      else
      {
        distance = 4500;
        flag.distanceValidity = 0.05;
      }
    }
    else
    {
      distance = flagRadius / sin(flag.x.getSize() / 2);
      if(!flag.isOnBorder(flag.x.min) && !flag.isOnBorder(flag.x.max)) // Flag touches no vertical border
        flag.distanceValidity = 0.7;
      else
        flag.distanceValidity = 0.2;
    }

    if(!flag.isOnBorder(flag.x.min) && !flag.isOnBorder(flag.x.max)) // Flag touches no vertical border
      flag.angleValidity = 0.8;
    else
      flag.angleValidity = 0.7;
    
    Pose2D p = Pose2D(cameraOffset) + Pose2D(direction) 
                       + Pose2D(Vector2<double>(distance,0));
//    flag.distance = p.translation.abs();
//    flag.angle = atan2(p.translation.y,p.translation.x);

      flag.distance = p.translation.abs();
      flag.angle = direction;
    
       
    if (flag.distance > 6000)
    {
      flag.distance = 6000;
      flag.distanceValidity=0; // flags far away are meassured very bad
    }
    else if (flag.distance > 3000) 
      flag.distanceValidity*=0.5; // flags medium far away are meassured medium bad
  }
}

void LandmarksPercept::addGoal(colorClass color,
                               const ConditionalBoundary& boundary,
                               const Vector2<int>& topLeft, const Vector2<int>& topRight, 
                               const Vector2<int>& bottomLeft, const Vector2<int>& bottomRight,
                               double distance, double angle, double rotation,
                               double distanceValidity, double angleValidity)
{
  // There is only memory for two goals
  if(numberOfGoals >= 2)
    return;

  // Calculate the positions of the seen goal on the field (field coordinate system depends on team colour)
  Vector2<double> leftGoalpost, rightGoalpost;
  colorClass ownGoalColor = (getPlayer().getTeamColor() == Player::blue) ? skyblue : yellow;
  if (color==ownGoalColor)
  {
    leftGoalpost.x = xPosOwnGoalpost;
    leftGoalpost.y = yPosRightGoal;
    rightGoalpost.x = xPosOwnGoalpost;
    rightGoalpost.y = yPosLeftGoal;
  }
  else
  {
    leftGoalpost.x = xPosOpponentGoalpost;
    leftGoalpost.y = yPosLeftGoal;
    rightGoalpost.x = xPosOpponentGoalpost;
    rightGoalpost.y = yPosRightGoal;
  }

  // Add goal percept
  Goal& goal = goals[numberOfGoals++];
  
  goal.color = color;
  goal.leftPost = leftGoalpost;
  goal.rightPost = rightGoalpost;
  goal.topLeft = topLeft;
  goal.topRight = topRight;
  goal.bottomLeft = bottomLeft;
  goal.bottomRight = bottomRight;
  (ConditionalBoundary&) goal = boundary;
  goal.distance = distance;
  goal.angle = angle;
  goal.rotation = rotation;
  goal.angleValidity = angleValidity;
  goal.distanceValidity = distanceValidity;
}

void LandmarksPercept::addGoal(colorClass color,
                               const ConditionalBoundary& boundary,
                               const Vector2<int>& topLeft,
                               const Vector2<int>& topRight,
                               const Vector2<int>& bottomLeft,
                               const Vector2<int>& bottomRight,
                               const CameraMatrix& cameraMatrix)
{
  if (numberOfGoals < 2)
  {
    addGoal(color, boundary, topLeft, topRight, bottomLeft, bottomRight, 0, 0, 0, 0, 0);
    estimateOffsetAndValiditiesForGoal(goals[numberOfGoals-1], cameraMatrix);
  }
}

void LandmarksPercept::estimateOffsetAndValiditiesForGoal(Goal& goal, const CameraMatrix& cameraMatrix)
{
    Vector2<double> cameraOffset(cameraMatrix.translation.x, cameraMatrix.translation.y);

    double distance, distanceByHeight,distanceByWidth;
    if(goal.y.max != goal.y.min)
      /* calculation of distance is based on the fact that goal is perpendicular to the ground */
      distanceByHeight = goalHeight / (tan(goal.y.max) - tan(goal.y.min));
    else
      distanceByHeight = 10000;

    if(goal.x.max != goal.x.min)
      /* calculation assumes that goal is perpendicular to the view direction */
      distanceByWidth = goalWidth / (2 * (tan((goal.x.max - goal.x.min)/2)));
    else
      distanceByWidth = 10000;

    if( distanceByHeight < distanceByWidth)
      distance = distanceByHeight;
    else
      distance = distanceByWidth;

    double direction = goal.x.getCenter();
    Pose2D p = Pose2D(cameraOffset) + Pose2D(direction) 
                       + Pose2D(Vector2<double>(distance,0));
    goal.distance = p.translation.abs();
//    goal.angle = atan2(p.translation.y,p.translation.x);
    goal.angle = direction;
    goal.rotation = 0; // wrong!
    if(!goal.isOnBorder(goal.x.min) && !goal.isOnBorder(goal.x.max)) // goal touches no vertical borders
    { 
      goal.angleValidity = 0.80;
      goal.distanceValidity = 0.50;
    }
    else
      if (goal.isOnBorder(goal.x.min) && goal.isOnBorder(goal.x.max)) // goal touches both vertical borders
      {
        goal.angleValidity = 0.20;
        goal.distanceValidity = 0.10;
      }
      else
      {
        goal.angleValidity = 0.5;
        goal.distanceValidity = 0.15;
      }

    if (goal.distance > xPosOpponentFieldBorder * 1.2)
    {
      goal.distanceValidity=0; // goals far away are meassured very bad
    }
    else if (goal.distance > xPosOpponentFieldBorder * 0.5) 
      goal.distanceValidity*=0.5; // goals medium far away are meassured medium bad

}

---