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
};
    
colorClass Flag::getLowerColor() const
{
  switch(type)
  {
    case pinkAboveYellow:
      return yellow;
    case pinkAboveSkyblue:
      return skyblue;
    default:
      return pink;
  }
}

colorClass Flag::getUpperColor() const
{
  switch(type)
  {
    case yellowAbovePink:
      return yellow;
    case skyblueAbovePink:
      return skyblue;
    default:
      return pink;
  }
}

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

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

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



void LandmarksPercept::addFlag
(
 Flag::FlagType type,
 bool ownTeamColorIsBlue,
 const ConditionalBoundary& boundary
 )
{
  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;
    (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)
      {
        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 Vector2<double>& leftPost,
                               const Vector2<double>& rightPost,
                               const ConditionalBoundary& boundary)
{
  if(numberOfGoals < 2)
  {
    goals[numberOfGoals].color = color;
    goals[numberOfGoals].leftPost = leftPost;
    goals[numberOfGoals].rightPost = rightPost;
    (ConditionalBoundary&) goals[numberOfGoals++] = boundary;
  }
}

void LandmarksPercept::addGoal
(
 colorClass color,
 bool ownTeamColorIsBlue,
 const ConditionalBoundary& boundary
 )
{
  if(numberOfGoals < 2)
  {
    int flip = ownTeamColorIsBlue ? -1 : 1;
    Vector2<double>leftGoalPost, rightGoalPost;
    switch (color)
    {
    case yellow:
      leftGoalPost.x = xPosOwnGoalpost * flip;
      leftGoalPost.y = yPosRightGoal * flip;
      rightGoalPost.x = xPosOwnGoalpost * flip;
      rightGoalPost.y = yPosLeftGoal * flip;
      break;
    case skyblue:
      leftGoalPost.x = xPosOpponentGoalpost * flip;
      leftGoalPost.y = yPosLeftGoal * flip;
      rightGoalPost.x = xPosOpponentGoalpost * flip;
      rightGoalPost.y = yPosRightGoal* flip;
      break;
    }
    goals[numberOfGoals].color = color;
    goals[numberOfGoals].leftPost = leftGoalPost;
    goals[numberOfGoals].rightPost = rightGoalPost;
    (ConditionalBoundary&) goals[numberOfGoals++] = boundary;
  }
}

void LandmarksPercept::estimateOffsetForGoals
(
  const Vector2<double>& cameraOffset
)
{
  for (int i = 0; i < numberOfGoals; i++)
  {
    if(i >= 2)
    {
      OUTPUT(idText,text,"to many Goals");
      numberOfGoals = 2;
      return;
    }
    Goal& goal = goals[i];

    /** @todo distinguish between height of left and right post
      * @todo isOnBorder(flag.?.?) not checked
      */
    double distance;
    if(goal.y.max != goal.y.min)
      distance = goalHeight / (tan(goal.y.max) - tan(goal.y.min));
    else
      distance = 10000;
    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 > 6000)
    {
      goal.distance = 6000;
      goal.distanceValidity=0; // goals far away are meassured very bad
    }
    else if (goal.distance > 3000) 
      goal.distanceValidity*=0.5; // goals medium far away are meassured medium bad

  }
}


In& operator>>(In& stream,LandmarksPercept& landmarksPercept)
{
  stream >> landmarksPercept.frameNumber;
  stream.read(&landmarksPercept.numberOfFlags,sizeof(int));
  stream.read(&landmarksPercept.flags,sizeof(Flag) * landmarksPercept.numberOfFlags);
  stream.read(&landmarksPercept.numberOfGoals,sizeof(int));
  stream.read(&landmarksPercept.goals,sizeof(Goal) * landmarksPercept.numberOfGoals);
  stream.read(&landmarksPercept.cameraOffset,sizeof(landmarksPercept.cameraOffset));
  return stream;
}

Out& operator<<(Out& stream, const LandmarksPercept& landmarksPercept)
{
  stream << landmarksPercept.frameNumber;
  stream.write(&landmarksPercept.numberOfFlags,sizeof(int));
  stream.write(&landmarksPercept.flags,sizeof(Flag) * landmarksPercept.numberOfFlags);
  stream.write(&landmarksPercept.numberOfGoals,sizeof(int));
  stream.write(&landmarksPercept.goals,sizeof(Goal) * landmarksPercept.numberOfGoals);
  stream.write(&landmarksPercept.cameraOffset,sizeof(landmarksPercept.cameraOffset));

  return stream;
}

/*
 * Change log :
 * 
 * $Log: LandmarksPercept.cpp,v $
 * Revision 1.2  2004/06/17 14:34:46  dassler
 * GT2004HeadControl updated
 * Now looks after propergated ball, followed up withe the communicated ball
 * GT2004HeadPathPlanner work now with time optimized moves
 * Middle Beacons removed of the Landmarkspercept
 *
 * Revision 1.1.1.1  2004/05/22 17:25:51  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.3  2004/01/19 14:53:46  dueffert
 * all frameNumbers (and not only some of them) are unsigned long now
 *
 * Revision 1.2  2003/11/12 16:19:35  goehring
 * frameNumber added to percepts
 *
 * Revision 1.1  2003/10/07 10:09:36  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.2  2003/09/26 15:27:27  juengel
 * Renamed DataTypes to representations.
 *
 * Revision 1.1.1.1  2003/07/02 09:40:22  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.7  2003/03/22 16:53:10  juengel
 * Added estimateOffsetForGoals.
 *
 * Revision 1.6  2003/03/15 13:28:01  juengel
 * Added  void addGoal(colorClass color,  bool ownTeamColorIsBlue,  const ConditionalBoundary& boundary);
 *
 * Revision 1.5  2003/03/07 11:35:01  juengel
 * removed  smoothedDistance;
 *
 * added void addFlag(Flag::FlagType type, bool ownTeamColorIsBlue, const ConditionalBoundary& boundary);
 * added void LandmarksPercept::estimateOffsetForFlags()
 *
 * Revision 1.4  2002/10/14 13:14:24  dueffert
 * doxygen comments corrected
 *
 * Revision 1.3  2002/09/22 18:40:49  risler
 * added new math functions, removed GTMath library
 *
 * Revision 1.2  2002/09/17 23:55:20  loetzsch
 * - unraveled several datatypes
 * - changed the WATCH macro
 * - completed the process restructuring
 *
 * Revision 1.1  2002/09/10 15:26:40  cvsadm
 * Created new project GT2003 (M.L.)
 * - Cleaned up the /Src/DataTypes directory
 * - Removed Challenge Code
 * - Removed processing of incoming audio data
 * - Renamed AcousticMessage to SoundRequest
 *
 * Revision 1.2  2002/08/30 13:33:05  dueffert
 * removed unused includes
 *
 * Revision 1.1.1.1  2002/05/10 12:40:13  cvsadm
 * Moved GT2002 Project from ute to tamara.
 *
 * Revision 1.6  2002/04/23 15:20:18  roefer
 * Number of goals and landmarks limited
 *
 * Revision 1.5  2002/04/02 13:10:18  dueffert
 * big change: odometryData and cameraMatrix in image now, old logfiles may be obsolete
 *
 * Revision 1.4  2002/02/13 22:43:02  roefer
 * First working versions of DefaultLandmarksPerceptor and MonteCarloSelfLocator
 *
 * Revision 1.3  2002/02/12 09:45:17  roefer
 * Progress in DefaultLandmarksPerceptor and MonteCarloSelfLocator
 *
 * Revision 1.2  2002/02/06 10:30:11  AndySHB
 * MonteCarloLocalization First Draft
 *
 * Revision 1.3  2001/12/10 17:47:06  risler
 * change log added
 *
 */

---