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

# Modules/ImageProcessor/SlamImageProcessor/SlamGoalRecognizer.cpp

Go to the documentation of this file.

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