Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Representations/Perception/LandmarksPercept.cpp

Go to the documentation of this file.

    00001 /**
    00002  * @file Representations/Perception/LandmarksPercept.cpp
    00003  *
    00004  * Contains the implementation of class LandmarksPercept.
    00005  *
    00006  * @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
    00007  * @author <A href=mailto:asbre01@tzi.de>Andreas Sztybryc</A>
    00008  */
    00009
    00010 #include "Tools/Streams/InOut.h"
    00011 #include "Tools/FieldDimensions.h"
    00012 #include "LandmarksPercept.h"
    00013
    00014
    00015 enum FlagType
    00016 {
    00017   pinkAboveYellow, pinkAboveSkyblue,
    00018   yellowAbovePink, skyblueAbovePink
    00019 };
    00020
    00021 void Flag::setBottomColor()
    00022 {
    00023   switch(type)
    00024   {
    00025     case pinkAboveYellow:
    00026       bottomColor =  yellow;
    00027       return;
    00028     case pinkAboveSkyblue:
    00029       bottomColor = skyblue;
    00030       return;
    00031     default:
    00032       bottomColor = pink;
    00033   }
    00034 }
    00035
    00036 void Flag::setTopColor()
    00037 {
    00038   switch(type)
    00039   {
    00040     case yellowAbovePink:
    00041       topColor =  yellow;
    00042       return;
    00043     case skyblueAbovePink:
    00044       topColor = skyblue;
    00045       return;
    00046     default:
    00047       topColor = pink;
    00048   }
    00049 }
    00050
    00051 LandmarksPercept::LandmarksPercept()
    00052 {
    00053   reset(0);
    00054 }
    00055
    00056 void LandmarksPercept::reset(unsigned long frameNumber)
    00057 {
    00058   this->frameNumber = frameNumber;
    00059   numberOfFlags = numberOfGoals = 0;
    00060   viewAngle = 0;
    00061   viewAngleValid = false;
    00062 }
    00063
    00064 void LandmarksPercept::addFlag(Flag::FlagType type,
    00065                                const Vector2<double>& position,
    00066                                const ConditionalBoundary& boundary,
    00067                                const Vector2<int>& topLeft,
    00068                                const Vector2<int>& topRight,
    00069                                const Vector2<int>& bottomLeft,
    00070                                const Vector2<int>& bottomRight)
    00071 {
    00072   if(numberOfFlags < 4)
    00073   {
    00074     flags[numberOfFlags].type = type;
    00075     flags[numberOfFlags].position = position;
    00076     flags[numberOfFlags].topLeft = topLeft;
    00077     flags[numberOfFlags].topRight = topRight;
    00078     flags[numberOfFlags].bottomLeft = bottomLeft;
    00079     flags[numberOfFlags].bottomRight = bottomRight;
    00080     flags[numberOfFlags].setBottomColor();
    00081     flags[numberOfFlags].setTopColor();
    00082     (ConditionalBoundary&) flags[numberOfFlags++] = boundary;
    00083   }
    00084 }
    00085
    00086
    00087
    00088 void LandmarksPercept::addFlag
    00089 (
    00090  Flag::FlagType type,
    00091  bool ownTeamColorIsBlue,
    00092  const ConditionalBoundary& boundary,
    00093  const Vector2<int>& topLeft,
    00094  const Vector2<int>& topRight,
    00095  const Vector2<int>& bottomLeft,
    00096  const Vector2<int>& bottomRight
    00097 )
    00098 {
    00099   if(numberOfFlags < 4)
    00100   {
    00101     Vector2<double> flagPosition;
    00102
    00103     int flip = ownTeamColorIsBlue ? -1 : 1;
    00104
    00105     switch (type)
    00106     {
    00107     case Flag::pinkAboveYellow:
    00108       flagPosition.x = xPosBackFlags * flip;
    00109       flagPosition.y = yPosRightFlags * flip;
    00110       break;
    00111     case Flag::pinkAboveSkyblue:
    00112       flagPosition.x = xPosFrontFlags * flip;
    00113       flagPosition.y = yPosRightFlags * flip;
    00114       break;
    00115     case Flag::yellowAbovePink:
    00116       flagPosition.x = xPosBackFlags * flip;
    00117       flagPosition.y = yPosLeftFlags * flip;
    00118       break;
    00119     case Flag::skyblueAbovePink:
    00120       flagPosition.x = xPosFrontFlags * flip;
    00121       flagPosition.y = yPosLeftFlags * flip;
    00122       break;
    00123     }
    00124
    00125     flags[numberOfFlags].type = type;
    00126     flags[numberOfFlags].position = flagPosition;
    00127     flags[numberOfFlags].topLeft = topLeft;
    00128     flags[numberOfFlags].topRight = topRight;
    00129     flags[numberOfFlags].bottomLeft = bottomLeft;
    00130     flags[numberOfFlags].bottomRight = bottomRight;
    00131     flags[numberOfFlags].setBottomColor();
    00132     flags[numberOfFlags].setTopColor();
    00133     (ConditionalBoundary&) flags[numberOfFlags++] = boundary;
    00134   }
    00135 }
    00136
    00137 void LandmarksPercept::estimateOffsetForFlags
    00138 (
    00139  const Vector2<double>& cameraOffset
    00140  )
    00141 {
    00142   for(int i = 0; i < numberOfFlags; ++i)
    00143   {
    00144     Flag& flag = flags[i];
    00145
    00146     /** @todo improve, isOnBorder(flag.x.?) not checked */
    00147     double distance;
    00148     double direction = flag.x.getCenter();
    00149
    00150     if(!flag.isOnBorder(flag.y.min) && !flag.isOnBorder(flag.y.max))
    00151     {
    00152       if(flag.y.min != flag.y.max)
    00153       {
    00154         /* calculation of distance is based on the fact that flag is perpendicular to the ground */
    00155         distance = flagHeight / (tan(flag.y.max) - tan(flag.y.min)) + flagRadius;
    00156         flag.distanceValidity = 0.8;
    00157       }
    00158       else
    00159       {
    00160         distance = 4500;
    00161         flag.distanceValidity = 0.05;
    00162       }
    00163     }
    00164     else
    00165     {
    00166       distance = flagRadius / sin(flag.x.getSize() / 2);
    00167       if(!flag.isOnBorder(flag.x.min) && !flag.isOnBorder(flag.x.max)) // Flag touches no vertical border
    00168         flag.distanceValidity = 0.7;
    00169       else
    00170         flag.distanceValidity = 0.2;
    00171     }
    00172
    00173     if(!flag.isOnBorder(flag.x.min) && !flag.isOnBorder(flag.x.max)) // Flag touches no vertical border
    00174       flag.angleValidity = 0.8;
    00175     else
    00176       flag.angleValidity = 0.7;
    00177
    00178     Pose2D p = Pose2D(cameraOffset) + Pose2D(direction)
    00179                        + Pose2D(Vector2<double>(distance,0));
    00180 //    flag.distance = p.translation.abs();
    00181 //    flag.angle = atan2(p.translation.y,p.translation.x);
    00182
    00183       flag.distance = p.translation.abs();
    00184       flag.angle = direction;
    00185
    00186
    00187     if (flag.distance > 6000)
    00188     {
    00189       flag.distance = 6000;
    00190       flag.distanceValidity=0; // flags far away are meassured very bad
    00191     }
    00192     else if (flag.distance > 3000)
    00193       flag.distanceValidity*=0.5; // flags medium far away are meassured medium bad
    00194   }
    00195 }
    00196
    00197 void LandmarksPercept::addGoal(colorClass color,
    00198                                const ConditionalBoundary& boundary,
    00199                                const Vector2<int>& topLeft, const Vector2<int>& topRight,
    00200                                const Vector2<int>& bottomLeft, const Vector2<int>& bottomRight,
    00201                                double distance, double angle, double rotation,
    00202                                double distanceValidity, double angleValidity)
    00203 {
    00204   // There is only memory for two goals
    00205   if(numberOfGoals >= 2)
    00206     return;
    00207
    00208   // Calculate the positions of the seen goal on the field (field coordinate system depends on team colour)
    00209   Vector2<double> leftGoalpost, rightGoalpost;
    00210   colorClass ownGoalColor = (getPlayer().getTeamColor() == Player::blue) ? skyblue : yellow;
    00211   if (color==ownGoalColor)
    00212   {
    00213     leftGoalpost.x = xPosOwnGoalpost;
    00214     leftGoalpost.y = yPosRightGoal;
    00215     rightGoalpost.x = xPosOwnGoalpost;
    00216     rightGoalpost.y = yPosLeftGoal;
    00217   }
    00218   else
    00219   {
    00220     leftGoalpost.x = xPosOpponentGoalpost;
    00221     leftGoalpost.y = yPosLeftGoal;
    00222     rightGoalpost.x = xPosOpponentGoalpost;
    00223     rightGoalpost.y = yPosRightGoal;
    00224   }
    00225
    00226   // Add goal percept
    00227   Goal& goal = goals[numberOfGoals++];
    00228
    00229   goal.color = color;
    00230   goal.leftPost = leftGoalpost;
    00231   goal.rightPost = rightGoalpost;
    00232   goal.topLeft = topLeft;
    00233   goal.topRight = topRight;
    00234   goal.bottomLeft = bottomLeft;
    00235   goal.bottomRight = bottomRight;
    00236   (ConditionalBoundary&) goal = boundary;
    00237   goal.distance = distance;
    00238   goal.angle = angle;
    00239   goal.rotation = rotation;
    00240   goal.angleValidity = angleValidity;
    00241   goal.distanceValidity = distanceValidity;
    00242 }
    00243
    00244 void LandmarksPercept::addGoal(colorClass color,
    00245                                const ConditionalBoundary& boundary,
    00246                                const Vector2<int>& topLeft,
    00247                                const Vector2<int>& topRight,
    00248                                const Vector2<int>& bottomLeft,
    00249                                const Vector2<int>& bottomRight,
    00250                                const CameraMatrix& cameraMatrix)
    00251 {
    00252   if (numberOfGoals < 2)
    00253   {
    00254     addGoal(color, boundary, topLeft, topRight, bottomLeft, bottomRight, 0, 0, 0, 0, 0);
    00255     estimateOffsetAndValiditiesForGoal(goals[numberOfGoals-1], cameraMatrix);
    00256   }
    00257 }
    00258
    00259 void LandmarksPercept::estimateOffsetAndValiditiesForGoal(Goal& goal, const CameraMatrix& cameraMatrix)
    00260 {
    00261     Vector2<double> cameraOffset(cameraMatrix.translation.x, cameraMatrix.translation.y);
    00262
    00263     double distance, distanceByHeight,distanceByWidth;
    00264     if(goal.y.max != goal.y.min)
    00265       /* calculation of distance is based on the fact that goal is perpendicular to the ground */
    00266       distanceByHeight = goalHeight / (tan(goal.y.max) - tan(goal.y.min));
    00267     else
    00268       distanceByHeight = 10000;
    00269
    00270     if(goal.x.max != goal.x.min)
    00271       /* calculation assumes that goal is perpendicular to the view direction */
    00272       distanceByWidth = goalWidth / (2 * (tan((goal.x.max - goal.x.min)/2)));
    00273     else
    00274       distanceByWidth = 10000;
    00275
    00276     if( distanceByHeight < distanceByWidth)
    00277       distance = distanceByHeight;
    00278     else
    00279       distance = distanceByWidth;
    00280
    00281     double direction = goal.x.getCenter();
    00282     Pose2D p = Pose2D(cameraOffset) + Pose2D(direction)
    00283                        + Pose2D(Vector2<double>(distance,0));
    00284     goal.distance = p.translation.abs();
    00285 //    goal.angle = atan2(p.translation.y,p.translation.x);
    00286     goal.angle = direction;
    00287     goal.rotation = 0; // wrong!
    00288     if(!goal.isOnBorder(goal.x.min) && !goal.isOnBorder(goal.x.max)) // goal touches no vertical borders
    00289     {
    00290       goal.angleValidity = 0.80;
    00291       goal.distanceValidity = 0.50;
    00292     }
    00293     else
    00294       if (goal.isOnBorder(goal.x.min) && goal.isOnBorder(goal.x.max)) // goal touches both vertical borders
    00295       {
    00296         goal.angleValidity = 0.20;
    00297         goal.distanceValidity = 0.10;
    00298       }
    00299       else
    00300       {
    00301         goal.angleValidity = 0.5;
    00302         goal.distanceValidity = 0.15;
    00303       }
    00304
    00305     if (goal.distance > xPosOpponentFieldBorder * 1.2)
    00306     {
    00307       goal.distanceValidity=0; // goals far away are meassured very bad
    00308     }
    00309     else if (goal.distance > xPosOpponentFieldBorder * 0.5)
    00310       goal.distanceValidity*=0.5; // goals medium far away are meassured medium bad
    00311
    00312 }