Modules/ImageProcessor/SlamImageProcessor/SlamGoalRecognizer.cpp

Go to the documentation of this file.

/**
* @file SlamGoalRecognizer.cpp
*
* Implementation of class SlamGoalRecognizer
*
* @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
*/

#include "Tools/Player.h"
#include "Tools/FieldDimensions.h"
#include "Tools/RangeArray.h"
#include "Tools/Debugging/View.h"
#include "Representations/Perception/Image.h"
#include "Representations/Perception/ColorTable.h"
#include "Representations/Perception/ObstaclesPercept.h"
#include "Representations/Perception/LandmarksPercept.h"
#include "Representations/Perception/CameraMatrix.h"
#include "Modules/ImageProcessor/ImageProcessorTools/ColorTableReferenceColor.h"
#include "Tools/Debugging/DebugImages.h"
#include "SlamImageProcessorTools.h"
#include "SlamGoalRecognizer.h"


SlamGoalRecognizer::SlamGoalRecognizer
(
 const Image& image, 
 const CameraMatrix& cameraMatrix,
 const CameraMatrix& prevCameraMatrix,
 const ColorTable& colorTable,
 const ColorCorrector& colorCorrector,
 ObstaclesPercept& obstaclesPercept,
 LandmarksPercept& landmarksPercept
 ):
image(image),
cameraMatrix(cameraMatrix),
prevCameraMatrix(cameraMatrix),
colorTable(colorTable),
colorCorrector(colorCorrector),
obstaclesPercept(obstaclesPercept),
landmarksPercept(landmarksPercept)
{
  useFixedScanLines = true;
}


SlamGoalRecognizer::~SlamGoalRecognizer()
{
}

void SlamGoalRecognizer::execute()
{
  if(getPlayer().getTeamColor() == Player::blue) 
  {
    colorOfOpponentGoal = yellow;
    colorOfOwnGoal = skyblue;
  }
  else 
  {
    colorOfOpponentGoal = skyblue;
    colorOfOwnGoal = yellow;
  }

  INIT_DEBUG_IMAGE(imageProcessorGoals, image);
  
  horizonLine = Geometry::calculateHorizon(cameraMatrix, image.cameraInfo);
  verticalLine.base = horizonLine.base;
  verticalLine.direction.x = - horizonLine.direction.y;
  verticalLine.direction.y = horizonLine.direction.x;

  if(useFixedScanLines)
    calculateScanLinesParallelToHorizon();
  else
    calculateScanLinesParallelToHorizon(goalIndicationAboveHorizon, goalIndicationBelowHorizon, 7);

  scanHorizontalForGoals();
  calculateVerticalGoalScanLines();
  scanLinesForGoals();

  SEND_DEBUG_IMAGE(imageProcessorGoals);
}

void SlamGoalRecognizer::calculateScanLinesParallelToHorizon()
{
  numberOfHorizontalScanLines = 0;
  
  Geometry::Line lineAboveHorizon;
  lineAboveHorizon.direction = horizonLine.direction;
  
  int i;
  
  Vector2<int> bottomLeft(0,0);
  Vector2<int> topRight(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3);
  
  for(i = 14; i >= 0; i--)
  {
    lineAboveHorizon.base = horizonLine.base + verticalLine.direction * ( (i * 4.5) - 25 );
    
    if(Geometry::getIntersectionPointsOfLineAndRectangle(
      bottomLeft, topRight, lineAboveHorizon,
      leftPoint[numberOfHorizontalScanLines], 
      rightPoint[numberOfHorizontalScanLines])
      )
    {
      numberOfHorizontalScanLines++;
    }
  }
}

void SlamGoalRecognizer::calculateScanLinesParallelToHorizon
(
 int distanceAboveHorizon,
 int distanceBelowHorizon,
 int numberOfScanLines
 )
{
  numberOfHorizontalScanLines = 0;
  
  Geometry::Line scanLine;
  scanLine.direction = horizonLine.direction;
  
  int i;
  
  Vector2<int> bottomLeft(0,0);
  Vector2<int> topRight(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3);
  
  for(i = 0; i < numberOfScanLines; i++)
  {
    scanLine.base = 
      horizonLine.base + 
      verticalLine.direction * -(distanceBelowHorizon + (distanceAboveHorizon - distanceBelowHorizon) * i / (numberOfScanLines - 1));
//      verticalLine.direction * distanceBelowHorizon;// - (-distanceAboveHorizon + distanceBelowHorizon) * i / numberOfScanLines);
    
    if(Geometry::getIntersectionPointsOfLineAndRectangle(
      bottomLeft, topRight, scanLine,
      leftPoint[numberOfHorizontalScanLines], 
      rightPoint[numberOfHorizontalScanLines])
      )
    {
      numberOfHorizontalScanLines++;
    }
  }
}

void SlamGoalRecognizer::scanHorizontalForGoals()
{
  enum {yellowGoal = 1, skyblueGoal = 0, numberOfGoalColors = 2};
  int colorToCheck;
  colorClass colorClassToCheck;

  RangeArray<double> goalClusters[numberOfGoalColors];
  numberOfGoalIndications = 0;

  // variables needed to determine free part of goal
  RangeArray<double> goalClustersForFreePart[numberOfGoalColors];
  int numberOfLinesWithLargeParts[numberOfGoalColors] = {0, 0};

  int i;
  for(i = 0; i < numberOfHorizontalScanLines; i++)
  {
    Geometry::PixeledLine line(leftPoint[i], rightPoint[i]);
    
    // state machine
    // states
    enum { noGoalColorFound, 
      foundBeginOfGoalColor, 
      foundEndOfGoalColor
    } goalColorState[numberOfGoalColors] = {noGoalColorFound,noGoalColorFound};
    // counter
    int numberOfPixelsSinceLastOccurrenceOfGoalColor[numberOfGoalColors];
    int numberOfPixelsSinceFirstOccurrenceOfGoalColor[numberOfGoalColors] = {0,0};

    bool colorStartsAtBeginOfImage[numberOfGoalColors] = {false, false};
    ColoredPartsCheck goalCheck[numberOfGoalColors];

    int x = 0, y = 0;
    colorClass color;

    if(line.getNumberOfPixels() > 3)
    {
      for(int z = 2; z < line.getNumberOfPixels(); z++)
      {
        x = line.getPixelX(z);
        y = line.getPixelY(z);
        
        //color = COLOR_CLASS(image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]);
        color = CORRECTED_COLOR_CLASS(x,y,image.image[y][0][x],image.image[y][1][x],image.image[y][2][x]);
        for(colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
        {
          if(colorToCheck == yellowGoal) colorClassToCheck = yellow;
          else colorClassToCheck = skyblue;

          if(goalColorState[colorToCheck] == noGoalColorFound)
          {
            DEBUG_IMAGE_SET_PIXEL_BLACK(imageProcessorGoals, x, y + colorToCheck);
            if(color == colorClassToCheck)
            {
              goalCheck[colorToCheck].numberOfColoredPixels = 0;
              goalCheck[colorToCheck].rangeOfColor = 0;
              if(numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck] == 0)
              {
                goalCheck[colorToCheck].firstPoint.x = x;
                goalCheck[colorToCheck].firstPoint.y = y;
              }
              numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck]++;
              if(numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck] > 2)
              {
                goalColorState[colorToCheck] = foundBeginOfGoalColor;
                if(z < 5) 
                {
                  DOT(imageProcessor_general, x, y, Drawings::green, Drawings::green);
                  colorStartsAtBeginOfImage[colorToCheck] = true;
                }
              }
            }
            else
            {
              numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck] = 0;
            }
          }
          else if(goalColorState[colorToCheck] == foundBeginOfGoalColor)
          {
            DEBUG_IMAGE_SET_PIXEL_GRAY(imageProcessorGoals, x, y + colorToCheck);
            if(color == colorClassToCheck) goalCheck[colorToCheck].numberOfColoredPixels++;
            else
            {
              goalCheck[colorToCheck].lastPoint.x = x;
              goalCheck[colorToCheck].lastPoint.y = y;
              goalColorState[colorToCheck] = foundEndOfGoalColor;
              numberOfPixelsSinceLastOccurrenceOfGoalColor[colorToCheck] = 0;
              numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck] = 0;
            }
            goalCheck[colorToCheck].rangeOfColor++;
          }
          else if(goalColorState[colorToCheck] == foundEndOfGoalColor)
          {
            DEBUG_IMAGE_SET_PIXEL_WHITE(imageProcessorGoals, x, y + colorToCheck);
            numberOfPixelsSinceLastOccurrenceOfGoalColor[colorToCheck]++;
            if(color == colorClassToCheck)
            {
              numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck]++;
              if(numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck] > 2)
              {
                goalColorState[colorToCheck] = foundBeginOfGoalColor;
              }
            }
            else if(
              numberOfPixelsSinceLastOccurrenceOfGoalColor[colorToCheck] > 5
              ||
              color == white
              )
            {
              goalColorState[colorToCheck] = noGoalColorFound;
              goalCheck[colorToCheck].determineLargePart(10, colorStartsAtBeginOfImage[colorToCheck], false, cameraMatrix, image.cameraInfo);
              DOT(imageProcessor_general, x, y, Drawings::green, Drawings::white);
              colorStartsAtBeginOfImage[colorToCheck] = false;
            }
            else
            {
              numberOfPixelsSinceFirstOccurrenceOfGoalColor[colorToCheck] = 0;  
            }
            goalCheck[colorToCheck].rangeOfColor++;
          }
        }//for(int colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
      }//for(int z = 2; z < line.getNumberOfPixels(); z++)

      for(colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
      {
        if(goalColorState[colorToCheck] == foundBeginOfGoalColor || goalColorState[colorToCheck] == foundEndOfGoalColor)
        {
          if(goalColorState[colorToCheck] == foundBeginOfGoalColor)
          {
            goalCheck[colorToCheck].lastPoint.x = x;
            goalCheck[colorToCheck].lastPoint.y = y;
          }
          goalCheck[colorToCheck].determineLargePart(10, colorStartsAtBeginOfImage[colorToCheck], true, cameraMatrix, image.cameraInfo);
        }//if(goalColorState == foundBeginOfGoalColor || goalColorState == foundEndOfGoalColor)
      }//for(colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
    }//if(line.getNumberOfPixels() > 3)

    for(colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
    {
      // Add all large parts found on that line to the structure that determines clusters.
      if(goalCheck[colorToCheck].numberOfLargeParts > 0) numberOfLinesWithLargeParts[colorToCheck]++;
      int index;
      for(index = 0; index < goalCheck[colorToCheck].numberOfLargeParts; index++)
      {
        goalClusters[colorToCheck].add(
          Range<double>(goalCheck[colorToCheck].largePartEndAngles[index].x, goalCheck[colorToCheck].largePartBeginAngles[index].x), 
          goalCheck[colorToCheck].largePartBeginIsOnBorder[index],
          goalCheck[colorToCheck].largePartEndIsOnBorder[index]
          );
        if(numberOfLinesWithLargeParts[colorToCheck] > 2 && numberOfLinesWithLargeParts[colorToCheck] < 5)
        {
          goalClustersForFreePart[colorToCheck].add(
            Range<double>(goalCheck[colorToCheck].largePartEndAngles[index].x, goalCheck[colorToCheck].largePartBeginAngles[index].x), 
            goalCheck[colorToCheck].largePartBeginIsOnBorder[index],
            goalCheck[colorToCheck].largePartEndIsOnBorder[index]
            );
        }
      }
    }
  }// for(int i = 0; i < numberOfHorizontalScanLines; i++)

  for(colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
  {
    if(colorToCheck == yellowGoal) colorClassToCheck = yellow;
    else colorClassToCheck = skyblue;

    // Add one goal indication for each cluster
    int index;
    for(index = 0; index < goalClusters[colorToCheck].getNumberOfClusters(); index++)
    {
      Range<double> currentRange = goalClusters[colorToCheck].getCluster(index);
      Vector2<int> leftPoint, rightPoint;
      Geometry::calculatePointByAngles(Vector2<double>(currentRange.max,0.02 * index), cameraMatrix, image.cameraInfo, leftPoint);
      Geometry::calculatePointByAngles(Vector2<double>(currentRange.min,0.02 * index), cameraMatrix, image.cameraInfo, rightPoint);
      LINE(imageProcessor_flagsAndGoals, leftPoint.x, leftPoint.y, rightPoint.x, rightPoint.y, 2, Drawings::ps_solid, Drawings::blue);
      goalIndicationLeft[numberOfGoalIndications]   = leftPoint;
      goalIndicationRight[numberOfGoalIndications]  = rightPoint;
      goalIndicationCenter[numberOfGoalIndications] = (leftPoint + rightPoint) / 2;
      leftOfGoalIndicationIsOnBorder[numberOfGoalIndications] = goalClusters[colorToCheck].isLeftOnBorder(index);
      rightOfGoalIndicationIsOnBorder[numberOfGoalIndications] = goalClusters[colorToCheck].isRightOnBorder(index);
      colorOfGoalIndication[numberOfGoalIndications] = colorClassToCheck;
      numberOfGoalIndications++;
    }

    // Determine the free part of the goal
    for(index = 0; index < goalClustersForFreePart[colorToCheck].getNumberOfClusters(); index++)
    {
      Range<double> currentRange = goalClustersForFreePart[colorToCheck].getCluster(index);
      Vector2<int> leftPoint, rightPoint;
      Geometry::calculatePointByAngles(Vector2<double>(currentRange.min,-0.1 + 0.02 * index), cameraMatrix, image.cameraInfo, leftPoint);
      Geometry::calculatePointByAngles(Vector2<double>(currentRange.max,-0.1 + 0.02 * index), cameraMatrix, image.cameraInfo, rightPoint);
      LINE(imageProcessor_flagsAndGoals, leftPoint.x, leftPoint.y, rightPoint.x, rightPoint.y, 2, Drawings::ps_solid, Drawings::green);
    }

    double center, angle;
    bool determinedFreePartOfGoal = false;
    if(goalClustersForFreePart[colorToCheck].getNumberOfClusters() == 1)
    {
      bool leftOfPartIsOnBorder = goalClustersForFreePart[colorToCheck].isLeftOnBorder(0);
      bool rightOfPartIsOnBorder = goalClustersForFreePart[colorToCheck].isRightOnBorder(0);
      Range<double> freePartRange = goalClustersForFreePart[colorToCheck].getCluster(0);
      if(!leftOfPartIsOnBorder && !rightOfPartIsOnBorder || freePartRange.getSize() > 0.7 * image.cameraInfo.openingAngleWidth)
      {
        determinedFreePartOfGoal = true;
        center = (freePartRange.min + freePartRange.max) / 2.0;
        angle = freePartRange.max - freePartRange.min;
      }
    }
    else if(goalClustersForFreePart[colorToCheck].getNumberOfClusters() == 2)
    {
      Range<double> freePartRange[2];
      freePartRange[0] = goalClustersForFreePart[colorToCheck].getCluster(0);
      freePartRange[1] = goalClustersForFreePart[colorToCheck].getCluster(1);

      bool freePartIsSeenCompletely[2];

      freePartIsSeenCompletely[0] =
        !goalClustersForFreePart[colorToCheck].isLeftOnBorder(0) && 
        !goalClustersForFreePart[colorToCheck].isRightOnBorder(0);

      freePartIsSeenCompletely[1] =
        !goalClustersForFreePart[colorToCheck].isLeftOnBorder(1) && 
        !goalClustersForFreePart[colorToCheck].isRightOnBorder(1);

      int largePart, smallPart;
      if(freePartRange[0].getSize() < freePartRange[1].getSize()) 
      {
        largePart = 1;
        smallPart = 0;
      }
      else 
      {
        largePart = 0;
        smallPart = 1;
      }

      if(
        freePartIsSeenCompletely[largePart] && freePartIsSeenCompletely[smallPart] ||
        !freePartIsSeenCompletely[largePart] && freePartIsSeenCompletely[smallPart]
        )
      {
        determinedFreePartOfGoal = true;
        center = (freePartRange[largePart].min + freePartRange[largePart].max) / 2.0;
        angle = freePartRange[largePart].max - freePartRange[largePart].min;
      }
    }

    if(determinedFreePartOfGoal)
    {
      ObstaclesPercept::FreePartOfGoal& f = obstaclesPercept.freePartOfGoal[
        colorClassToCheck == colorOfOpponentGoal ? ObstaclesPercept::opponentGoal : ObstaclesPercept::ownGoal];
      f.directionAsAngle = center;
      f.widthAsAngle = angle;
      f.wasDetermined = true;
    }

  }//for(colorToCheck = 0; colorToCheck < numberOfGoalColors; colorToCheck++)
}//void GridImageProcessor2::scanHorizontalForFlagsAndGoals()

void SlamGoalRecognizer::calculateVerticalGoalScanLines()
{
  numberOfGoalScanLines = 0;
  int i;
  Vector2<double> anglesForGoalScanLine;
  for(i = 0; i < numberOfGoalIndications; i++)
  { 
    // Line through center of indication
    Geometry::calculateAnglesForPoint(goalIndicationCenter[i], cameraMatrix, image.cameraInfo, anglesForGoalScanLine);
    anglesForGoalScanLine.y = -1.0;
    Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, bottomGoalPoint[numberOfGoalScanLines]);
    anglesForGoalScanLine.y =  0.3;
    Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, topGoalPoint[numberOfGoalScanLines]);

    if(Geometry::clipLineWithRectangleCohenSutherland(Vector2<int>(0,0), Vector2<int>(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3),
      topGoalPoint[numberOfGoalScanLines], bottomGoalPoint[numberOfGoalScanLines]))
    {
      colorOfGoalScanLine[numberOfGoalScanLines] = colorOfGoalIndication[i];
      indexOfGoalIndication[numberOfGoalScanLines] = i;
      scanLineToCheckBestAngle[numberOfGoalScanLines] = false;
      numberOfGoalScanLines++;
    }

    // Line at the left side
    Geometry::calculateAnglesForPoint((goalIndicationLeft[i] + goalIndicationCenter[i]) / 2, cameraMatrix, image.cameraInfo, anglesForGoalScanLine);
    anglesForGoalScanLine.y = -1.0;
    Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, bottomGoalPoint[numberOfGoalScanLines]);
    anglesForGoalScanLine.y =  0.3;
    Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, topGoalPoint[numberOfGoalScanLines]);

    if(Geometry::clipLineWithRectangleCohenSutherland(Vector2<int>(0,0), Vector2<int>(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3),
      topGoalPoint[numberOfGoalScanLines], bottomGoalPoint[numberOfGoalScanLines]))
    {
      colorOfGoalScanLine[numberOfGoalScanLines] = colorOfGoalIndication[i];
      indexOfGoalIndication[numberOfGoalScanLines] = i;
      scanLineToCheckBestAngle[numberOfGoalScanLines] = false;
      numberOfGoalScanLines++;
    }

    // Line at the right side
    Geometry::calculateAnglesForPoint((goalIndicationRight[i] + goalIndicationCenter[i]) / 2, cameraMatrix, image.cameraInfo, anglesForGoalScanLine);
    anglesForGoalScanLine.y = -1.0;
    Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, bottomGoalPoint[numberOfGoalScanLines]);
    anglesForGoalScanLine.y =  0.3;
    Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, topGoalPoint[numberOfGoalScanLines]);

    if(Geometry::clipLineWithRectangleCohenSutherland(Vector2<int>(0,0), Vector2<int>(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3),
      topGoalPoint[numberOfGoalScanLines], bottomGoalPoint[numberOfGoalScanLines]))
    {
      colorOfGoalScanLine[numberOfGoalScanLines] = colorOfGoalIndication[i];
      indexOfGoalIndication[numberOfGoalScanLines] = i;
      scanLineToCheckBestAngle[numberOfGoalScanLines] = false;
      numberOfGoalScanLines++;
    }

  }

  // line for the free part of the goal
  for(i = 0; i < 2; i++)
  {
    const ObstaclesPercept::FreePartOfGoal& f = obstaclesPercept.freePartOfGoal[i];
    if(f.wasDetermined)
    {
      anglesForGoalScanLine.x = f.directionAsAngle;
      anglesForGoalScanLine.y = -1.0;
      Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, bottomGoalPoint[numberOfGoalScanLines]);
      anglesForGoalScanLine.y =  0.3;
      Geometry::calculatePointByAngles(anglesForGoalScanLine, cameraMatrix, image.cameraInfo, topGoalPoint[numberOfGoalScanLines]);

      if(Geometry::clipLineWithRectangleCohenSutherland(Vector2<int>(0,0), Vector2<int>(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3),
        topGoalPoint[numberOfGoalScanLines], bottomGoalPoint[numberOfGoalScanLines]))
      {
        if(i==1) colorOfGoalScanLine[numberOfGoalScanLines] = colorOfOpponentGoal;
        else colorOfGoalScanLine[numberOfGoalScanLines] = colorOfOwnGoal;
        scanLineToCheckBestAngle[numberOfGoalScanLines] = true;
        numberOfGoalScanLines++;
      }
    }
  }
}// calculateVerticalGoalScanLines()

void SlamGoalRecognizer::scanLinesForGoals()
{
  ConditionalBoundary yellowGoalBoundary, skyblueGoalBoundary;
  ConditionalBoundary boundaryOfIndication[maxNumberOfGoalScanLines];
  bool useBoundaryOfIndication[maxNumberOfGoalScanLines];
  int i;
  for(i = 0; i < numberOfGoalIndications; i++) useBoundaryOfIndication[i] = true;

  bool foundYellowGoal = false;
  bool foundSkyblueGoal = false;
  bool pixelHasGoalColor;

  for(i = 0; i < numberOfGoalScanLines; i++)
  {
    Geometry::PixeledLine line(topGoalPoint[i], bottomGoalPoint[i]);

    // state machine
    // states
    enum { noGoalColorFound, 
      foundBeginOfGoalColor, 
      foundEndOfGoalColor,
      determinedGoal
    } goalColorState = noGoalColorFound;
    // counter
    int numberOfPixelsSinceLastOccurrenceOfGoalColor = 0;
    int numberOfPixelsSinceFirstOccurrenceOfGoalColor = 0;
    int whiteAndPinkCounter = 0;

    bool colorStartsAtBeginOfImage = false;

    ColoredPartsCheck goalCheck;

    int x = 0, y = 0;
    colorClass color;

    if(line.getNumberOfPixels() > 3)
    {
      bool scanLineIsPartOfGoal = true;
      for(int z = 2; z < line.getNumberOfPixels(); z++) 
      {
        pixelHasGoalColor = false;
        x = line.getPixelX(z);
        y = line.getPixelY(z);

        if(scanLineToCheckBestAngle[i]){
          DEBUG_IMAGE_SET_PIXEL_GRAY(imageProcessorGoals, x, y);
        }
        else {DEBUG_IMAGE_SET_PIXEL_WHITE(imageProcessorGoals, x, y);}

        //color = COLOR_CLASS(image.image[y][0][x], image.image[y][1][x], image.image[y][2][x]);
        color = CORRECTED_COLOR_CLASS(x,y,image.image[y][0][x],image.image[y][1][x],image.image[y][2][x]);
        
        if(colorOfGoalScanLine[i] == skyblue && color == skyblue) pixelHasGoalColor = true;
        if(colorOfGoalScanLine[i] == yellow && color == yellow) pixelHasGoalColor = true;
        
        if(goalColorState == noGoalColorFound)
        {
          if(pixelHasGoalColor)
          {
            DEBUG_IMAGE_SET_PIXEL_BLACK(imageProcessorGoals, x, y);
            goalCheck.numberOfColoredPixels = 0;
            goalCheck.rangeOfColor = 0;
            if(numberOfPixelsSinceFirstOccurrenceOfGoalColor == 0)
            {
              goalCheck.firstPoint.x = x;
              goalCheck.firstPoint.y = y;
            }
            numberOfPixelsSinceFirstOccurrenceOfGoalColor++;
            if(numberOfPixelsSinceFirstOccurrenceOfGoalColor > 2)
            {
              goalColorState = foundBeginOfGoalColor;
              if(z < 5) colorStartsAtBeginOfImage = true;
            }
          }
          else
          {
            numberOfPixelsSinceFirstOccurrenceOfGoalColor = 0;
          }
        }
        else if(goalColorState == foundBeginOfGoalColor)
        {
          DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorGoals, x, y);
          if(pixelHasGoalColor) goalCheck.numberOfColoredPixels++;
          else
          {
            goalCheck.lastPoint.x = x;
            goalCheck.lastPoint.y = y;
            goalColorState = foundEndOfGoalColor;
            numberOfPixelsSinceLastOccurrenceOfGoalColor = 0;
            numberOfPixelsSinceFirstOccurrenceOfGoalColor = 0;
          }
          goalCheck.rangeOfColor++;
        }
        else if(goalColorState == foundEndOfGoalColor)
        {
          numberOfPixelsSinceLastOccurrenceOfGoalColor++;
          DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorGoals, x, y);
          if(pixelHasGoalColor)
          {
            numberOfPixelsSinceFirstOccurrenceOfGoalColor++;
            if(numberOfPixelsSinceFirstOccurrenceOfGoalColor > 2)
            {
              goalColorState = foundBeginOfGoalColor;
            }
          }
          else if(numberOfPixelsSinceLastOccurrenceOfGoalColor > 20)
          {
            DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorGoals, x, y);
//            goalColorState = noGoalColorFound;
            goalColorState = determinedGoal;
            if(goalCheck.determineLargePart(7, colorStartsAtBeginOfImage, false, cameraMatrix, image.cameraInfo))
            {
              if(whiteAndPinkCounter > 5)
              {
                scanLineIsPartOfGoal = false;
                DOT(imageProcessor_general, x, y, Drawings::blue, Drawings::blue);
              }
            }
            whiteAndPinkCounter = 0;
            colorStartsAtBeginOfImage = false;
          }
          else if(color == white || color == pink) 
          {
            DEBUG_IMAGE_SET_PIXEL_PINK(imageProcessorGoals, x, y);
            whiteAndPinkCounter++;
          }
          else
          {
            numberOfPixelsSinceFirstOccurrenceOfGoalColor = 0;  
          }
          goalCheck.rangeOfColor++;
        }
      }//for(int z = 0; z < line.getNumberOfPixels(); z++) 
      if(goalColorState == foundBeginOfGoalColor || goalColorState == foundEndOfGoalColor)
      {
        if(goalColorState == foundBeginOfGoalColor)
        {
          goalCheck.lastPoint.x = x;
          goalCheck.lastPoint.y = y;
        }
        goalCheck.determineLargePart(7, colorStartsAtBeginOfImage, true, cameraMatrix,image.cameraInfo);
      }//if(goalColorState == foundBeginOfGoalColor || goalColorState == foundEndOfGoalColor)
      
      if(!scanLineIsPartOfGoal && !scanLineToCheckBestAngle[i]) 
        useBoundaryOfIndication[indexOfGoalIndication[i]] = false;

      if(goalCheck.numberOfLargeParts > 0)
      {
        Vector2<double> topAngles, bottomAngles, leftAngles, rightAngles;
        Geometry::calculateAnglesForPoint(goalCheck.largePartBegin[0], cameraMatrix, prevCameraMatrix, image.cameraInfo, topAngles);
        Geometry::calculateAnglesForPoint(goalCheck.largePartEnd[goalCheck.numberOfLargeParts - 1], cameraMatrix, prevCameraMatrix, image.cameraInfo, bottomAngles);
        
        if(scanLineToCheckBestAngle[i])
        {
          if(bottomAngles.y > 0 || !scanLineIsPartOfGoal) 
          {
            if(colorOfOpponentGoal == colorOfGoalScanLine[i])
              obstaclesPercept.freePartOfGoal[ObstaclesPercept::opponentGoal].wasDetermined = false;
            else
              obstaclesPercept.freePartOfGoal[ObstaclesPercept::ownGoal].wasDetermined = false;
          }
          else
          {
            //calculate distance
            int distance = (int)Geometry::getDistanceBySize(
              image.cameraInfo,
              goalHeight, 
              (goalCheck.largePartBegin[0] - goalCheck.largePartEnd[goalCheck.numberOfLargeParts - 1]).abs() 
            );
            //~ int distance = Geometry::getDistanceBySize(
              //~ goalHeight, 
              //~ (goalCheck.largePartBegin[0] - goalCheck.largePartEnd[goalCheck.numberOfLargeParts - 1]).abs(), 
              //~ image.cameraInfo.resolutionWidth, image.cameraInfo.openingAngleWidth);

            ObstaclesPercept::FreePartOfGoal& f = obstaclesPercept.freePartOfGoal[
              colorOfOpponentGoal == colorOfGoalScanLine[i] ? ObstaclesPercept::opponentGoal : ObstaclesPercept::ownGoal];
            f.distanceOnField = distance;
      
            double leftAngle = f.directionAsAngle + f.widthAsAngle / 2.0;
            double rightAngle = f.directionAsAngle - f.widthAsAngle / 2.0;
            Vector2<int> leftPointOnField((int) (cos(leftAngle) * f.distanceOnField),
                                          (int) (sin(leftAngle) * f.distanceOnField) );
            Vector2<int> rightPointOnField((int) (cos(rightAngle) * f.distanceOnField),
                                           (int)(sin(rightAngle) * f.distanceOnField) );
            
            Vector2<int> leftBottomPoint, 
                         rightBottomPoint;
            Geometry::calculatePointInImage(leftPointOnField, cameraMatrix, image.cameraInfo, leftBottomPoint);
            Geometry::calculatePointInImage(rightPointOnField, cameraMatrix, image.cameraInfo, rightBottomPoint);
            f.leftInImage = Vector2<int>(leftBottomPoint.x, leftBottomPoint.y);
            f.rightInImage = Vector2<int>(rightBottomPoint.x, rightBottomPoint.y);
          }
        }
        else
        {
          Geometry::calculateAnglesForPoint(goalIndicationLeft[indexOfGoalIndication[i]], cameraMatrix, prevCameraMatrix, image.cameraInfo, leftAngles);
          Geometry::calculateAnglesForPoint(goalIndicationRight[indexOfGoalIndication[i]], cameraMatrix, prevCameraMatrix, image.cameraInfo, rightAngles);
          
          // if(bottomAngles.y < 0)
          //if(topAngles.y > 0)
          {
            boundaryOfIndication[indexOfGoalIndication[i]].addY(topAngles.y, goalCheck.largePartBeginIsOnBorder[0]);
            boundaryOfIndication[indexOfGoalIndication[i]].addY(bottomAngles.y, goalCheck.largePartEndIsOnBorder[goalCheck.numberOfLargeParts - 1]);
            boundaryOfIndication[indexOfGoalIndication[i]].addX(leftAngles.x, leftOfGoalIndicationIsOnBorder[indexOfGoalIndication[i]]);
            boundaryOfIndication[indexOfGoalIndication[i]].addX(rightAngles.x, rightOfGoalIndicationIsOnBorder[indexOfGoalIndication[i]]);
          }//
        }//!scanLineToCheckBestAngle[i]
      }//if(goalCheck.numberOfLargeParts > 0)
      else
      {
        if(scanLineToCheckBestAngle[i])
        {
          if(colorOfOpponentGoal == colorOfGoalScanLine[i])
            obstaclesPercept.freePartOfGoal[ObstaclesPercept::opponentGoal].wasDetermined = false;
          else
            obstaclesPercept.freePartOfGoal[ObstaclesPercept::ownGoal].wasDetermined = false;
        }
      }
    }//if(line.getNumberOfPixels() > 3)
  }//for(int i = 0; i < numberOfGoalScanLines; i++)


  Vector2<int> corner[4];
  int penWidth[4] = {3,3,3,3};

  for(i = 0; i < numberOfGoalIndications; i++)
  {
    Drawings::Color color;
    color = Drawings::dark_gray;

    if(useBoundaryOfIndication[i])
    {
      color = Drawings::light_gray;
      if(colorOfGoalIndication[i] == yellow) 
      {
        yellowGoalBoundary.add(boundaryOfIndication[i]);
        foundYellowGoal = true;
      }
      if(colorOfGoalIndication[i] == skyblue) 
      {
        skyblueGoalBoundary.add(boundaryOfIndication[i]);
        foundSkyblueGoal = true;
      }
    }

    (boundaryOfIndication[i].isOnBorder(boundaryOfIndication[i].y.max))? penWidth[0] = 1 : penWidth[0] = 3;
    (boundaryOfIndication[i].isOnBorder(boundaryOfIndication[i].x.min))? penWidth[1] = 1 : penWidth[1] = 3;
    (boundaryOfIndication[i].isOnBorder(boundaryOfIndication[i].y.min))? penWidth[2] = 1 : penWidth[2] = 3;
    (boundaryOfIndication[i].isOnBorder(boundaryOfIndication[i].x.max))? penWidth[3] = 1 : penWidth[3] = 3;

    COMPLEX_DRAWING(imageProcessor_flagsAndGoals,
      Geometry::calculatePointByAngles(
        Vector2<double>(boundaryOfIndication[i].x.max,boundaryOfIndication[i].y.max),
        cameraMatrix, image.cameraInfo, 
        corner[0]
        );
      
      Geometry::calculatePointByAngles(
        Vector2<double>(boundaryOfIndication[i].x.min,boundaryOfIndication[i].y.max),
        cameraMatrix, image.cameraInfo, 
        corner[1]
        );
      
      Geometry::calculatePointByAngles(
        Vector2<double>(boundaryOfIndication[i].x.min,boundaryOfIndication[i].y.min),
        cameraMatrix, image.cameraInfo, 
        corner[2]
        );
      
      Geometry::calculatePointByAngles(
        Vector2<double>(boundaryOfIndication[i].x.max,boundaryOfIndication[i].y.min),
        cameraMatrix, image.cameraInfo, 
        corner[3]
        );

      LINE(imageProcessor_flagsAndGoals,
        (int)corner[0].x, (int)corner[0].y, 
        (int)corner[1].x, (int)corner[1].y,
        penWidth[0], Drawings::ps_solid, color);
      LINE(imageProcessor_flagsAndGoals,
        (int)corner[1].x, (int)corner[1].y, 
        (int)corner[2].x, (int)corner[2].y,
        penWidth[1], Drawings::ps_solid, color);
      LINE(imageProcessor_flagsAndGoals,
        (int)corner[2].x, (int)corner[2].y, 
        (int)corner[3].x, (int)corner[3].y,
        penWidth[2], Drawings::ps_solid, color);
      LINE(imageProcessor_flagsAndGoals,
        (int)corner[3].x, (int)corner[3].y, 
        (int)corner[0].x, (int)corner[0].y,
        penWidth[3], Drawings::ps_solid, color);
    )
  }

  if(
    foundYellowGoal && 
    yellowGoalBoundary.y.min < fromDegrees(0) &&
    (
    yellowGoalBoundary.x.getSize() > 1.5 * yellowGoalBoundary.y.getSize() ||
      yellowGoalBoundary.isOnBorder(yellowGoalBoundary.x.min) ||
      yellowGoalBoundary.isOnBorder(yellowGoalBoundary.x.max)
    )
    )
  {
    Vector2<int> topLeft, topRight, bottomLeft, bottomRight;
    Geometry::calculatePointByAngles(
      Vector2<double>(yellowGoalBoundary.x.max,yellowGoalBoundary.y.max),
      cameraMatrix, image.cameraInfo, 
      topLeft
      );
    Geometry::calculatePointByAngles(
      Vector2<double>(yellowGoalBoundary.x.min,yellowGoalBoundary.y.max),
      cameraMatrix, image.cameraInfo, 
      topRight
      );
    
    Geometry::calculatePointByAngles(
      Vector2<double>(yellowGoalBoundary.x.min,yellowGoalBoundary.y.min),
      cameraMatrix, image.cameraInfo, 
      bottomRight
      );
    
    Geometry::calculatePointByAngles(
      Vector2<double>(yellowGoalBoundary.x.max,yellowGoalBoundary.y.min),
      cameraMatrix, image.cameraInfo, 
      bottomLeft
      );
    landmarksPercept.addGoal(yellow, yellowGoalBoundary,topLeft,topRight,bottomLeft,bottomRight,cameraMatrix);
    landmarksPercept.addGoal(yellow, yellowGoalBoundary, topLeft, topRight, bottomLeft, bottomRight,cameraMatrix);
  }
  if(
    foundSkyblueGoal && 
    skyblueGoalBoundary.y.min < fromDegrees(0) &&
    (
    skyblueGoalBoundary.x.getSize() > 1.5 * skyblueGoalBoundary.y.getSize() ||
      skyblueGoalBoundary.isOnBorder(skyblueGoalBoundary.x.min) ||
      skyblueGoalBoundary.isOnBorder(skyblueGoalBoundary.x.max)
    )
    )
  {
    Vector2<int> topLeft, topRight, bottomLeft, bottomRight;
    Geometry::calculatePointByAngles(
      Vector2<double>(skyblueGoalBoundary.x.max,skyblueGoalBoundary.y.max),
      cameraMatrix, image.cameraInfo, 
      topLeft
      );
    Geometry::calculatePointByAngles(
      Vector2<double>(skyblueGoalBoundary.x.min,skyblueGoalBoundary.y.max),
      cameraMatrix, image.cameraInfo, 
      topRight
      );
    
    Geometry::calculatePointByAngles(
      Vector2<double>(skyblueGoalBoundary.x.min,skyblueGoalBoundary.y.min),
      cameraMatrix, image.cameraInfo, 
      bottomRight
      );
    
    Geometry::calculatePointByAngles(
      Vector2<double>(skyblueGoalBoundary.x.max,skyblueGoalBoundary.y.min),
      cameraMatrix, image.cameraInfo, 
      bottomLeft
      );
    landmarksPercept.addGoal(skyblue, skyblueGoalBoundary,topLeft,topRight,bottomLeft,bottomRight,cameraMatrix);
  }
}//scanLinesForGoals()

---