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

# Modules/ImageProcessor/ImageProcessorTools/GoalRecognizer.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GoalRecognizer.cpp
    00003 *
    00004 * Implementation of class GoalRecognizer
    00005 *
    00006 * @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
    00007 */
    00008
    00009 #include "GoalRecognizer.h"
    00010 #include "Tools/Player.h"
    00011 #include "Tools/FieldDimensions.h"
    00012 #include "Tools/RangeArray.h"
    00013 #include "Representations/Perception/ColorTable64.h"
    00014
    00015 #include "ColorTableReferenceColor.h"
    00016
    00017 //color table 64
    00018 //~ #define COLOR_CLASS(y,u,v) (colorClass)((const ColorTable64&) colorTable).colorClasses[(y) >> 2][(u) >> 2][(v) >> 2]
    00019 #define COLOR_CLASS(y,u,v) colorTable.getColorClass(y,u,v)
    00020
    00021 GoalRecognizer::GoalRecognizer
    00022 (
    00023  const Image& image,
    00024  const CameraMatrix& cameraMatrix,
    00025  const ColorTable& colorTable,
    00026  int goalIndicationAboveHorizon,
    00027  int goalIndicationBelowHorizon,
    00028  ObstaclesPercept& obstaclesPercept,
    00029  LandmarksPercept& landmarksPercept
    00030  )
    00031  :
    00032 image(image),
    00033 cameraMatrix(cameraMatrix),
    00034 colorTable(colorTable),
    00035 goalIndicationAboveHorizon(goalIndicationAboveHorizon),
    00036 goalIndicationBelowHorizon(goalIndicationBelowHorizon),
    00037 obstaclesPercept(obstaclesPercept),
    00038 landmarksPercept(landmarksPercept)
    00039 {
    00040   useFixedScanLines = false;
    00041 }
    00042
    00043 GoalRecognizer::GoalRecognizer
    00044 (
    00045  const Image& image,
    00046  const CameraMatrix& cameraMatrix,
    00047  const ColorTable& colorTable,
    00048  ObstaclesPercept& obstaclesPercept,
    00049  LandmarksPercept& landmarksPercept
    00050  )
    00051  :
    00052 image(image),
    00053 cameraMatrix(cameraMatrix),
    00054 colorTable(colorTable),
    00055 obstaclesPercept(obstaclesPercept),
    00056 landmarksPercept(landmarksPercept)
    00057 {
    00058   useFixedScanLines = true;
    00059 }
    00060
    00061
    00062 GoalRecognizer::~GoalRecognizer()
    00063 {
    00064 }
    00065
    00066 void GoalRecognizer::getGoalPercept(LandmarksPercept& landmarksPercept)
    00067 {
    00068   if(getPlayer().getTeamColor() == Player::blue)
    00069   {
    00070     colorOfOpponentGoal = yellow;
    00071     colorOfOwnGoal = skyblue;
    00072   }
    00073   else
    00074   {
    00075     colorOfOpponentGoal = skyblue;
    00076     colorOfOwnGoal = yellow;
    00077   }
    00078
    00079   INIT_DEBUG_IMAGE(imageProcessorGoals, image);
    00080
    00081   horizonLine = Geometry::calculateHorizon(cameraMatrix, image.cameraInfo);
    00082   verticalLine.base = horizonLine.base;
    00083   verticalLine.direction.x = - horizonLine.direction.y;
    00084   verticalLine.direction.y = horizonLine.direction.x;
    00085
    00086   if(useFixedScanLines)
    00087     calculateScanLinesParallelToHorizon();
    00088   else
    00089     calculateScanLinesParallelToHorizon(goalIndicationAboveHorizon, goalIndicationBelowHorizon, 7);
    00090
    00091   scanHorizontalForGoals();
    00092   calculateVerticalGoalScanLines();
    00093   scanLinesForGoals();
    00094
    00095   SEND_DEBUG_IMAGE(imageProcessorGoals);
    00096 }
    00097
    00098 void GoalRecognizer::calculateScanLinesParallelToHorizon()
    00099 {
    00100   numberOfHorizontalScanLines = 0;
    00101
    00102   Geometry::Line lineAboveHorizon;
    00103   lineAboveHorizon.direction = horizonLine.direction;
    00104
    00105   int i;
    00106
    00107   Vector2<int> bottomLeft(0,0);
    00108   Vector2<int> topRight(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3);
    00109
    00110   for(i = 14; i >= 0; i--)
    00111   {
    00112     lineAboveHorizon.base = horizonLine.base + verticalLine.direction * ( (i * 4.5) - 25 );
    00113
    00114     if(Geometry::getIntersectionPointsOfLineAndRectangle(
    00115       bottomLeft, topRight, lineAboveHorizon,
    00116       leftPoint[numberOfHorizontalScanLines],
    00117       rightPoint[numberOfHorizontalScanLines])
    00118       )
    00119     {
    00120       numberOfHorizontalScanLines++;
    00121     }
    00122   }
    00123 }
    00124
    00125 void GoalRecognizer::calculateScanLinesParallelToHorizon
    00126 (
    00127  int distanceAboveHorizon,
    00128  int distanceBelowHorizon,
    00129  int numberOfScanLines
    00130  )
    00131 {
    00132   numberOfHorizontalScanLines = 0;
    00133
    00134   Geometry::Line scanLine;
    00135   scanLine.direction = horizonLine.direction;
    00136
    00137   int i;
    00138
    00139   Vector2<int> bottomLeft(0,0);
    00140   Vector2<int> topRight(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3);
    00141
    00142   for(i = 0; i < numberOfScanLines; i++)
    00143   {
    00144     scanLine.base =
    00145       horizonLine.base +
    00146       verticalLine.direction * -(distanceBelowHorizon + (distanceAboveHorizon - distanceBelowHorizon) * i / (numberOfScanLines - 1));
    00147 //      verticalLine.direction * distanceBelowHorizon;// - (-distanceAboveHorizon + distanceBelowHorizon) * i / numberOfScanLines);
    00148
    00149     if(Geometry::getIntersectionPointsOfLineAndRectangle(
    00150       bottomLeft, topRight, scanLine,
    00151       leftPoint[numberOfHorizontalScanLines],
    00152       rightPoint[numberOfHorizontalScanLines])
    00153       )
    00154     {
    00155       numberOfHorizontalScanLines++;
    00156     }
    00157   }
    00158 }
    00159
    00160 void GoalRecognizer::scanHorizontalForGoals()
    00161 {
    00162   enum {yellowGoal = 1, skyblueGoal = 0, numberOfGoalColors = 2};
    00163   int colorToCheck;
    00164   colorClass colorClassToCheck;
    00165
    00166   RangeArray<double> goalClusters[numberOfGoalColors];
    00167   numberOfGoalIndications = 0;
    00168
    00169   // variables needed to determine free part of goal
    00170   RangeArray<double> goalClustersForFreePart[numberOfGoalColors];
    00171   int numberOfLinesWithLargeParts[numberOfGoalColors] = {0, 0};
    00172
    00173   int i;
    00174   for(i = 0; i < numberOfHorizontalScanLines; i++)
    00175   {
    00176     Geometry::PixeledLine line(leftPoint[i], rightPoint[i]);
    00177
    00178     // state machine
    00179     // states
    00180     enum { noGoalColorFound,
    00181       foundBeginOfGoalColor,
    00182       foundEndOfGoalColor
    00183     } goalColorState[numberOfGoalColors] = {noGoalColorFound,noGoalColorFound};
    00184     // counter
    00185     int numberOfPixelsSinceLastOccurrenceOfGoalColor[numberOfGoalColors];
    00186     int numberOfPixelsSinceFirstOccurrenceOfGoalColor[numberOfGoalColors] = {0,0};
    00187
    00188     bool colorStartsAtBeginOfImage[numberOfGoalColors] = {false, false};
    00189     ColoredPartsCheck goalCheck[numberOfGoalColors];
    00190
    00191     int x = 0, y = 0;
    00192     colorClass color;
    00193
    00194     if(line.getNumberOfPixels() > 3)
    00195     {
    00196       for(int z = 2; z < line.getNumberOfPixels(); z++)
    00197       {
    00198         x = line.getPixelX(z);
    00199         y = line.getPixelY(z);
    00200
    00201         color = COLOR_CLASS(image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]);
    00202
    00203         for(colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
    00204         {
    00205           if(colorToCheck == yellowGoal) colorClassToCheck = yellow;
    00206           else colorClassToCheck = skyblue;
    00207
    00208           if(goalColorState[colorToCheck] == noGoalColorFound)
    00209           {
    00210             DEBUG_IMAGE_SET_PIXEL_BLACK(imageProcessorGoals, x, y + colorToCheck);
    00211             if(color == colorClassToCheck)
    00212             {
    00213               goalCheck[colorToCheck].numberOfColoredPixels = 0;
    00214               goalCheck[colorToCheck].rangeOfColor = 0;
    00215               if(numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck] == 0)
    00216               {
    00217                 goalCheck[colorToCheck].firstPoint.x = x;
    00218                 goalCheck[colorToCheck].firstPoint.y = y;
    00219               }
    00220               numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck]++;
    00221               if(numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck] > 2)
    00222               {
    00223                 goalColorState[colorToCheck] = foundBeginOfGoalColor;
    00224                 if(z < 5)
    00225                 {
    00226                   DOT(imageProcessor_general, x, y, Drawings::green, Drawings::green);
    00227                   colorStartsAtBeginOfImage[colorToCheck] = true;
    00228                 }
    00229               }
    00230             }
    00231             else
    00232             {
    00233               numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck] = 0;
    00234             }
    00235           }
    00236           else if(goalColorState[colorToCheck] == foundBeginOfGoalColor)
    00237           {
    00238             DEBUG_IMAGE_SET_PIXEL_GRAY(imageProcessorGoals, x, y + colorToCheck);
    00239             if(color == colorClassToCheck) goalCheck[colorToCheck].numberOfColoredPixels++;
    00240             else
    00241             {
    00242               goalCheck[colorToCheck].lastPoint.x = x;
    00243               goalCheck[colorToCheck].lastPoint.y = y;
    00244               goalColorState[colorToCheck] = foundEndOfGoalColor;
    00245               numberOfPixelsSinceLastOccurrenceOfGoalColor[colorToCheck] = 0;
    00246               numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck] = 0;
    00247             }
    00248             goalCheck[colorToCheck].rangeOfColor++;
    00249           }
    00250           else if(goalColorState[colorToCheck] == foundEndOfGoalColor)
    00251           {
    00252             DEBUG_IMAGE_SET_PIXEL_WHITE(imageProcessorGoals, x, y + colorToCheck);
    00253             numberOfPixelsSinceLastOccurrenceOfGoalColor[colorToCheck]++;
    00254             if(color == colorClassToCheck)
    00255             {
    00256               numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck]++;
    00257               if(numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck] > 2)
    00258               {
    00259                 goalColorState[colorToCheck] = foundBeginOfGoalColor;
    00260               }
    00261             }
    00262             else if(
    00263               numberOfPixelsSinceLastOccurrenceOfGoalColor[colorToCheck] > 5
    00264               ||
    00265               color == white
    00266               )
    00267             {
    00268               goalColorState[colorToCheck] = noGoalColorFound;
    00269               goalCheck[colorToCheck].determineLargePart(10, colorStartsAtBeginOfImage[colorToCheck], false, cameraMatrix, image.cameraInfo);
    00270               DOT(imageProcessor_general, x, y, Drawings::green, Drawings::white);
    00271               colorStartsAtBeginOfImage[colorToCheck] = false;
    00272             }
    00273             else
    00274             {
    00275               numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck] = 0;
    00276             }
    00277             goalCheck[colorToCheck].rangeOfColor++;
    00278           }
    00279         }//for(int colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
    00280       }//for(int z = 2; z < line.getNumberOfPixels(); z++)
    00281
    00282       for(colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
    00283       {
    00284         if(goalColorState[colorToCheck] == foundBeginOfGoalColor || goalColorState[colorToCheck] == foundEndOfGoalColor)
    00285         {
    00286           if(goalColorState[colorToCheck] == foundBeginOfGoalColor)
    00287           {
    00288             goalCheck[colorToCheck].lastPoint.x = x;
    00289             goalCheck[colorToCheck].lastPoint.y = y;
    00290           }
    00291           goalCheck[colorToCheck].determineLargePart(10, colorStartsAtBeginOfImage[colorToCheck], true, cameraMatrix, image.cameraInfo);
    00292         }//if(goalColorState == foundBeginOfGoalColor || goalColorState == foundEndOfGoalColor)
    00293       }//for(colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
    00294     }//if(line.getNumberOfPixels() > 3)
    00295
    00296     for(colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
    00297     {
    00298       // Add all large parts found on that line to the structure that determines clusters.
    00299       if(goalCheck[colorToCheck].numberOfLargeParts > 0) numberOfLinesWithLargeParts[colorToCheck]++;
    00300       int index;
    00301       for(index = 0; index < goalCheck[colorToCheck].numberOfLargeParts; index++)
    00302       {
    00303         goalClusters[colorToCheck].add(
    00304           Range<double>(goalCheck[colorToCheck].largePartEndAngles[index].x, goalCheck[colorToCheck].largePartBeginAngles[index].x),
    00305           goalCheck[colorToCheck].largePartBeginIsOnBorder[index],
    00306           goalCheck[colorToCheck].largePartEndIsOnBorder[index]
    00307           );
    00308         if(numberOfLinesWithLargeParts[colorToCheck] > 2 && numberOfLinesWithLargeParts[colorToCheck] < 5)
    00309         {
    00310           goalClustersForFreePart[colorToCheck].add(
    00311             Range<double>(goalCheck[colorToCheck].largePartEndAngles[index].x, goalCheck[colorToCheck].largePartBeginAngles[index].x),
    00312             goalCheck[colorToCheck].largePartBeginIsOnBorder[index],
    00313             goalCheck[colorToCheck].largePartEndIsOnBorder[index]
    00314             );
    00315         }
    00316       }
    00317     }
    00318   }// for(int i = 0; i < numberOfHorizontalScanLines; i++)
    00319
    00320   for(colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
    00321   {
    00322     if(colorToCheck == yellowGoal) colorClassToCheck = yellow;
    00323     else colorClassToCheck = skyblue;
    00324
    00325     // Add one goal indication for each cluster
    00326     int index;
    00327     for(index = 0; index < goalClusters[colorToCheck].getNumberOfClusters(); index++)
    00328     {
    00329       Range<double> currentRange = goalClusters[colorToCheck].getCluster(index);
    00330       Vector2<int> leftPoint, rightPoint;
    00331       Geometry::calculatePointByAngles(Vector2<double>(currentRange.max,0.02 * index), cameraMatrix, image.cameraInfo, leftPoint);
    00332       Geometry::calculatePointByAngles(Vector2<double>(currentRange.min,0.02 * index), cameraMatrix, image.cameraInfo, rightPoint);
    00333       LINE(imageProcessor_flagsAndGoals, leftPoint.x, leftPoint.y, rightPoint.x, rightPoint.y, 2, Drawings::ps_solid, Drawings::blue);
    00334       goalIndicationLeft[numberOfGoalIndications]   = leftPoint;
    00335       goalIndicationRight[numberOfGoalIndications]  = rightPoint;
    00336       goalIndicationCenter[numberOfGoalIndications] = (leftPoint + rightPoint) / 2;
    00337       leftOfGoalIndicationIsOnBorder[numberOfGoalIndications] = goalClusters[colorToCheck].isLeftOnBorder(index);
    00338       rightOfGoalIndicationIsOnBorder[numberOfGoalIndications] = goalClusters[colorToCheck].isRightOnBorder(index);
    00339       colorOfGoalIndication[numberOfGoalIndications] = colorClassToCheck;
    00340       numberOfGoalIndications++;
    00341     }
    00342
    00343     // Determine the free part of the goal
    00344     for(index = 0; index < goalClustersForFreePart[colorToCheck].getNumberOfClusters(); index++)
    00345     {
    00346       Range<double> currentRange = goalClustersForFreePart[colorToCheck].getCluster(index);
    00347       Vector2<int> leftPoint, rightPoint;
    00348       Geometry::calculatePointByAngles(Vector2<double>(currentRange.min,-0.1 + 0.02 * index), cameraMatrix, image.cameraInfo, leftPoint);
    00349       Geometry::calculatePointByAngles(Vector2<double>(currentRange.max,-0.1 + 0.02 * index), cameraMatrix, image.cameraInfo, rightPoint);
    00350       LINE(imageProcessor_flagsAndGoals, leftPoint.x, leftPoint.y, rightPoint.x, rightPoint.y, 2, Drawings::ps_solid, Drawings::green);
    00351     }
    00352
    00353     double center, angle;
    00354     bool determinedFreePartOfGoal = false;
    00355     if(goalClustersForFreePart[colorToCheck].getNumberOfClusters() == 1)
    00356     {
    00357       bool leftOfPartIsOnBorder = goalClustersForFreePart[colorToCheck].isLeftOnBorder(0);
    00358       bool rightOfPartIsOnBorder = goalClustersForFreePart[colorToCheck].isRightOnBorder(0);
    00359       Range<double> freePartRange = goalClustersForFreePart[colorToCheck].getCluster(0);
    00360       if(!leftOfPartIsOnBorder && !rightOfPartIsOnBorder || freePartRange.getSize() > 0.7 * image.cameraInfo.openingAngleWidth)
    00361       {
    00362         determinedFreePartOfGoal = true;
    00363         center = (freePartRange.min + freePartRange.max) / 2.0;
    00364         angle = freePartRange.max - freePartRange.min;
    00365       }
    00366     }
    00367     else if(goalClustersForFreePart[colorToCheck].getNumberOfClusters() == 2)
    00368     {
    00369       Range<double> freePartRange[2];
    00370       freePartRange[0] = goalClustersForFreePart[colorToCheck].getCluster(0);
    00371       freePartRange[1] = goalClustersForFreePart[colorToCheck].getCluster(1);
    00372
    00373       bool freePartIsSeenCompletely[2];
    00374
    00375       freePartIsSeenCompletely[0] =
    00376         !goalClustersForFreePart[colorToCheck].isLeftOnBorder(0) &&
    00377         !goalClustersForFreePart[colorToCheck].isRightOnBorder(0);
    00378
    00379       freePartIsSeenCompletely[1] =
    00380         !goalClustersForFreePart[colorToCheck].isLeftOnBorder(1) &&
    00381         !goalClustersForFreePart[colorToCheck].isRightOnBorder(1);
    00382
    00383       int largePart, smallPart;
    00384       if(freePartRange[0].getSize() < freePartRange[1].getSize())
    00385       {
    00386         largePart = 1;
    00387         smallPart = 0;
    00388       }
    00389       else
    00390       {
    00391         largePart = 0;
    00392         smallPart = 1;
    00393       }
    00394
    00395       if(
    00396         freePartIsSeenCompletely[largePart] && freePartIsSeenCompletely[smallPart] ||
    00397         !freePartIsSeenCompletely[largePart] && freePartIsSeenCompletely[smallPart]
    00398         )
    00399       {
    00400         determinedFreePartOfGoal = true;
    00401         center = (freePartRange[largePart].min + freePartRange[largePart].max) / 2.0;
    00402         angle = freePartRange[largePart].max - freePartRange[largePart].min;
    00403       }
    00404     }
    00405
    00406     if(determinedFreePartOfGoal)
    00407     {
    00408       if(colorClassToCheck == colorOfOpponentGoal)
    00409       {
    00410         obstaclesPercept.angleToFreePartOfGoal[ObstaclesPercept::opponentGoal] = center;
    00411         obstaclesPercept.widthOfFreePartOfGoal[ObstaclesPercept::opponentGoal] = angle;
    00412         obstaclesPercept.angleToFreePartOfGoalWasDetermined[ObstaclesPercept::opponentGoal] = true;
    00413       }
    00414       else
    00415       {
    00416         obstaclesPercept.angleToFreePartOfGoal[ObstaclesPercept::ownGoal] = center;
    00417         obstaclesPercept.widthOfFreePartOfGoal[ObstaclesPercept::ownGoal] = angle;
    00418         obstaclesPercept.angleToFreePartOfGoalWasDetermined[ObstaclesPercept::ownGoal] = true;
    00419       }
    00420     }
    00421
    00422   }//for(colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
    00423 }//void GridImageProcessor2::scanHorizontalForFlagsAndGoals()
    00424
    00425 void GoalRecognizer::calculateVerticalGoalScanLines()
    00426 {
    00427   numberOfGoalScanLines = 0;
    00428   int i;
    00429   Vector2<double> anglesForGoalScanLine;
    00430   for(i = 0; i < numberOfGoalIndications; i++)
    00431   {
    00432     // Line through center of indication
    00433     Geometry::calculateAnglesForPoint(goalIndicationCenter[i], cameraMatrix, image.cameraInfo, anglesForGoalScanLine);
    00434     anglesForGoalScanLine.y = -1.0;
    00435     Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, bottomGoalPoint[numberOfGoalScanLines]);
    00436     anglesForGoalScanLine.y =  0.3;
    00437     Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, topGoalPoint[numberOfGoalScanLines]);
    00438
    00439     if(Geometry::clipLineWithRectangleCohenSutherland(Vector2<int>(0,0), Vector2<int>(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3),
    00440       topGoalPoint[numberOfGoalScanLines], bottomGoalPoint[numberOfGoalScanLines]))
    00441     {
    00442       colorOfGoalScanLine[numberOfGoalScanLines] = colorOfGoalIndication[i];
    00443       indexOfGoalIndication[numberOfGoalScanLines] = i;
    00444       scanLineToCheckBestAngle[numberOfGoalScanLines] = false;
    00445       numberOfGoalScanLines++;
    00446     }
    00447
    00448     // Line at the left side
    00449     Geometry::calculateAnglesForPoint((goalIndicationLeft[i] + goalIndicationCenter[i]) / 2, cameraMatrix, image.cameraInfo, anglesForGoalScanLine);
    00450     anglesForGoalScanLine.y = -1.0;
    00451     Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, bottomGoalPoint[numberOfGoalScanLines]);
    00452     anglesForGoalScanLine.y =  0.3;
    00453     Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, topGoalPoint[numberOfGoalScanLines]);
    00454
    00455     if(Geometry::clipLineWithRectangleCohenSutherland(Vector2<int>(0,0), Vector2<int>(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3),
    00456       topGoalPoint[numberOfGoalScanLines], bottomGoalPoint[numberOfGoalScanLines]))
    00457     {
    00458       colorOfGoalScanLine[numberOfGoalScanLines] = colorOfGoalIndication[i];
    00459       indexOfGoalIndication[numberOfGoalScanLines] = i;
    00460       scanLineToCheckBestAngle[numberOfGoalScanLines] = false;
    00461       numberOfGoalScanLines++;
    00462     }
    00463
    00464     // Line at the right side
    00465     Geometry::calculateAnglesForPoint((goalIndicationRight[i] + goalIndicationCenter[i]) / 2, cameraMatrix, image.cameraInfo, anglesForGoalScanLine);
    00466     anglesForGoalScanLine.y = -1.0;
    00467     Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, bottomGoalPoint[numberOfGoalScanLines]);
    00468     anglesForGoalScanLine.y =  0.3;
    00469     Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, topGoalPoint[numberOfGoalScanLines]);
    00470
    00471     if(Geometry::clipLineWithRectangleCohenSutherland(Vector2<int>(0,0), Vector2<int>(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3),
    00472       topGoalPoint[numberOfGoalScanLines], bottomGoalPoint[numberOfGoalScanLines]))
    00473     {
    00474       colorOfGoalScanLine[numberOfGoalScanLines] = colorOfGoalIndication[i];
    00475       indexOfGoalIndication[numberOfGoalScanLines] = i;
    00476       scanLineToCheckBestAngle[numberOfGoalScanLines] = false;
    00477       numberOfGoalScanLines++;
    00478     }
    00479
    00480   }
    00481
    00482   // line for the free part of the goal
    00483   for(i = 0; i < 2; i++)
    00484   {
    00485     if(obstaclesPercept.angleToFreePartOfGoalWasDetermined[i] == true)
    00486     {
    00487       anglesForGoalScanLine.x = obstaclesPercept.angleToFreePartOfGoal[i];
    00488       anglesForGoalScanLine.y = -1.0;
    00489       Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, bottomGoalPoint[numberOfGoalScanLines]);
    00490       anglesForGoalScanLine.y =  0.3;
    00491       Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, topGoalPoint[numberOfGoalScanLines]);
    00492
    00493       if(Geometry::clipLineWithRectangleCohenSutherland(Vector2<int>(0,0), Vector2<int>(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3),
    00494         topGoalPoint[numberOfGoalScanLines], bottomGoalPoint[numberOfGoalScanLines]))
    00495       {
    00496         if(i==1) colorOfGoalScanLine[numberOfGoalScanLines] = colorOfOpponentGoal;
    00497         else colorOfGoalScanLine[numberOfGoalScanLines] = colorOfOwnGoal;
    00498         scanLineToCheckBestAngle[numberOfGoalScanLines] = true;
    00499         numberOfGoalScanLines++;
    00500       }
    00501     }
    00502   }
    00503 }// calculateVerticalGoalScanLines()
    00504
    00505 void GoalRecognizer::scanLinesForGoals()
    00506 {
    00507   ConditionalBoundary yellowGoalBoundary, skyblueGoalBoundary;
    00508   ConditionalBoundary boundaryOfIndication[maxNumberOfGoalScanLines];
    00509   bool useBoundaryOfIndication[maxNumberOfGoalScanLines];
    00510   int i;
    00511   for(i = 0; i < numberOfGoalIndications; i++) useBoundaryOfIndication[i] = true;
    00512
    00513   bool foundYellowGoal = false;
    00514   bool foundSkyblueGoal = false;
    00515   bool pixelHasGoalColor;
    00516
    00517   for(i = 0; i < numberOfGoalScanLines; i++)
    00518   {
    00519     Geometry::PixeledLine line(topGoalPoint[i], bottomGoalPoint[i]);
    00520
    00521     // state machine
    00522     // states
    00523     enum { noGoalColorFound,
    00524       foundBeginOfGoalColor,
    00525       foundEndOfGoalColor,
    00526       determinedGoal
    00527     } goalColorState = noGoalColorFound;
    00528     // counter
    00529     int numberOfPixelsSinceLastOccurrenceOfGoalColor = 0;
    00530     int numberOfPixelsSinceFirstOccurrenceOfGoalColor = 0;
    00531     int whiteAndPinkCounter = 0;
    00532
    00533     bool colorStartsAtBeginOfImage = false;
    00534
    00535     ColoredPartsCheck goalCheck;
    00536
    00537     int x = 0, y = 0;
    00538     colorClass color;
    00539
    00540     if(line.getNumberOfPixels() > 3)
    00541     {
    00542       bool scanLineIsPartOfGoal = true;
    00543       for(int z = 2; z < line.getNumberOfPixels(); z++)
    00544       {
    00545         pixelHasGoalColor = false;
    00546         x = line.getPixelX(z);
    00547         y = line.getPixelY(z);
    00548
    00549         if(scanLineToCheckBestAngle[i]){
    00550           DEBUG_IMAGE_SET_PIXEL_GRAY(imageProcessorGoals, x, y);
    00551         }
    00552         else {DEBUG_IMAGE_SET_PIXEL_WHITE(imageProcessorGoals, x, y);}
    00553
    00554         color = COLOR_CLASS(image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]);
    00555
    00556         if(colorOfGoalScanLine[i] == skyblue && color == skyblue) pixelHasGoalColor = true;
    00557         if(colorOfGoalScanLine[i] == yellow && color == yellow) pixelHasGoalColor = true;
    00558
    00559         if(goalColorState == noGoalColorFound)
    00560         {
    00561           if(pixelHasGoalColor)
    00562           {
    00563             DEBUG_IMAGE_SET_PIXEL_BLACK(imageProcessorGoals, x, y);
    00564             goalCheck.numberOfColoredPixels = 0;
    00565             goalCheck.rangeOfColor = 0;
    00566             if(numberOfPixelsSinceFirstOccurrenceOfGoalColor == 0)
    00567             {
    00568               goalCheck.firstPoint.x = x;
    00569               goalCheck.firstPoint.y = y;
    00570             }
    00571             numberOfPixelsSinceFirstOccurrenceOfGoalColor++;
    00572             if(numberOfPixelsSinceFirstOccurrenceOfGoalColor > 2)
    00573             {
    00574               goalColorState = foundBeginOfGoalColor;
    00575               if(z < 5) colorStartsAtBeginOfImage = true;
    00576             }
    00577           }
    00578           else
    00579           {
    00580             numberOfPixelsSinceFirstOccurrenceOfGoalColor = 0;
    00581           }
    00582         }
    00583         else if(goalColorState == foundBeginOfGoalColor)
    00584         {
    00585           DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorGoals, x, y);
    00586           if(pixelHasGoalColor) goalCheck.numberOfColoredPixels++;
    00587           else
    00588           {
    00589             goalCheck.lastPoint.x = x;
    00590             goalCheck.lastPoint.y = y;
    00591             goalColorState = foundEndOfGoalColor;
    00592             numberOfPixelsSinceLastOccurrenceOfGoalColor = 0;
    00593             numberOfPixelsSinceFirstOccurrenceOfGoalColor = 0;
    00594           }
    00595           goalCheck.rangeOfColor++;
    00596         }
    00597         else if(goalColorState == foundEndOfGoalColor)
    00598         {
    00599           numberOfPixelsSinceLastOccurrenceOfGoalColor++;
    00600           DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorGoals, x, y);
    00601           if(pixelHasGoalColor)
    00602           {
    00603             numberOfPixelsSinceFirstOccurrenceOfGoalColor++;
    00604             if(numberOfPixelsSinceFirstOccurrenceOfGoalColor > 2)
    00605             {
    00606               goalColorState = foundBeginOfGoalColor;
    00607             }
    00608           }
    00609           else if(numberOfPixelsSinceLastOccurrenceOfGoalColor > 20)
    00610           {
    00611             DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorGoals, x, y);
    00612 //            goalColorState = noGoalColorFound;
    00613             goalColorState = determinedGoal;
    00614             if(goalCheck.determineLargePart(7, colorStartsAtBeginOfImage, false, cameraMatrix, image.cameraInfo))
    00615             {
    00616               if(whiteAndPinkCounter > 5)
    00617               {
    00618                 scanLineIsPartOfGoal = false;
    00619                 DOT(imageProcessor_general, x, y, Drawings::blue, Drawings::blue);
    00620               }
    00621             }
    00622             whiteAndPinkCounter = 0;
    00623             colorStartsAtBeginOfImage = false;
    00624           }
    00625           else if(color == white || color == pink)
    00626           {
    00627             DEBUG_IMAGE_SET_PIXEL_PINK(imageProcessorGoals, x, y);
    00628             whiteAndPinkCounter++;
    00629           }
    00630           else
    00631           {
    00632             numberOfPixelsSinceFirstOccurrenceOfGoalColor = 0;
    00633           }
    00634           goalCheck.rangeOfColor++;
    00635         }
    00636       }//for(int z = 0; z < line.getNumberOfPixels(); z++)
    00637       if(goalColorState == foundBeginOfGoalColor || goalColorState == foundEndOfGoalColor)
    00638       {
    00639         if(goalColorState == foundBeginOfGoalColor)
    00640         {
    00641           goalCheck.lastPoint.x = x;
    00642           goalCheck.lastPoint.y = y;
    00643         }
    00644         goalCheck.determineLargePart(7, colorStartsAtBeginOfImage, true, cameraMatrix,image.cameraInfo);
    00645       }//if(goalColorState == foundBeginOfGoalColor || goalColorState == foundEndOfGoalColor)
    00646
    00647       if(!scanLineIsPartOfGoal && !scanLineToCheckBestAngle[i])
    00648         useBoundaryOfIndication[indexOfGoalIndication[i]] = false;
    00649
    00650       if(goalCheck.numberOfLargeParts > 0)
    00651       {
    00652         Vector2<double> topAngles, bottomAngles, leftAngles, rightAngles;
    00653         Geometry::calculateAnglesForPoint(goalCheck.largePartBegin[0], cameraMatrix, image.cameraInfo, topAngles);
    00654         Geometry::calculateAnglesForPoint(goalCheck.largePartEnd[goalCheck.numberOfLargeParts - 1], cameraMatrix, image.cameraInfo, bottomAngles);
    00655
    00656         if(scanLineToCheckBestAngle[i])
    00657         {
    00658           if(bottomAngles.y > 0 || !scanLineIsPartOfGoal)
    00659           {
    00660             if(colorOfOpponentGoal == colorOfGoalScanLine[i])
    00661               obstaclesPercept.angleToFreePartOfGoalWasDetermined[ObstaclesPercept::opponentGoal] = false;
    00662             else
    00663               obstaclesPercept.angleToFreePartOfGoalWasDetermined[ObstaclesPercept::ownGoal] = false;
    00664           }
    00665           else
    00666           {
    00667             //calculate distance
    00668             int distance = Geometry::getDistanceBySize(
    00669               image.cameraInfo,
    00670               goalHeight,
    00671               (goalCheck.largePartBegin[0] - goalCheck.largePartEnd[goalCheck.numberOfLargeParts - 1]).abs()
    00672             );
    00673             //~ int distance = Geometry::getDistanceBySize(
    00674               //~ goalHeight,
    00675               //~ (goalCheck.largePartBegin[0] - goalCheck.largePartEnd[goalCheck.numberOfLargeParts - 1]).abs(),
    00676               //~ image.cameraInfo.resolutionWidth, image.cameraInfo.openingAngleWidth);
    00677
    00678             if(colorOfOpponentGoal == colorOfGoalScanLine[i])
    00679               obstaclesPercept.distanceToFreePartOfGoal[ObstaclesPercept::opponentGoal] = distance;
    00680             else
    00681               obstaclesPercept.distanceToFreePartOfGoal[ObstaclesPercept::ownGoal] = distance;
    00682           }
    00683         }
    00684         else
    00685         {
    00686           Geometry::calculateAnglesForPoint(goalIndicationLeft[indexOfGoalIndication[i]], cameraMatrix, image.cameraInfo, leftAngles);
    00687           Geometry::calculateAnglesForPoint(goalIndicationRight[indexOfGoalIndication[i]], cameraMatrix, image.cameraInfo, rightAngles);
    00688
    00689           // if(bottomAngles.y < 0)
    00690           //if(topAngles.y > 0)
    00691           {
    00692             boundaryOfIndication[indexOfGoalIndication[i]].addY(topAngles.y, goalCheck.largePartBeginIsOnBorder[0]);
    00693             boundaryOfIndication[indexOfGoalIndication[i]].addY(bottomAngles.y, goalCheck.largePartEndIsOnBorder[goalCheck.numberOfLargeParts - 1]);
    00694             boundaryOfIndication[indexOfGoalIndication[i]].addX(leftAngles.x, leftOfGoalIndicationIsOnBorder[indexOfGoalIndication[i]]);
    00695             boundaryOfIndication[indexOfGoalIndication[i]].addX(rightAngles.x, rightOfGoalIndicationIsOnBorder[indexOfGoalIndication[i]]);
    00696           }//
    00697         }//!scanLineToCheckBestAngle[i]
    00698       }//if(goalCheck.numberOfLargeParts > 0)
    00699       else
    00700       {
    00701         if(scanLineToCheckBestAngle[i])
    00702         {
    00703           if(colorOfOpponentGoal == colorOfGoalScanLine[i])
    00704             obstaclesPercept.angleToFreePartOfGoalWasDetermined[ObstaclesPercept::opponentGoal] = false;
    00705           else
    00706             obstaclesPercept.angleToFreePartOfGoalWasDetermined[ObstaclesPercept::ownGoal] = false;
    00707         }
    00708       }
    00709     }//if(line.getNumberOfPixels() > 3)
    00710   }//for(int i = 0; i < numberOfGoalScanLines; i++)
    00711
    00712
    00713   Vector2<int> corner[4];
    00714   int penWidth[4] = {3,3,3,3};
    00715
    00716   for(i = 0; i < numberOfGoalIndications; i++)
    00717   {
    00718     Drawings::Color color;
    00719     color = Drawings::dark_gray;
    00720
    00721     if(useBoundaryOfIndication[i])
    00722     {
    00723       color = Drawings::light_gray;
    00724       if(colorOfGoalIndication[i] == yellow)
    00725       {
    00726         yellowGoalBoundary.add(boundaryOfIndication[i]);
    00727         foundYellowGoal = true;
    00728       }
    00729       if(colorOfGoalIndication[i] == skyblue)
    00730       {
    00731         skyblueGoalBoundary.add(boundaryOfIndication[i]);
    00732         foundSkyblueGoal = true;
    00733       }
    00734     }
    00735
    00736     (boundaryOfIndication[i].isOnBorder(boundaryOfIndication[i].y.max))? penWidth[0] = 1 : penWidth[0] = 3;
    00737     (boundaryOfIndication[i].isOnBorder(boundaryOfIndication[i].x.min))? penWidth[1] = 1 : penWidth[1] = 3;
    00738     (boundaryOfIndication[i].isOnBorder(boundaryOfIndication[i].y.min))? penWidth[2] = 1 : penWidth[2] = 3;
    00739     (boundaryOfIndication[i].isOnBorder(boundaryOfIndication[i].x.max))? penWidth[3] = 1 : penWidth[3] = 3;
    00740
    00741     Geometry::calculatePointByAngles(
    00742       Vector2<double>(boundaryOfIndication[i].x.max,boundaryOfIndication[i].y.max),
    00743       cameraMatrix, image.cameraInfo,
    00744       corner[0]
    00745       );
    00746
    00747     Geometry::calculatePointByAngles(
    00748       Vector2<double>(boundaryOfIndication[i].x.min,boundaryOfIndication[i].y.max),
    00749       cameraMatrix, image.cameraInfo,
    00750       corner[1]
    00751       );
    00752
    00753     Geometry::calculatePointByAngles(
    00754       Vector2<double>(boundaryOfIndication[i].x.min,boundaryOfIndication[i].y.min),
    00755       cameraMatrix, image.cameraInfo,
    00756       corner[2]
    00757       );
    00758
    00759     Geometry::calculatePointByAngles(
    00760       Vector2<double>(boundaryOfIndication[i].x.max,boundaryOfIndication[i].y.min),
    00761       cameraMatrix, image.cameraInfo,
    00762       corner[3]
    00763       );
    00764
    00765     LINE(imageProcessor_flagsAndGoals,
    00766       (int)corner[0].x, (int)corner[0].y,
    00767       (int)corner[1].x, (int)corner[1].y,
    00768       penWidth[0], Drawings::ps_solid, color);
    00769     LINE(imageProcessor_flagsAndGoals,
    00770       (int)corner[1].x, (int)corner[1].y,
    00771       (int)corner[2].x, (int)corner[2].y,
    00772       penWidth[1], Drawings::ps_solid, color);
    00773     LINE(imageProcessor_flagsAndGoals,
    00774       (int)corner[2].x, (int)corner[2].y,
    00775       (int)corner[3].x, (int)corner[3].y,
    00776       penWidth[2], Drawings::ps_solid, color);
    00777     LINE(imageProcessor_flagsAndGoals,
    00778       (int)corner[3].x, (int)corner[3].y,
    00779       (int)corner[0].x, (int)corner[0].y,
    00780       penWidth[3], Drawings::ps_solid, color);
    00781   }
    00782
    00783   bool ownTeamColorIsBlue = getPlayer().getTeamColor() == Player::blue;
    00784   if(
    00785     foundYellowGoal &&
    00786     yellowGoalBoundary.y.min < fromDegrees(0) &&
    00787     (
    00788     yellowGoalBoundary.x.getSize() > 1.5 * yellowGoalBoundary.y.getSize() ||
    00789       yellowGoalBoundary.isOnBorder(yellowGoalBoundary.x.min) ||
    00790       yellowGoalBoundary.isOnBorder(yellowGoalBoundary.x.max)
    00791     )
    00792     )
    00793   {
    00794     landmarksPercept.addGoal(yellow, ownTeamColorIsBlue, yellowGoalBoundary);
    00795     Vector2<double> cameraOffset(cameraMatrix.translation.x, cameraMatrix.translation.y);
    00796     landmarksPercept.estimateOffsetForGoals(cameraOffset);
    00797   }
    00798   if(
    00799     foundSkyblueGoal &&
    00800     skyblueGoalBoundary.y.min < fromDegrees(0) &&
    00801     (
    00802     skyblueGoalBoundary.x.getSize() > 1.5 * skyblueGoalBoundary.y.getSize() ||
    00803       skyblueGoalBoundary.isOnBorder(skyblueGoalBoundary.x.min) ||
    00804       skyblueGoalBoundary.isOnBorder(skyblueGoalBoundary.x.max)
    00805     )
    00806     )
    00807   {
    00808     landmarksPercept.addGoal(skyblue, ownTeamColorIsBlue, skyblueGoalBoundary);
    00809     Vector2<double> cameraOffset(cameraMatrix.translation.x, cameraMatrix.translation.y);
    00810     landmarksPercept.estimateOffsetForGoals(cameraOffset);
    00811   }
    00812 }//scanLinesForGoals()