Modules/ImageProcessor/GT2005ImageProcessor/GT2005ImageProcessor.cpp

Go to the documentation of this file.

/**
* @file GT2005ImageProcessor.cpp
*
* Implementation of class GT2005ImageProcessor
*
* @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
* @author <a href="mailto:roefer@tzi.de">Thomas Röfer</a>
*/

#include "GT2005ImageProcessor.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "Tools/FieldDimensions.h"
#include "Tools/Math/Geometry.h"
#include "GT2005ImageProcessorTools.h"
#include "Representations/Perception/JPEGImage.h"


// Parameters for the scanning grid
const double GT2005ImageProcessor::verticalScanLineTopAboveHorizon = 15.0;
const double GT2005ImageProcessor::verticalScanLineLength13 = 55.0;
const double GT2005ImageProcessor::verticalScanLineLength2 = 75.0;
const double GT2005ImageProcessor::verticalScanLineSpacing = 4.0;


#define PLOT(p,c) COMPLEX_DRAWING(imageProcessor_general,plot(p,c))

const int Y = 0,
          U = cameraResolutionWidth_ERS7, /**< Relative offset of U component. */
          V = 2 * cameraResolutionWidth_ERS7; /**< Relative offset of V component. */

const double GT2005ImageProcessor::minAngleBetweenFlagAndGoal = 0.05;


GT2005ImageProcessor::GT2005ImageProcessor(const ImageProcessorInterfaces& interfaces)
: ImageProcessor(interfaces),
beaconDetector(image, cameraMatrix, imageInfo.previousCameraMatrix, imageInfo, colorTable, colorCorrector, landmarksPercept),
goalSpecialistY(yellow, interfaces, colorCorrector, imageInfo),
goalSpecialistB(skyblue, interfaces, colorCorrector, imageInfo),
ballSpecialist(colorCorrector),
playerSpecialist(colorCorrector,colorTable,image,imageInfo,linesPercept,cameraMatrix,playersPercept),
ballClustering(),
lineFinder(colorCorrector, colorTable),
circleFinder(colorCorrector, colorTable)
#ifdef OC_GUIDEDOG
  ,guideDogRobotSpecialist(image, cameraMatrix, imageInfo.previousCameraMatrix, colorTable, colorCorrector, playersPercept)
#endif //OC_GUIDEDOG
{
  yThreshold = 15;
  vThreshold = 8;
  beaconDetector.analyzeColorTable();
  double c = cos(-3*pi/4);
  double s = sin(-3*pi/4);
  rotation2x2 = Matrix2x2<double>(
    Vector2<double>(c,-s),
    Vector2<double>(s,c)
  );
}

void GT2005ImageProcessor::execute()
{
  INIT_DEBUG_IMAGE(imageProcessorPlayers, image);
  N_INIT_DEBUG_GRAY_SCALE_IMAGE(scanLines, image);

  xFactor = yFactor = image.cameraInfo.focalLength;
  
  numberOfScannedPixels = 0;

  lastRedMidPoint.x = 1000;
  lastBlueMidPoint.x = 1000;

  // reset everything
  landmarksPercept.reset(image.frameNumber);
  linesPercept.reset(image.frameNumber);
  ballPercept.reset(image.frameNumber);
  playersPercept.reset(image.frameNumber);
  obstaclesPercept.reset(image.frameNumber);
  circleFinder.reset();
  lineFinder.reset();
  goalSpecialistY.notifyAboutNewImage();
  goalSpecialistB.notifyAboutNewImage();


  DEBUG_RESPONSE_NOT("gt05-ip:disable-multiple-ball-candidates",
    ballClustering.reset();
  );

  DEBUG_RESPONSE("gt05-ip:enable-edge-detection",
    edgesPercept.reset(image.frameNumber);
    edgeSpecialist.reset();
  );

  // variations of the camera matrix for different object heights
  cmTricot = cameraMatrix;
  cmTricot.translation.z -= 100; // upper tricot border

  longestBallRun = 0;

//-----------------------------------------------------
  Vector3<double> vectorInDirectionOfViewCamera(1,0,0);
  Vector3<double> vectorInDirectionOfViewWorld;
  vectorInDirectionOfViewWorld = cameraMatrix.rotation * vectorInDirectionOfViewCamera;

  const int visualizationScale = 20;
  Vector3<double> vectorInDirectionOfViewWorldOld;
  vectorInDirectionOfViewWorldOld = imageInfo.previousCameraMatrix.rotation * vectorInDirectionOfViewCamera;
  Vector3<double> cameraMotionVectorWorld = vectorInDirectionOfViewWorld - vectorInDirectionOfViewWorldOld;
  long frameNumber = cameraMatrix.frameNumber, 
  prevFrameNumber = imageInfo.previousCameraMatrix.frameNumber;
  const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
  double timeDiff = (frameNumber - prevFrameNumber) * r.motionCycleTime; // in seconds
  if (prevFrameNumber == 0)
    timeDiff = 0; // Previous camera matrix was uninitialized
  Vector2<double> currentOnGround(vectorInDirectionOfViewWorld.x, vectorInDirectionOfViewWorld.y), 
      oldOnGround(vectorInDirectionOfViewWorldOld.x, vectorInDirectionOfViewWorldOld.y);
  currentOnGround.normalize();
  oldOnGround.normalize();
  Vector3<double> cameraSpeedVectorWorld(0, 0, 0);
  double panningVelocity = 0;
  if (timeDiff > 0) //should never happen otherwise, but better safe than sorry
  {
    cameraSpeedVectorWorld = cameraMotionVectorWorld/timeDiff;
    double cosAng = currentOnGround*oldOnGround;
    if (cosAng > 1.0)
      cosAng = 1.0;
    else
    if (cosAng <-1.0)
      cosAng = -1.0;
    panningVelocity = normalize(acos(cosAng))/timeDiff;
  }
  Vector3<double> cameraSpeedVectorCamera = cameraMatrix.invert().rotation * cameraSpeedVectorWorld;
  Vector2<double> cameraSpeedVectorImg(cameraSpeedVectorCamera.y,
        cameraSpeedVectorCamera.z);
  cameraSpeedVectorImg /= cameraSpeedVectorCamera.x + // perspective projection, 
    image.cameraInfo.focalLength; // focal length is added to translate the vector away from the lens in case the distance
                                    // is too small and hence the visualization would be greatly magnified
  cameraSpeedVectorImg *= image.cameraInfo.focalLength*visualizationScale;
  Drawings::Color arrowColor = Drawings::blue;
  if (fabs(panningVelocity) > pi/6)
    arrowColor = Drawings::red;
  ARROW(imageProcessor_horizon,
    image.cameraInfo.opticalCenter.x, image.cameraInfo.opticalCenter.y, 
    int(image.cameraInfo.opticalCenter.x-cameraSpeedVectorImg.x), int(image.cameraInfo.opticalCenter.y-cameraSpeedVectorImg.y),
    3, Drawings::ps_solid, arrowColor);
//-----------------------------------------------------
  angleBetweenDirectionOfViewAndGround = 
    toDegrees(
    atan2(
    vectorInDirectionOfViewWorld.z,
    sqrt(sqr(vectorInDirectionOfViewWorld.x) + sqr(vectorInDirectionOfViewWorld.y))
    )
    );
  
  // Compute information related to the horizon
  imageInfo.maxImageCoordinates.x = image.cameraInfo.resolutionWidth - 1;
  imageInfo.maxImageCoordinates.y = image.cameraInfo.resolutionHeight - 1;
  imageInfo.horizon = Geometry::calculateHorizon(cameraMatrix, image.cameraInfo);
  imageInfo.horizon.normalizeDirection();

  imageInfo.horizonInImage = Geometry::getIntersectionPointsOfLineAndRectangle(
      Vector2<int>(0,0), imageInfo.maxImageCoordinates, imageInfo.horizon, 
      imageInfo.horizonStart, imageInfo.horizonEnd);

  imageInfo.vertLine = imageInfo.horizon;
  imageInfo.vertLine.direction.x = -imageInfo.vertLine.direction.y;
  imageInfo.vertLine.direction.y = imageInfo.horizon.direction.x;

  // Make up the rotation matrix from the direction vector of the horizon (1st column)
  // and the direction vector of the vertical line (2nd column).
  imageInfo.rotation.c[0] = imageInfo.horizon.direction;
  imageInfo.rotation.c[1] = imageInfo.vertLine.direction;
  imageInfo.invRotation = imageInfo.rotation.invert();

  // Compute the distance of the top left corner from the horizon. This is the same as 
  // the y-coordinate of the points on the horizon in the horizon-aligned coordinate 
  // system.
  imageInfo.distanceTopLeftCorner = (imageInfo.invRotation*imageInfo.horizon.base).y;

  
  // Run BeaconDetector which finds flags
  landmarksPercept.cameraOffset = cameraMatrix.translation;
  if(imageInfo.horizonInImage)
  {
    beaconDetector.execute();
  }
  
  // Tell GoalRecognizer about the seen flags, to avoid scans for goals there (the 
  // GoalRecognizer reads the flags positions from the LandmarksPercept)
  goalSpecialistY.notifyAboutFlags();
  goalSpecialistB.notifyAboutFlags();


  // Reset horizontal counters (??)
  noRedCount = noBlueCount = 100;
  closestBottom = 40000;
  goalAtBorder = false;

  // Main scanning grid
  scanColumns();
  scanRows();

  // Signalize end of scanning to specialists
  goalSpecialistY.notifyAboutFinish();
  goalSpecialistB.notifyAboutFinish();
  
  
 // circleFinder.determineCirclePoint(cameraMatrix, image, robotPose, prevCameraMatrix);
  Vector2<double> circleMidPoint;
  lineFinder.execute(linesPercept, cameraMatrix, image, imageInfo, robotPose, circleFinder.getCircle(circleMidPoint), circleMidPoint);

  DEBUG_DRAWING_FINISHED(circlePoints_image); 
  DEBUG_DRAWING_FINISHED(circlePoints_Field); 

  DEBUG_RESPONSE("gt05-ip:enable-edge-detection",
    // get line-point percepts from specialist
    //edgeSpecialist.getEdgePointPercepts(linesPercept, cameraMatrix, imageInfo.previousCameraMatrix, image);
    // get edge percepts from specialist
    edgeSpecialist.getEdgesPercept(edgesPercept, cameraMatrix, imageInfo.previousCameraMatrix, image);
  );

  // clears the multiple ball percept list
  ballSpecialist.resetMultiplePerceptsList(ballPercept);


  // Analyze results
  DEBUG_RESPONSE_NOT("gt05-ip:disable-multiple-ball-candidates",
    if (longestBallRun > 2)
    {
      int numberOfCandidates = ballClustering.getSize();
      for (int i=0; i<numberOfCandidates; i++)
      {
        Vector2<double> temp = ballClustering.getMidpoint(i);
        ballCandidate.x = (int)temp.x;
          ballCandidate.y = (int)temp.y;
          DOT(imageProcessor_ball3, ballCandidate.x, ballCandidate.y, Drawings::black, Drawings::orange);
        ballSpecialist.searchBall(image, colorTable, cameraMatrix, imageInfo.previousCameraMatrix,
                                          ballCandidate.x, ballCandidate.y, ballPercept);

        //CRASH_CHECK(i);
      }
          DEBUG_DRAWING_FINISHED(imageProcessor_multipleBallPercepts);
          DEBUG_DRAWING_FINISHED(multipleBallPerceptsField);
    }
  );
  DEBUG_RESPONSE("gt05-ip:disable-multiple-ball-candidates",
    if (longestBallRun > 2)
    {
      ballSpecialist.searchBall(image, colorTable, cameraMatrix, imageInfo.previousCameraMatrix,
                                      ballCandidate.x, ballCandidate.y, ballPercept);
    }
  );

  ballSpecialist.forwardPercept(ballPercept);
    
  //calculate the panning velocity value for the MultipleBallPerceptList
  ballPercept.multiplePercepts.calculatePanningVelocityValue(panningVelocity);

  // is not needed any more, because fieldLine-percepts now come from recognized lines
  // -- reverted --
  filterPercepts();
  
  DEBUG_RESPONSE_NOT("gt05-ip:disable-players-perception",
  {
    // Check if percepts exists and the y axis could be used to compare 
    // the results
    if ((linesPercept.points[LinesPercept::redRobot].numberOfPoints != 0
      ||linesPercept.points[LinesPercept::blueRobot].numberOfPoints != 0)
      && imageInfo.horizon.direction.x != 0
      && imageInfo.horizon.direction.y/imageInfo.horizon.direction.x < 1)
    {
      playerSpecialist.execute();
    }
  });

  //everyone else is done, let's search for red robots
#ifdef OC_GUIDEDOG
  guideDogRobotSpecialist.execute();
#endif //OC_GUIDEDOG

  // Drawing stuff
  if(imageInfo.horizonInImage)
  {
    LINE(imageProcessor_horizon,
     imageInfo.horizonStart.x, imageInfo.horizonStart.y, imageInfo.horizonEnd.x, 
     imageInfo.horizonEnd.y, 1, Drawings::ps_solid, Drawings::white );
  }
  NDECLARE_DEBUGDRAWING("imageProcessor:horizon", "drawingOnImage", "shows the horizon in the image");
  NLINE("imageProcessor:horizon",
     imageInfo.horizonStart.x, imageInfo.horizonStart.y, 
     imageInfo.horizonEnd.x, imageInfo.horizonEnd.y, 2, Drawings::ps_solid, Drawings::white );

  NDECLARE_DEBUGDRAWING("imageProcessor:obstacle detection", "drawingOnImage", "draws debug information from the obstacle detection");

  NDECLARE_DEBUGDRAWING("imageProcessor:robot detection", "drawingOnImage", "draws debug information from the robot detection");

//  NDECLARE_DEBUGDRAWING("imageProcessor:obstacle detection field", "drawingOnField", "draws debug information from the obstacle detection");

  DEBUG_DRAWING_FINISHED(imageProcessor_horizon);
  DEBUG_DRAWING_FINISHED(imageProcessor_scanLines);
  DEBUG_DRAWING_FINISHED(imageProcessor_general);
  DEBUG_DRAWING_FINISHED(imageProcessor_edges);
  DEBUG_DRAWING_FINISHED(imageProcessor_robot_detection);

  SEND_DEBUG_IMAGE(imageProcessorPlayers);
  GENERATE_DEBUG_IMAGE(imageProcessorGeneral,
    Image correctedImage(image);
    colorCorrector.correct(correctedImage);
    INIT_DEBUG_IMAGE(imageProcessorGeneral, correctedImage);
  )
  SEND_DEBUG_IMAGE(imageProcessorGeneral);

  GENERATE_DEBUG_IMAGE(segmentedImage1,
    Image correctedImage(image);
    colorCorrector.correct(correctedImage);
    colorTable.generateColorClassImage(correctedImage, segmentedImage1ColorClassImage);
  )
  SEND_DEBUG_COLOR_CLASS_IMAGE(segmentedImage1);

  GENERATE_DEBUG_IMAGE(imageProcessorGradients,
    SUSANEdgeDetectionLite edgeDetectionU(GT2005BeaconDetector::edgeThresholdU);
    SUSANEdgeDetectionLite edgeDetectionV(GT2005BeaconDetector::edgeThresholdV);
    Image edgeDetectionImage;
    bool edgeY;
    bool edgeU;
    bool edgeV;
    edgeDetectionImage.cameraInfo = image.cameraInfo;
    for (int y=1; y<edgeDetectionImage.cameraInfo.resolutionHeight-1; y++) 
    {
      for (int x=1; x<edgeDetectionImage.cameraInfo.resolutionWidth-1; x++) 
      {
        edgeY = edgeDetectionU.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentA);
        edgeU = edgeDetectionU.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentB);
        edgeV = edgeDetectionV.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentC);
        if (edgeU&&edgeV)
        {
          edgeDetectionImage.image[y][0][x] = 210;
          edgeDetectionImage.image[y][1][x] = 127;
          edgeDetectionImage.image[y][2][x] = 127;
        }
        else if (edgeU)
        {
          edgeDetectionImage.image[y][0][x] = 130;
          edgeDetectionImage.image[y][1][x] = 170;
          edgeDetectionImage.image[y][2][x] = 80;
        }
        else if (edgeV)
        {
          edgeDetectionImage.image[y][0][x] = 130;
          edgeDetectionImage.image[y][1][x] = 80;
          edgeDetectionImage.image[y][2][x] = 170;
        }
        else if (edgeY)
        {
          edgeDetectionImage.image[y][0][x] = 255;
          edgeDetectionImage.image[y][1][x] = 127;
          edgeDetectionImage.image[y][2][x] = 127;
        }
        else 
        {
          edgeDetectionImage.image[y][0][x] = image.image[y][0][x]/2;
          edgeDetectionImage.image[y][1][x] = image.image[y][1][x];
          edgeDetectionImage.image[y][2][x] = image.image[y][2][x];
        }
        edgeDetectionImage.image[y][3][x] = 128;
        edgeDetectionImage.image[y][4][x] = 128;
        edgeDetectionImage.image[y][5][x] = 128;
      }
    }
    INIT_DEBUG_IMAGE(imageProcessorGradients, edgeDetectionImage);
  )
  SEND_DEBUG_IMAGE(imageProcessorGradients);

  GENERATE_DEBUG_IMAGE(imageProcessorBall,
    Image correctedImage(image);
    colorCorrector.correct(correctedImage);
    for (int iPBix=0; iPBix<correctedImage.cameraInfo.resolutionWidth; iPBix++) 
    {
      for (int iPBiy=0; iPBiy<correctedImage.cameraInfo.resolutionHeight; iPBiy++) 
      {
        correctedImage.image[iPBiy][0][iPBix]=ballSpecialist.getSimilarityToOrange(correctedImage.image[iPBiy][0][iPBix],correctedImage.image[iPBiy][1][iPBix],correctedImage.image[iPBiy][2][iPBix]);
        if (correctedImage.image[iPBiy][0][iPBix]==0)
        {
          correctedImage.image[iPBiy][1][iPBix]=127;
          correctedImage.image[iPBiy][2][iPBix]=127;
        }
        else
        {
          if (correctedImage.image[iPBiy][0][iPBix]>30)
            correctedImage.image[iPBiy][1][iPBix]=0;
          else
            correctedImage.image[iPBiy][1][iPBix]=255;
          if (correctedImage.image[iPBiy][0][iPBix]>60)
            correctedImage.image[iPBiy][2][iPBix]=0;
          else
            correctedImage.image[iPBiy][2][iPBix]=255;
        }
      }
    }
    INIT_DEBUG_IMAGE(imageProcessorBall, correctedImage);
  )
  SEND_DEBUG_IMAGE(imageProcessorBall);
  DEBUG_DRAWING_FINISHED(imageProcessor_ball4);

  //for(i = 0; i < LinesPercept::numberOfLineTypes; ++i)
  //    numberOfPoints[i] = linesPercept.numberOfPoints[i];
  // output number of found percepts
  /*OUTPUT(idText,text,"GT2005ImageProcessor: percept-counting " << endl
    << "\tflags: " << landmarksPercept.numberOfFlags << endl
    << "\tgoals: " << landmarksPercept.numberOfGoals << endl
    << "\tlines: " << linesPercept.numberOfPoints[1] << endl
    << "\tballs: " << ballPercept.ballWasSeen << endl
    << "\tblue:  " << playersPercept.numberOfBluePlayers << endl
    << "\tred:   " << playersPercept.numberOfRedPlayers << endl
    << "\tobst:  " << obstaclesPercept.numberOfPoints
  );*/

  imageInfo.previousCameraMatrix = cameraMatrix;
  prevCmTricot = cmTricot;
  /*OUTPUT(idText, text, "---");
  for(int ii = 0; ii < linesPercept.numberOfPoints[1]; ++ii)
    OUTPUT(idText, text, int(linesPercept.points[1][ii].angle * 180 / pi));*/


//////// New Debug Images
  N_GENERATE_DEBUG_IMAGE(colorCorrectedAsJpeg,
    colorCorrectedAsJpegImage = image;
    colorCorrector.correct(colorCorrectedAsJpegImage);
  );
  N_SEND_DEBUG_IMAGE_AS_JPEG(colorCorrectedAsJpeg)

  N_GENERATE_DEBUG_IMAGE(colorCorrected,
    colorCorrectedImage = image;
    colorCorrector.correct(colorCorrectedImage);
  );
  N_SEND_DEBUG_IMAGE(colorCorrected);

  N_GENERATE_DEBUG_IMAGE(segmented,
    Image colorCorrectedImage(image);
    colorCorrector.correct(colorCorrectedImage);
    colorTable.generateColorClassImage(colorCorrectedImage, segmentedImage);
  );
  N_SEND_DEBUG_COLOR_CLASS_IMAGE(segmented)

  N_GENERATE_DEBUG_IMAGE(edgeDetection,
    SUSANEdgeDetectionLite edgeDetectionU(GT2005BeaconDetector::edgeThresholdU);
    SUSANEdgeDetectionLite edgeDetectionV(GT2005BeaconDetector::edgeThresholdV);
    bool edgeY;
    bool edgeU;
    bool edgeV;
    edgeDetectionImage.cameraInfo = image.cameraInfo;
    for (int y=1; y<edgeDetectionImage.cameraInfo.resolutionHeight-1; y++) 
    {
      for (int x=1; x<edgeDetectionImage.cameraInfo.resolutionWidth-1; x++) 
      {
        edgeY = edgeDetectionU.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentA);
        edgeU = edgeDetectionU.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentB);
        edgeV = edgeDetectionV.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentC);
        if (edgeU&&edgeV)
        {
          edgeDetectionImage.image[y][0][x] = 210;
          edgeDetectionImage.image[y][1][x] = 127;
          edgeDetectionImage.image[y][2][x] = 127;
        }
        else if (edgeU)
        {
          edgeDetectionImage.image[y][0][x] = 130;
          edgeDetectionImage.image[y][1][x] = 170;
          edgeDetectionImage.image[y][2][x] = 80;
        }
        else if (edgeV)
        {
          edgeDetectionImage.image[y][0][x] = 130;
          edgeDetectionImage.image[y][1][x] = 80;
          edgeDetectionImage.image[y][2][x] = 170;
        }
        else if (edgeY)
        {
          edgeDetectionImage.image[y][0][x] = 255;
          edgeDetectionImage.image[y][1][x] = 127;
          edgeDetectionImage.image[y][2][x] = 127;
        }
        else 
        {
          edgeDetectionImage.image[y][0][x] = image.image[y][0][x]/2;
          edgeDetectionImage.image[y][1][x] = image.image[y][1][x];
          edgeDetectionImage.image[y][2][x] = image.image[y][2][x];
        }
        edgeDetectionImage.image[y][3][x] = 128;
        edgeDetectionImage.image[y][4][x] = 128;
        edgeDetectionImage.image[y][5][x] = 128;
      }
    }
  );
  N_SEND_DEBUG_IMAGE_AS_JPEG(edgeDetection);

  N_GENERATE_DEBUG_IMAGE(ball,
    ballImage = image;
    colorCorrector.correct(ballImage);
    for (int iPBix=0; iPBix<ballImage.cameraInfo.resolutionWidth; iPBix++) 
    {
      for (int iPBiy=0; iPBiy<ballImage.cameraInfo.resolutionHeight; iPBiy++) 
      {
        ballImage.image[iPBiy][0][iPBix]=ballSpecialist.getSimilarityToOrange(ballImage.image[iPBiy][0][iPBix],ballImage.image[iPBiy][1][iPBix],ballImage.image[iPBiy][2][iPBix]);
        if (ballImage.image[iPBiy][0][iPBix]==0)
        {
          ballImage.image[iPBiy][1][iPBix]=127;
          ballImage.image[iPBiy][2][iPBix]=127;
        }
        else
        {
          if (ballImage.image[iPBiy][0][iPBix]>30)
            ballImage.image[iPBiy][1][iPBix]=0;
          else
            ballImage.image[iPBiy][1][iPBix]=255;
          if (ballImage.image[iPBiy][0][iPBix]>60)
            ballImage.image[iPBiy][2][iPBix]=0;
          else
            ballImage.image[iPBiy][2][iPBix]=255;
        }
      }
    }
  );
  N_SEND_DEBUG_IMAGE_AS_JPEG(ball);

  N_SEND_DEBUG_GRAY_SCALE_IMAGE(scanLines);
}

void GT2005ImageProcessor::scanColumns()
{
  /** NB: HX and HY in variable names denote x- and y-values in the horizon-aligned 
      coordinate system. */


  // Compute the y coordinate (in ha-coordinates) of the starting point of the scanlines
  double scanLineStartHY = imageInfo.distanceTopLeftCorner - verticalScanLineTopAboveHorizon;

  // Compute the leftmost scanline
  // The leftmost scanline is determined either through the top left corner of the image
  // or the bottom left corner. To decide which one is more left, both points are 
  // transformed into the horizon-aligned coordinate system and then the x-coordinates are
  // compared. 
  // The following facts make the code more efficient yet less readable:
  //  - The coodinates of the top left corner is (0, 0) in the horizon-aligned coordinate
  //    system.
  //  - As for the bottom right corner, the coordinate transformation only needs to be done 
  //    for the x-values, and as the x-values is zero (in image coordinates), the 
  //    transformation shrinks to a multiplication with one entry of the roation matrix.
  //  NB: There is no translation between the horzion-aligned and the image coordinate 
  //  system. 
  double bottomLeftHX = imageInfo.invRotation.c[1].x * static_cast<double>(imageInfo.maxImageCoordinates.y);
  Geometry::Line scanLine(
    imageInfo.rotation * Vector2<double>(min(0.0, bottomLeftHX), scanLineStartHY),
    imageInfo.vertLine.direction);

    // Compute the horizontal offset between scanlines
  Vector2<double> scanLineOffset(imageInfo.horizon.direction.x * verticalScanLineSpacing, imageInfo.horizon.direction.y * verticalScanLineSpacing);

  // Compute the offset to the endpoints for the short scanlines near the horizon
  Vector2<double> scanLineEnd2(imageInfo.vertLine.direction.x * verticalScanLineLength2, imageInfo.vertLine.direction.y * verticalScanLineLength2);
  Vector2<double> scanLineEnd13(imageInfo.vertLine.direction.x * verticalScanLineLength13, imageInfo.vertLine.direction.y * verticalScanLineLength13);


  // Flags for different specialists
  bool relevantForLines;
  int pixelsRelevantForGoal;

  Vector2<int> topBorderIntersection, bottomBorderIntersection;
  Vector2<int> topEndpoint, bottomEndpoint;
  
  // For each scanline from left to right
  for (int i = 0; i<=80; ++i)
  {
    // Shift scanline to the right
    scanLine.base += scanLineOffset;

    // Intersect scanline with image
    if(!Geometry::getIntersectionPointsOfLineAndRectangle(Vector2<int>(0,0), imageInfo.maxImageCoordinates,
          scanLine, topBorderIntersection, bottomBorderIntersection))
    {
      // End loop if no more intersection with image
            break;
    }

    // The scanlines are actually rays or line segments. The just calculated intersection
    // points however assume a line (infinite in both direcations). Thus we need to 
    // check, if the rectangle would also have intersected with the ray or line segment. 
    // If yes, the intersection point is used as endpoint for image scanning, if no, use 
    // the endpoint of the scanline.
    // To do the checks, the intersection points are transformed into the horizon-aligned 
    // coordinate system, in which the scanlines are parallel to the y-axis. Then we can
    // simply compare the y-coordinates.
    double topBorderHY = imageInfo.invRotation.c[0].y * topBorderIntersection.x 
               + imageInfo.invRotation.c[1].y * topBorderIntersection.y;
    double bottomBorderHY = imageInfo.invRotation.c[0].y * bottomBorderIntersection.x 
                + imageInfo.invRotation.c[1].y * bottomBorderIntersection.y;

    // Check if scanline ray/line segment is entirely below the image
    if (bottomBorderHY < scanLineStartHY)
    {
      // Skip scanline
      continue;
    }

    // Select upper endpoint
    if (topBorderHY > scanLineStartHY)
    {
      // The start point of scanline is outside the image, so use the intersection point
      topEndpoint = topBorderIntersection;
    }
    else
    {
      // Take the start point of the scanline
            topEndpoint.x = static_cast<int>(scanLine.base.x);
      topEndpoint.y = static_cast<int>(scanLine.base.y);
      ASSERT(!Geometry::clipPointInsideRectange(Vector2<int>(0,0), imageInfo.maxImageCoordinates, topEndpoint));
    }


    // The lower endpoint depends on the length of the scanline
        switch (i & 3)
    {
    // Short line segment
    case 1:
    case 3:
      // Skip if line segment is entirely above the image
      if (topBorderHY > scanLineStartHY + verticalScanLineLength13)
        continue;

      // Select lower endpoint
      if (bottomBorderHY < scanLineStartHY + verticalScanLineLength13)
      {
        // The end point of the scanline is outside the image, so use the intersection
                bottomEndpoint = bottomBorderIntersection;
      }
      else
      {
        // Use the end point of the scanline
        bottomEndpoint.x = static_cast<int>(scanLine.base.x + scanLineEnd13.x);
        bottomEndpoint.y = static_cast<int>(scanLine.base.y + scanLineEnd13.y);
        ASSERT(!Geometry::clipPointInsideRectange(Vector2<int>(0,0), imageInfo.maxImageCoordinates, bottomEndpoint));
      }

      // Scanline is not relevant for field line detection
      relevantForLines = false;
      break;


    // Medium line segment
    case 2:
      // Skip if line segment is entirely above the image
      if (topBorderHY > scanLineStartHY + verticalScanLineLength2)
        continue;

      // Select lower endpoint
      if (bottomBorderHY < scanLineStartHY + verticalScanLineLength2)
      {
        // The end point of the scanline is outside the image, so use the intersection
                bottomEndpoint = bottomBorderIntersection;
      }
      else
      {
        // Use the end point of the scanline
        bottomEndpoint.x = static_cast<int>(scanLine.base.x + scanLineEnd2.x);
        bottomEndpoint.y = static_cast<int>(scanLine.base.y + scanLineEnd2.y);
        ASSERT(!Geometry::clipPointInsideRectange(Vector2<int>(0,0), imageInfo.maxImageCoordinates, bottomEndpoint));
      }

      // Scanline is not relevant for field line detection
      relevantForLines = false;
      break;


    // Ray (infinite length)
    case 0:
    default:
      // Always use intersection point
      bottomEndpoint = bottomBorderIntersection;

      // Relevant for field line detection
      relevantForLines = true;
    }


    /** The goal detection should be only be done near the horizon. Therefore I calculate
      the (maximum) number of pixels on the current scanline that should be considered. 
      NB: Assumes the usage of the Bresenham line algorithm when scanning the lines. */

    if (i & 1)
    {
      // Entire scanline, so just take a number greater than the number of pixels on the line
      pixelsRelevantForGoal = 1024;
    }
    else
    {
      if (topBorderHY > scanLineStartHY + verticalScanLineLength13)
      {
        // Relevant part of the scanline is entirely outside (above) the image
        pixelsRelevantForGoal = -1;
      }
      else
      {
        // Compute the bottom point for goal detection (might be outside the bottom of the image, but that doesn't matter)
        int goalBottomX = static_cast<int>(scanLine.base.x + scanLineEnd13.x);
        int goalBottomY = static_cast<int>(scanLine.base.y + scanLineEnd13.y);

        // The number of pixels is the maximum of the differences of the coordinates
        pixelsRelevantForGoal = 1 + max(abs(goalBottomX-topEndpoint.x), abs(goalBottomY-topEndpoint.y));
      }
    }
    
    // Scan the image on the line between top and bottom endpoint
    scan(topEndpoint, bottomEndpoint, true, !relevantForLines, pixelsRelevantForGoal);
  }
}


void GT2005ImageProcessor::scanRows()
{
  Geometry::Line scanLine(imageInfo.horizon);
  double horizontalScanLineSpacing(20.0);
  Vector2<double> scanOffset(imageInfo.vertLine.direction * horizontalScanLineSpacing);
  scanLine.base += (scanOffset*2.0);
  Vector2<int> startPoint,
                 endPoint;
  //Test if the horizon is inside the image
  if(!Geometry::getIntersectionPointsOfLineAndRectangle(Vector2<int>(0,0),
                                                        imageInfo.maxImageCoordinates,
                                                        scanLine, startPoint, endPoint))
  {
    scanLine.base.x = (double)imageInfo.maxImageCoordinates.x / 2.0;
    scanLine.base.y = 1;
  }
  while(true)
  { 
    if(Geometry::getIntersectionPointsOfLineAndRectangle(
                                        Vector2<int>(0,0),
                                        imageInfo.maxImageCoordinates,
                                        scanLine, startPoint, endPoint))
    {
      LINE(imageProcessor_general,startPoint.x,startPoint.y,endPoint.x,endPoint.y,
       1,Drawings::ps_solid, Drawings::green);
      if(rand() > RAND_MAX / 2)
        scan(startPoint, endPoint, false, false, -1);
      else
        scan(startPoint, endPoint, false, false, -1);
      scanLine.base += scanOffset;
    }
    else
    {
      return;
    }
  }
}

void GT2005ImageProcessor::scan(const Vector2<int>& start, const Vector2<int>& end,
            bool vertical, bool noLines, int pixelsRelevantForGoal)
{
  // Signalize the begin of a new scanline
  if (vertical)
  {
    // GoalSpecialist only works on vertical scanlines
    goalSpecialistY.notifyAboutNewScanline(start);
    goalSpecialistB.notifyAboutNewScanline(start);
  }

  DEBUG_RESPONSE("gt05-ip:enable-edge-detection",
    edgeSpecialist.resetLine();
  );

  LinesPercept::LineType typeOfLinesPercept = LinesPercept::field;
  int pixelsSinceGoal = 0;
  int pixelsBetweenGoalAndObstacle = 0;
    
  Vector2<int> actual = start,
        diff = end - start,
        step(sgn(diff.x),sgn(diff.y)),
        size(abs(diff.x),abs(diff.y));
  bool isVertical = abs(diff.y) > abs(diff.x);
  int count,
      sum;
  double scanAngle = diff.angle();

  LINE(imageProcessor_scanLines,
      start.x, start.y, end.x, end.y,
      1, Drawings::ps_solid, Drawings::gray );
   
  // init Bresenham
  if(isVertical)
  {
    count = size.y;
    sum = size.y / 2;
  }
  else
  {
    count = size.x;
    sum = size.x / 2;
  }

  if(count > 7)
  {
    int numberOfPoints[LinesPercept::numberOfLineTypes],
        i;
    for(i = 0; i < LinesPercept::numberOfLineTypes; ++i)
      numberOfPoints[i] = linesPercept.points[i].numberOfPoints;
    RingBuffer<const unsigned char*,7> pixelBuffer;
    RingBuffer<const unsigned char*,7> pixelBufferFromUp;
    RingBuffer<unsigned char, 7> yBuffer;
    RingBuffer<unsigned char, 7> vBuffer;
    RingBuffer<colorClass, 7> colorClassBuffer;

    //the image is scanned vertically
    for(i = 0; i < 6; ++i) // fill buffer
    {
      unsigned char y,u,v;
      pixelBuffer.add(&image.image[actual.y][0][actual.x]);
      if (actual.x < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y < 0 || actual.y >= image.cameraInfo.resolutionHeight)
        y = u = v = 0;
      else
      {
        y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
        u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
        v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
      }
      yBuffer.add(y);
      vBuffer.add(v);
      colorClass color = COLOR_CLASS(y, u, v, colorTable);
      colorClassBuffer.add(color);
      

      /** Let specialists inspect the current pixel    
          (NB: current [dt: aktuell], NB [lat: nota bene] [dt: Hinweis]) */
      DEBUG_RESPONSE("gt05-ip:enable-edge-detection",
        edgeSpecialist.checkPoint(actual, color, cameraMatrix, imageInfo.previousCameraMatrix, image);
      );
    
      // GoalSpecialist only works close to the horizon
      if (i < pixelsRelevantForGoal)
      {
        goalSpecialistY.inspectPixel(actual, color);
        goalSpecialistB.inspectPixel(actual, color);
      }


      // Bresenham
        if(isVertical)
        {
          actual.y += step.y;
          sum += size.x;
           if(sum >= size.y)
          {
            sum -= size.y;
            actual.x += step.x;
          }
        }
        else        
        {
          actual.x += step.x;
          sum += size.y;
          if(sum >= size.x)
          {
            sum -= size.x;
            actual.y += step.y;
          }
        }
    }
    lineColor = Drawings::numberOfColors;

    int redCount = 0,
        blueCount = 0,
        greenCount = 0,
        noGreenCount = 100,
        pinkCount = 0,
        noPinkCount = 100,
        yellowCount = 0,
        noYellowCount = 100,
        skyblueCount = 0,
        noSkyblueCount = 100,
        orangeCount = 0,
        noOrangeCount = 100;
    const unsigned char* firstRed = 0,
                       * lastRed = 0,
                       * firstBlue = 0,
                       * lastBlue = 0,
                       * firstGreen = 0,
                       * firstOrange = 0,
                       * lastOrange = 0,
                       * firstPink = 0,
                       * lastPink = 0,
                       * pFirst = pixelBuffer[2],
                       * up = pFirst;
    enum{justUP, greenAndUP, down} state = justUP; // state justUP seems to be ignored
    bool borderFound = false; // there can be only one border point per scan line

    // obstacles
    enum{noGroundFound, foundBeginOfGround, foundEndOfGround} groundState = noGroundFound;
  const int minObstacleLineLength = 75;
    Vector2<int> firstGround(0,0);
    int numberOfPixelsSinceFirstOccurrenceOfGroundColor = 0;
    int numberOfPixelsSinceFirstOccurrenceOfNonGroundColor = 0;
    int numberOfPixelsSinceLastGround = 0;
    int numberOfWhitePixelsSinceLastGround = 0;
  bool beginOfGroundIsAtTheTopOfTheImage = false;
    int numberOfGroundPixels = 0;//unused
    int numberOfWhiteObstaclePixels = 0; //unused
    int numberOfNotWhiteObstaclePixels = 0; //unused

    struct BorderCandidate
    {
      bool found;
      Vector2<int> pointOnField;
      Vector2<int> pointInImage;
      double angleOnField;
      double angleInImage;
    };
    BorderCandidate borderCandidate;
    borderCandidate.found = false;

    for(; i <= count; ++i)
    {
      pixelsSinceGoal++;
      numberOfScannedPixels++;
      unsigned char y,u,v;
      pixelBuffer.add(&image.image[actual.y][0][actual.x]);
      if (actual.x < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y < 0 || actual.y >= image.cameraInfo.resolutionHeight)
        y = u = v = 0;
      else
      {
        y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
        u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
        v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
      }
      yBuffer.add(y);
      vBuffer.add(v);
      colorClass color = COLOR_CLASS(y, u, v, colorTable);
      colorClassBuffer.add(color);

      N_SET_COLORED_PIXEL_IN_GRAY_SCALE_IMAGE(scanLines, actual.x, actual.y, color);

      /** Let specialists inspect the current pixel. */
      DEBUG_RESPONSE("gt05-ip:enable-edge-detection",
        edgeSpecialist.checkPoint(actual, color, cameraMatrix, imageInfo.previousCameraMatrix, image);
      );
      
    // GoalSpecialist only works close to the horizon
      if (i < pixelsRelevantForGoal)
      {
        goalSpecialistY.inspectPixel(actual, color);
        goalSpecialistB.inspectPixel(actual, color);
      }


      // line point state machine
      const unsigned char* p3 = pixelBuffer[3];
      //UP: y-component increases ////////////////////////////////////////
      if(yBuffer[3] > yBuffer[4] + yThreshold)
      {
        up = p3;
        // is there green in one of the pixels above?
        if(colorClassBuffer[6] == green
          || colorClassBuffer[5] == green
          || colorClassBuffer[4] == green)
        {
          Vector2<int> coords = getCoords(p3),
                       pointOnField; //position on field, relative to robot
          pixelBufferFromUp.init();
          if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
            for(int k = pixelBuffer.getNumberOfEntries() - 1; k >= 0; --k)
               pixelBufferFromUp.add(pixelBuffer.getEntry(k));
          state = greenAndUP;
        }
        else 
          state = justUP;
      }
    
      //DOWN: y-component decreases //////////////////////////////////////
      else if(yBuffer[3] < 
              yBuffer[4] - yThreshold || 
              vBuffer[3] < 
              vBuffer[4] - vThreshold)
      {
        // is the pixel in the field ??
        if(state != down && // was an "up" pixel above?
           (colorClassBuffer[0] == green
           || colorClassBuffer[1] == green
           || colorClassBuffer[2] == green)) // is there green in one of the pixels below ?
        {
          colorClass c = colorClassBuffer[6];
          if(!noLines || c == skyblue || c == yellow)
          {
            Vector2<int> coords = getCoords(p3),
                         pointOnField; //position on field, relative to robot

            if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
            {
              Vector2<int> upCoords = getCoords(up);
              int height = (coords - upCoords).abs();
              int distance = (int) sqrt(sqr(cameraMatrix.translation.z) + sqr(pointOnField.abs())),
                lineHeight = (int) Geometry::getSizeByDistance(image.cameraInfo, 25, distance);

              //The bright region is assumed to be on the ground. If it is small enough, it is a field line.
              if(state == greenAndUP &&
                 (colorClassBuffer[5] == white ||
                  colorClassBuffer[4] == white))
              {
          double edgeAngleImage, edgeAngleField;
          Vector2<int> pointOnField2;
          bool edgeCalculated, isPointOnField;
          edgeCalculated = calcEdgeAngle(edgeAngleImage, edgeAngleField, coords, pointOnField, scanAngle, pixelBuffer);
          isPointOnField = Geometry::calculatePointOnField(upCoords.x, upCoords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField2);
          if (height < 3 * lineHeight)//old check, doesn't consider line orientation 
          {
            if(pixelBufferFromUp.getNumberOfEntries() > 0 && edgeCalculated && isPointOnField)
            {
              double upAngleImage, upAngleField;
              if(calcEdgeAngle(upAngleImage, upAngleField, upCoords, pointOnField2, scanAngle, pixelBufferFromUp, true))
              {
                // only add the closest field line. This makes filtering easier.
                linesPercept.points[LinesPercept::field].numberOfPoints = numberOfPoints[LinesPercept::field];
                linesPercept.add(LinesPercept::field, pointOnField2, upCoords, upAngleField, upAngleImage);
                linesPercept.add(LinesPercept::field, pointOnField, coords, edgeAngleField, edgeAngleImage);
              }
            }
          }
          //else if (isPointOnField)
          //{
           // NDOT("imageProcessor:obstacle detection",
            //  coords.x, coords.y, 
            //  Drawings::white, Drawings::white);
           // double Dx = 2*(image.image[coords.y][0][coords.x-1] - image.image[coords.y][0][coords.x + 1]);
           // Dx += image.image[coords.y-1][0][coords.x-1] - image.image[coords.y-1][0][coords.x + 1];
           // Dx += image.image[coords.y+1][0][coords.x-1] - image.image[coords.y+1][0][coords.x + 1];
           // double Dy = 2*(image.image[coords.y-1][0][coords.x] - image.image[coords.y+1][0][coords.x]);
           // Dy += image.image[coords.y-1][0][coords.x+1] - image.image[coords.y+1][0][coords.x+1];
           // Dy += image.image[coords.y-1][0][coords.x-1] - image.image[coords.y+1][0][coords.x-1];

           // Vector2<double> grad = Vector2<double>(-Dx, -Dy);
           // grad.normalize();
           // NARROW("imageProcessor:obstacle detection",
            //  coords.x, coords.y, coords.x - grad.x*10, coords.y - grad.y*10, 1,
            //  Drawings::ps_solid, Drawings::black);
           // BresenhamLineScan scanInDirectionOfGradient(coords, grad, image.cameraInfo);
           // Vector2<int> nextCoords(coords);
           // for (int i=0; i<2*lineHeight; i++)
           // {
            //  scanInDirectionOfGradient.getNext(nextCoords);
            //  y = colorCorrector.correct(nextCoords.x, nextCoords.y, 0, image.image[nextCoords.y][0][nextCoords.x]);
            //  u = colorCorrector.correct(nextCoords.x, nextCoords.y, 1, image.image[nextCoords.y][1][nextCoords.x]);
            //  v = colorCorrector.correct(nextCoords.x, nextCoords.y, 2, image.image[nextCoords.y][2][nextCoords.x]);
            //  int color = (int)COLOR_CLASS(y, u, v, colorTable);
            //  if (color != white && color != gray && color != noColor)
            //    break;
           // }
           // height = (coords - nextCoords).abs();
           // if (height <= 1.7*lineHeight)
           // {
            //  if(Geometry::calculatePointOnField(nextCoords.x, nextCoords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField2))
            //  {
            //    double angleImage = normalize(grad.angle());
            //    double angleField = normalize((pointOnField2 - pointOnField).angle());

            //    linesPercept.points[LinesPercept::field].numberOfPoints = numberOfPoints[LinesPercept::field];
            //    linesPercept.add(LinesPercept::field, pointOnField2, upCoords, normalize(angleField), normalize(angleImage));
            //    linesPercept.add(LinesPercept::field, pointOnField, coords, normalize(angleField+pi), normalize(grad.angle()+pi));
            //    NARROW("imageProcessor:obstacle detection field",
            //      pointOnField2.x, pointOnField2.y, pointOnField2.x + cos(angleField)*100, pointOnField2.y + sin(angleField)*100, 1,
            //      Drawings::ps_solid, Drawings::black);
            //    NARROW("imageProcessor:obstacle detection field",
            //      pointOnField.x, pointOnField.y, pointOnField.x + cos(normalize(angleField+pi))*100, pointOnField.y + sin(normalize(angleField+pi))*100, 1,
            //      Drawings::ps_solid, Drawings::blue);
            //    NARROW("imageProcessor:obstacle detection",
            //      nextCoords.x, nextCoords.y, nextCoords.x + grad.x*10, nextCoords.y + grad.y*10, 1,
            //      Drawings::ps_solid, Drawings::blue);
            //  }
            //  NDOT("imageProcessor:obstacle detection",
            //    nextCoords.x, nextCoords.y, 
            //    Drawings::white, Drawings::black);
           // }
          //}
              }
              else if(height >= 3 && !borderFound)
              {
                switch(c)
                {
                  case skyblue:
                    if(vertical)
                    {
                      borderFound = true;
                      if(getPlayer().getTeamColor() == Player::red && !linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints)
                        goalAtBorder = pointOnField.abs() < closestBottom;
                      closestBottom = 40000;
                      double edgeAngleField, edgeAngleImage;
                      if(calcEdgeAngle(edgeAngleImage, edgeAngleField, coords, pointOnField, scanAngle, pixelBuffer, false, false, 2))
                        linesPercept.add(LinesPercept::skyblueGoal, pointOnField, coords, edgeAngleField, edgeAngleImage);
                      typeOfLinesPercept = LinesPercept::skyblueGoal;
                      pixelsSinceGoal = 0;

                      lastRed = lastBlue = 0;
                      redCount = blueCount = 0;
                    }
                    break;
                  case yellow:
                    if(vertical)
                    {
                      borderFound = true;
                      if(getPlayer().getTeamColor() == Player::blue && !linesPercept.points[LinesPercept::yellowGoal].numberOfPoints)
                        goalAtBorder = pointOnField.abs() < closestBottom;
                      closestBottom = 40000;
                      double edgeAngleField, edgeAngleImage;
                      if(calcEdgeAngle(edgeAngleImage, edgeAngleField, coords, pointOnField, scanAngle, pixelBuffer, false, false))
                        linesPercept.add(LinesPercept::yellowGoal, pointOnField, coords, edgeAngleField, edgeAngleImage);
                      typeOfLinesPercept = LinesPercept::yellowGoal;
                      pixelsSinceGoal = 0;

                      lastRed = lastBlue = 0;
                      redCount = blueCount = 0; 
                    }
                    break;
                  case white:
                    if(height > lineHeight * 3 && vertical)
                    {
                      //borderFound = true;
                      double edgeAngleField, edgeAngleImage;
                      if(calcEdgeAngle(edgeAngleImage, edgeAngleField, coords, pointOnField, scanAngle, pixelBuffer))
                      {
                        //linesPercept.add(LinesPercept::border,pointOnField, edgeAngle);
                        borderCandidate.found = true;
                        borderCandidate.pointOnField = pointOnField;
                        borderCandidate.pointInImage = coords;
                        borderCandidate.angleOnField = edgeAngleField;
                        borderCandidate.angleInImage = edgeAngleImage;
                      }
                      typeOfLinesPercept = LinesPercept::border;
                      lastRed = lastBlue = 0;
                      redCount = blueCount = 0;
                    }
                    break;
                }
              }
            }
          }
          state = down;
        }
      }
      
      colorClass c3 = colorClassBuffer[3];
      ++noOrangeCount;
      if (c3 == orange)
      {
        if(noOrangeCount > 1)
        {
          if (orangeCount > longestBallRun)
          {
            longestBallRun = orangeCount;
            ballCandidate = (getCoords(firstOrange) + getCoords(lastOrange)) / 2;
          }
DEBUG_RESPONSE_NOT("gt05-ip:disable-multiple-ball-candidates",
          if (orangeCount>2)
          {
            ballClustering.addLine(getCoords(firstOrange), getCoords(lastOrange), orangeCount);
          }
);
          orangeCount = 1;
          firstOrange = p3;
          lastOrange = p3;
        }
        else
        {
          ++orangeCount;
          if(orangeCount > 2) // ignore robot if ball is below, distance would be wrong
          {
            lastRed = lastBlue = 0;
            redCount = blueCount = 0; 
          }
          lastOrange = p3;
        }
        firstGreen = 0;
        noOrangeCount = 0;
      }
      if(vertical)
      {
        // in scanColumns, recognize player and ball points
        ++noGreenCount;
        ++noPinkCount;
        ++noYellowCount;
        ++noSkyblueCount;
        switch(c3)
        {
          case blue: // count blue, remember last
            if(blueCount == 3)
              firstBlue = pixelBuffer[6];
            ++blueCount;
            lastBlue = p3;
            firstGreen = 0;
            break;
          case gray: // drop green above gray (i.e. robot legs)
            if(firstGreen && noGreenCount < 8)
              firstGreen = 0;
            break;
          case green: // find first green below a robot
            if(!firstGreen)
            { 
              firstGreen = p3;
            }
            if(noGreenCount > 6)
              greenCount = 1;
            else
              ++greenCount;
            noGreenCount = 0;
            break;
          case red:
            if(orangeCount < 6 || noOrangeCount > 4 || redCount > orangeCount)
            { // count red, remember last
              if(redCount == 3)
                firstRed = pixelBuffer[6];
              ++redCount;
              lastRed = p3;
              firstGreen = 0;
              break;
            }
            // no break, red below orange is interpreted as orange
          case pink:
            if(noPinkCount > 6)
            {
              pinkCount = 1;
              firstPink = p3;
            }
            else
            {
              ++pinkCount;
            }
            lastPink = p3;
            noPinkCount = 0;
            break;
          case yellow:
            if(noYellowCount > 6)
              yellowCount = 1;
            else
              ++yellowCount;
            noYellowCount = 0;
            break;
          case skyblue:
            if(noSkyblueCount > 6)
              skyblueCount = 1;
            else
              ++skyblueCount;
            noSkyblueCount = 0;
            break;
          default:
            break;
          
        }//switch(c3)

        // obstacles state machine
        if(count > minObstacleLineLength && cameraMatrix.isValid)
        {
          DEBUG_IMAGE_SET_PIXEL_BLACK(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
      NDOT("imageProcessor:obstacle detection",
        getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y, 
        Drawings::orange, Drawings::orange);
          colorClass color = colorClassBuffer[0];
          if(groundState == noGroundFound)
          {
            DEBUG_IMAGE_SET_PIXEL_YELLOW(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
      NDOT("imageProcessor:obstacle detection",
        getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y, 
        Drawings::yellow, Drawings::yellow);
            if(color == green || color == orange)
            {
              if(numberOfPixelsSinceFirstOccurrenceOfGroundColor == 0)
              {
                firstGround = getCoords(pixelBuffer[0]);
              }
              numberOfPixelsSinceFirstOccurrenceOfGroundColor++;
              if(numberOfPixelsSinceFirstOccurrenceOfGroundColor > 3 || color == orange)
              {
                Vector2<int> coords = getCoords(pixelBuffer[0]);
                int lineHeight = Geometry::calculateLineSize(coords, cameraMatrix, image.cameraInfo);
                if(i < lineHeight * 4) beginOfGroundIsAtTheTopOfTheImage = true;
                groundState = foundBeginOfGround;
                pixelsBetweenGoalAndObstacle = pixelsSinceGoal;
                numberOfPixelsSinceFirstOccurrenceOfNonGroundColor = 0;
              }
            }
            else if (color != noColor)
            {
              numberOfPixelsSinceFirstOccurrenceOfGroundColor = 0;
              if(color == white) numberOfWhiteObstaclePixels++;
              else numberOfNotWhiteObstaclePixels++;
            }
          }
          else if(groundState == foundBeginOfGround)
          {
            DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
      NDOT("imageProcessor:obstacle detection",
        getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y, 
        Drawings::dark_gray, Drawings::dark_gray);
            if(color != green && color != orange && color != noColor)
            {
              numberOfPixelsSinceFirstOccurrenceOfNonGroundColor++;
              if(numberOfPixelsSinceFirstOccurrenceOfNonGroundColor > 3)
              {
                groundState = foundEndOfGround;
                numberOfPixelsSinceLastGround = 0;
                numberOfWhitePixelsSinceLastGround = 0;
                numberOfPixelsSinceFirstOccurrenceOfGroundColor = 0;
              }
            }
            else if (color != noColor)
            {
              numberOfGroundPixels++;
              numberOfPixelsSinceFirstOccurrenceOfNonGroundColor = 0;
            }
          }
          else if(groundState == foundEndOfGround)
          {
            numberOfPixelsSinceLastGround++;
            if(color == white) numberOfWhitePixelsSinceLastGround++;
            DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
      NDOT("imageProcessor:obstacle detection",
        getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y, 
        Drawings::red, Drawings::red);

            if(color == green || color == orange)
            {
              numberOfPixelsSinceFirstOccurrenceOfGroundColor++;
              if(numberOfPixelsSinceFirstOccurrenceOfGroundColor > 3 || color == orange)
              {
                numberOfPixelsSinceFirstOccurrenceOfNonGroundColor = 0;
                groundState = foundBeginOfGround;
                Vector2<int> coords = getCoords(pixelBuffer[0]),
                             pointOnField; //position on field, relative to robot
                int lineHeight = Geometry::calculateLineSize(coords, cameraMatrix, image.cameraInfo);
                // * 4 wenn 80% weiß (schräge linie). sonst * 1.5 (keine linie)
                if(numberOfPixelsSinceLastGround > lineHeight * (numberOfPixelsSinceLastGround * 4 < numberOfWhitePixelsSinceLastGround * 5 ? 5 : 2.0))
                {
                  DEBUG_IMAGE_SET_PIXEL_PINK(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
          NDOT("imageProcessor:obstacle detection",
            getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y, 
            Drawings::blue, Drawings::blue);

                  firstGround = getCoords(pixelBuffer[0]);
                  numberOfGroundPixels = 0;
                  beginOfGroundIsAtTheTopOfTheImage = false;
                }
              }
            }
            else if (color != noColor)
            {
              numberOfPixelsSinceFirstOccurrenceOfGroundColor = 0;  
              if(color == white) 
                numberOfWhiteObstaclePixels++;
              else 
                numberOfNotWhiteObstaclePixels++;
            }
          }
        } // if(count > 100)

      }//if(vertical)
      
      PLOT(p3,ColorClasses::colorClassToDrawingsColor(c3));

      // Bresenham
      if(isVertical)
      {
        actual.y += step.y;
        sum += size.x;
        if(sum >= size.y)
        {
          sum -= size.y;
          actual.x += step.x;
        }
      }
      else        
      {
        actual.x += step.x;
        sum += size.y;
        if(sum >= size.x)
        {
          sum -= size.x;
          actual.y += step.y;
        }
      }
    }

    if(!borderFound && borderCandidate.found)
    {
      linesPercept.add(LinesPercept::border,borderCandidate.pointOnField, borderCandidate.pointInImage, borderCandidate.angleOnField, borderCandidate.angleInImage);
    }

    if (orangeCount > longestBallRun)
    {
      longestBallRun = orangeCount;
      ballCandidate = (getCoords(firstOrange) + getCoords(lastOrange)) / 2;
    }
DEBUG_RESPONSE_NOT("gt05-ip:disable-multiple-ball-candidates",
    if (orangeCount>2)
    {
      ballClustering.addLine(getCoords(firstOrange), getCoords(lastOrange), orangeCount);
    }
);

    const unsigned char* pLast = pixelBuffer[3];
    PLOT(pLast,Drawings::numberOfColors);

    if(vertical)
    { // line scanning finished, analyze results (only in scanColumns)
      int goal = getPlayer().getTeamColor() == Player::red ? LinesPercept::skyblueGoal
                                                           : LinesPercept::yellowGoal;
      if(linesPercept.points[goal].numberOfPoints == numberOfPoints[goal]) // no goal point found in this column
      {
        Vector2<int> coords = getCoords(pLast),
                     pointOnField;
        if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
        {
          int dist = pointOnField.abs();
          if(dist < closestBottom)
            closestBottom = dist;
        }
      }
      

      bool redFound = false,
           blueFound = false;

      // ---------------------------------------------------------------------------
      // ---------------------------------- Red Robots -----------------------------
      // ---------------------------------------------------------------------------
      if(redCount > blueCount && redCount > 4)
      {
        // ---------------- Init
        Vector2<int> coordsFirstRed = getCoords(firstRed),
                     coordsLastRed = getCoords(lastRed),
                     coordsFirstGreen= getCoords(firstGreen),
                     midPoint,
                     pointOnField; //position on field, relative to robot
        double distance;
        pixelBelowHorizon(coordsFirstRed,distance);
       
        // calculate by midpoint between first and last red point
        // do the same and compare results
        if(firstRed && distance >= -15)
        { 
          //pixel is not near the horizon
          midPoint = coordsFirstRed + ((coordsLastRed - coordsFirstRed) / 2);
          //DOT(imageProcessor_robot_detection, midPoint.x, midPoint.y, Drawings::white, Drawings::black);
          DOT(imageProcessor_robot_detection, coordsFirstRed.x, coordsFirstRed.y, Drawings::white, Drawings::red);
          DOT(imageProcessor_robot_detection, coordsFirstRed.x, coordsLastRed.y, Drawings::white, Drawings::red);
          NDOT("imageProcessor:robot detection", coordsFirstRed.x, coordsFirstRed.y, Drawings::white, Drawings::red);
          NDOT("imageProcessor:robot detection", coordsFirstRed.x, coordsLastRed.y, Drawings::white, Drawings::red);
          
          if (lastRedMidPoint.x != 1000)
          {
            LINE(imageProcessor_robot_detection,midPoint.x,midPoint.y,lastRedMidPoint.x,lastRedMidPoint.y,0,Drawings::ps_solid,Drawings::white);
            NLINE("imageProcessor:robot detection",midPoint.x,midPoint.y,lastRedMidPoint.x,lastRedMidPoint.y,0,Drawings::ps_solid,Drawings::white);
          }
          lastRedMidPoint = midPoint;
          Geometry::calculatePointOnField(midPoint.x, midPoint.y, cmTricot, prevCmTricot, image.cameraInfo, pointOnField);
          linesPercept.add(LinesPercept::redRobot, pointOnField, midPoint);
          redFound = true;
        }
      }
      // ---------------------------------------------------------------------------
      // --------------------------------- Blue Robots -----------------------------
      // ---------------------------------------------------------------------------
      // blue robot point found?
      else if(blueCount > redCount && blueCount > 4)
      {
        // ---------------- Init
        Vector2<int> coordsFirstBlue = getCoords(firstBlue),
                     coordsLastBlue = getCoords(lastBlue),
                     coordsFirstGreen= getCoords(firstGreen),
                     midPoint,
                     pointOnField; //position on field, relative to robot
        double distance;
        pixelBelowHorizon(coordsFirstBlue,distance);
       
        // calculate by midpoint between first and last blue point
        // do the same and compare results
        if(firstBlue && distance >= -15)
        { 
          //pixel is not near the horizon
          midPoint = coordsFirstBlue + ((coordsLastBlue - coordsFirstBlue) / 2);
          //DOT(imageProcessor_robot_detection, midPoint.x, midPoint.y, Drawings::white, Drawings::black);
          DOT(imageProcessor_robot_detection, coordsFirstBlue.x, coordsFirstBlue.y, Drawings::white, Drawings::blue);
          DOT(imageProcessor_robot_detection, coordsFirstBlue.x, coordsLastBlue.y, Drawings::white, Drawings::blue);
          NDOT("imageProcessor:robot detection", coordsFirstBlue.x, coordsFirstBlue.y, Drawings::white, Drawings::blue);
          NDOT("imageProcessor:robot detection", coordsFirstBlue.x, coordsLastBlue.y, Drawings::white, Drawings::blue);
          
          if (lastBlueMidPoint.x != 1000){
            LINE(imageProcessor_robot_detection,midPoint.x,midPoint.y,lastBlueMidPoint.x,lastBlueMidPoint.y,0,Drawings::ps_solid,Drawings::white);
            NLINE("imageProcessor:robot detection",midPoint.x,midPoint.y,lastBlueMidPoint.x,lastBlueMidPoint.y,0,Drawings::ps_solid,Drawings::white);
          }
          lastBlueMidPoint = midPoint;
          Geometry::calculatePointOnField(midPoint.x, midPoint.y, cmTricot, prevCmTricot, image.cameraInfo, pointOnField);
          linesPercept.add(LinesPercept::blueRobot, pointOnField, midPoint);
          blueFound = true;
        }
      }
      // obstacles found?
      bool addObstaclesPoint = false;
      ObstaclesPercept::Segment s;
      if(count > minObstacleLineLength && cameraMatrix.isValid)
      {
        Vector2<int> firstGroundOnField;
        bool firstGroundOnFieldIsOnField = 
          Geometry::calculatePointOnField(firstGround.x, firstGround.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, firstGroundOnField);

        Vector2<int> imageBottom = getCoords(pixelBuffer[0]);
        Vector2<int> imageBottomOnField;
        bool imageBottomIsOnField = 
          Geometry::calculatePointOnField(imageBottom.x, imageBottom.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, imageBottomOnField);

        if(groundState == foundBeginOfGround)
        {
          if(firstGroundOnFieldIsOnField)
          {
        NDOT("imageProcessor:obstacle detection",
          imageBottom.x, imageBottom.y, 
          Drawings::black, Drawings::red);
        NDOT("imageProcessor:obstacle detection",
          firstGround.x, firstGround.y, 
          Drawings::red, Drawings::black);

            addObstaclesPoint = true;
            s.nearPointOnField = Vector2<double>(imageBottomOnField.x, imageBottomOnField.y);
            s.farPointOnField = Vector2<double>(firstGroundOnField.x, firstGroundOnField.y);
            s.nearPointInImage = Vector2<double>(imageBottom.x, imageBottom.y);
            s.farPointInImage = Vector2<double>(firstGround.x, firstGround.y);
            s.farPointIsOnImageBorder = beginOfGroundIsAtTheTopOfTheImage;
          }
        }
        else if(groundState == foundEndOfGround)
        {
          int lineHeight = Geometry::calculateLineSize(imageBottom, cameraMatrix, image.cameraInfo);
          if(numberOfPixelsSinceLastGround < lineHeight * 4)
          {
            if(firstGroundOnFieldIsOnField)
            {
        NDOT("imageProcessor:obstacle detection",
          imageBottom.x, imageBottom.y, 
          Drawings::black, Drawings::blue);
        NDOT("imageProcessor:obstacle detection",
          firstGround.x, firstGround.y, 
          Drawings::blue, Drawings::black);
              addObstaclesPoint = true;
              s.nearPointOnField = Vector2<double>(imageBottomOnField.x, imageBottomOnField.y);
              s.farPointOnField = Vector2<double>(firstGroundOnField.x, firstGroundOnField.y);
              s.nearPointInImage = Vector2<double>(imageBottom.x, imageBottom.y);
              s.farPointInImage = Vector2<double>(firstGround.x, firstGround.y);
              s.farPointIsOnImageBorder = beginOfGroundIsAtTheTopOfTheImage;
            }
          }
          else if(imageBottomIsOnField)
          {
            addObstaclesPoint = true;
            s.nearPointOnField = s.farPointOnField = Vector2<double>(imageBottomOnField.x, imageBottomOnField.y);
            s.nearPointInImage = s.farPointInImage = Vector2<double>(imageBottom.x, imageBottom.y);
        NDOT("imageProcessor:obstacle detection",
          imageBottom.x, imageBottom.y, 
          Drawings::black, Drawings::yellow);
          }
        }
        else if(groundState == noGroundFound)
        {
          if(imageBottomIsOnField &&
            // the carpet often is not green under the robot
            imageBottomOnField.abs() > 150)
          {
            addObstaclesPoint = true;
            s.nearPointOnField = s.farPointOnField = Vector2<double>(imageBottomOnField.x, imageBottomOnField.y);
            s.nearPointInImage = s.farPointInImage = Vector2<double>(imageBottom.x, imageBottom.y);
        NDOT("imageProcessor:obstacle detection",
          imageBottom.x, imageBottom.y, 
          Drawings::black, Drawings::pink);
          }
        }
      }// if(count > 90 && cameraMatrix.isValid)
      if(addObstaclesPoint)
      {
        if(
          angleBetweenDirectionOfViewAndGround > -80.0 &&
          !(s.farPointOnField.x == 0 && s.farPointOnField.y == 0)
          )

        {
          switch(typeOfLinesPercept)
          {
          case LinesPercept::skyblueGoal: 
            if(pixelsBetweenGoalAndObstacle < 15) 
              s.obstacleType = ObstaclesPercept::goal; 
            obstaclesPercept.add(s);
            break;
          case LinesPercept::yellowGoal: 
            if(pixelsBetweenGoalAndObstacle < 15) 
              s.obstacleType = ObstaclesPercept::goal; 
            obstaclesPercept.add(s);
            break;
          case LinesPercept::blueRobot:
            if(getPlayer().getTeamColor() == Player::blue)
              s.obstacleType = ObstaclesPercept::teammate;
            else 
              s.obstacleType = ObstaclesPercept::opponent;
            obstaclesPercept.add(s);
            break;
          case LinesPercept::redRobot: 
            if(getPlayer().getTeamColor() == Player::red) 
              s.obstacleType = ObstaclesPercept::teammate;
            else 
              s.obstacleType = ObstaclesPercept::opponent;
            obstaclesPercept.add(s);
            break;
          case LinesPercept::border:
            break;
          }
        }
      }
    }//if(vertical)
  }
}


bool GT2005ImageProcessor::pixelBelowHorizon(Vector2<int> point, double &distance)
{
 // test if ball is below horizon
  if (!imageInfo.horizonInImage)
  {
    //dog doesnt look to the stars, so its always true
    return true;
  }
  else
  {
    Vector3<double> point3d((double)point.x,(double)point.y,0);
    // move horizon to center
    point3d.x -= imageInfo.horizon.base.x;
    point3d.y -= imageInfo.horizon.base.y;
    // save some time
    if (0.99 > (imageInfo.horizon.direction.x + imageInfo.horizon.direction.y)
       || (imageInfo.horizon.direction.x + imageInfo.horizon.direction.y) > 1.01)
      imageInfo.horizon.normalizeDirection();
    // rotate horizon to    
    RotationMatrix rot;
    rot.rotateZ(-sgn(imageInfo.horizon.direction.x) * asin(imageInfo.horizon.direction.y));
    point3d = rot * point3d;
    distance = point3d.y;
    if (distance<0)
      return false;
    return true;
  }
}
void GT2005ImageProcessor::filterPercepts()
{
  // If there are too few line points of a certain type, remove them all, they may be misreadings
  for(int i = 0; i < LinesPercept::numberOfLineTypes; ++i)
    if(linesPercept.points[i].numberOfPoints < 3)
      linesPercept.points[i].numberOfPoints = 0;

  // Remove outliers in the border
  filterLinesPercept(linesPercept, LinesPercept::border, image);


  /*
  // First run: find them
  int prev = 0;
  for(i = 1; i < linesPercept.numberOfPoints[LinesPercept::border] - 1; ++i)
  {
    LinesPercept::LinePoint& pPrev = linesPercept.points[LinesPercept::border][prev],
                           & p = linesPercept.points[LinesPercept::border][i],
                           & pNext = linesPercept.points[LinesPercept::border][i + 1];

    // Field wall is convex. So, no point can be closer than its two neighbors
    int dist = p.abs();
    if(pPrev.abs() > dist && pNext.abs() > dist)
      p.angle = LinesPercept::UNDEF;
    else
      ++prev;
  }

  // second run: remove marked points
  prev = 0;
  for(i = 1; i < linesPercept.numberOfPoints[LinesPercept::border] - 1; ++i)
    if(linesPercept.points[LinesPercept::border][i].angle != LinesPercept::UNDEF)
      linesPercept.points[LinesPercept::border][prev++] = linesPercept.points[LinesPercept::border][i];
  linesPercept.numberOfPoints[LinesPercept::border] = prev;
  */

  // Remove outliers in the field lines
  filterLinesPercept(linesPercept, LinesPercept::field, image);

#ifdef PERCEPT_FILTER

  char numberOfFreePartsOfGoals = 0;

  if (obstaclesPercept.angleToFreePartOfGoalWasDetermined[0]) numberOfFreePartsOfGoals++;
  if (obstaclesPercept.angleToFreePartOfGoalWasDetermined[1]) numberOfFreePartsOfGoals++;

  // there can only be one goal in a frame
  if (landmarksPercept.numberOfGoals == 2)
  {
    landmarksPercept.numberOfGoals = 0;
    obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
    obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
    //OUTPUT(idText, text, "2 goals in one frame");
  }

  if (numberOfFreePartsOfGoals > landmarksPercept.numberOfGoals)
  {
    obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
    obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
  }

  // there is a maximum of two flags in a frame
  if (landmarksPercept.numberOfFlags > 2)
  {
    landmarksPercept.numberOfFlags = 0;
    //OUTPUT(idText, text, "3+ flags in one frame");
  }

  if (landmarksPercept.numberOfFlags > 0)
  {
    // find invalid combinations of goal and flag
    if (landmarksPercept.numberOfGoals == 1)
    {
      if (landmarksPercept.goals[0].color == skyblue)
      {
        for (unsigned char i = 0;i<landmarksPercept.numberOfFlags;i++)
          if ((landmarksPercept.flags[i].type == Flag::pinkAboveYellow) ||
            (landmarksPercept.flags[i].type == Flag::yellowAbovePink))
          {
            //OUTPUT(idText, text, "blue goal and a yellow flag in one frame");
            landmarksPercept.numberOfGoals = 0;
            landmarksPercept.numberOfFlags = 0;
            obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
            obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
          }
      }
      else
      {
        for (unsigned char i = 0;i<landmarksPercept.numberOfFlags;i++)
          if ((landmarksPercept.flags[i].type == Flag::pinkAboveSkyblue) ||
            (landmarksPercept.flags[i].type == Flag::skyblueAbovePink))
          {
            //OUTPUT(idText, text, "yellow goal and a blue flag in one frame");
            landmarksPercept.numberOfGoals = 0;
            landmarksPercept.numberOfFlags = 0;
            obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
            obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
          }
      }
    }

    // two mutually exclusive flags
    if (landmarksPercept.numberOfFlags == 2)
    {
      switch(landmarksPercept.flags[0].type)
      {
      case Flag::pinkAboveSkyblue:
        switch(landmarksPercept.flags[1].type)
        {
        case Flag::yellowAbovePink:
          //OUTPUT(idText, text, "pinkAboveSkyblue and yellowAbovePink Flag in one frame");
          landmarksPercept.numberOfFlags = 0;
          break;
        case Flag::skyblueAbovePink:
          if (landmarksPercept.flags[0].angle > landmarksPercept.flags[1].angle)
          {
            //OUTPUT(idText, text, "pinkAboveSkyblue and skyblueAbovePink Flag in wrong order");
            landmarksPercept.numberOfFlags = 0;
          }
          break;
        case Flag::pinkAboveYellow:
          if (landmarksPercept.flags[0].angle < landmarksPercept.flags[1].angle)
          {
            //OUTPUT(idText, text, "pinkAboveSkyblue and pinkAboveYellow Flag in wrong order");
            landmarksPercept.numberOfFlags = 0;
          }
          break;
        }
        break;
      case Flag::skyblueAbovePink:
        switch(landmarksPercept.flags[1].type)
        {
        case Flag::pinkAboveYellow:
          //OUTPUT(idText, text, "skyblueAbovePink and pinkAboveYellow Flag in one frame");
          landmarksPercept.numberOfFlags = 0;
        case Flag::yellowAbovePink:
          if (landmarksPercept.flags[0].angle > landmarksPercept.flags[1].angle)
          {
            //OUTPUT(idText, text, "skyblueAbovePink and yellowAbovePink Flag in wrong order");
            landmarksPercept.numberOfFlags = 0;
          }
          break;
        case Flag::pinkAboveSkyblue:
          if (landmarksPercept.flags[0].angle < landmarksPercept.flags[1].angle)
          {
            //OUTPUT(idText, text, "skyblueAbovePink and pinkAboveSkyblue Flag in wrong order");
            landmarksPercept.numberOfFlags = 0;
          }
          break;
        }
        break;
      case Flag::yellowAbovePink:
        switch(landmarksPercept.flags[1].type)
        {
        case Flag::pinkAboveSkyblue:
          //OUTPUT(idText, text, "yellowAbovePink and pinkAboveSkyblue Flag in one frame");
          landmarksPercept.numberOfFlags = 0;
          break;
        case Flag::pinkAboveYellow:
          if (landmarksPercept.flags[0].angle > landmarksPercept.flags[1].angle)
          {
            //OUTPUT(idText, text, "yellowAbovePink and pinkAboveYellow Flag in wrong order");
            landmarksPercept.numberOfFlags = 0;
          }
          break;
        case Flag::skyblueAbovePink:
          if (landmarksPercept.flags[0].angle < landmarksPercept.flags[1].angle)
          {
            //OUTPUT(idText, text, "yellowAbovePink and skyblueAbovePink Flag in wrong order");
            landmarksPercept.numberOfFlags = 0;
          }
          break;
        }
        break;
      case Flag::pinkAboveYellow:
        switch(landmarksPercept.flags[1].type)
        {
        case Flag::skyblueAbovePink:
          //OUTPUT(idText, text, "pinkAboveYellow and skyblueAbovePink Flag in one frame");
          landmarksPercept.numberOfFlags = 0;
          break;
        case Flag::pinkAboveSkyblue:
          if (landmarksPercept.flags[0].angle > landmarksPercept.flags[1].angle)
          {
            //OUTPUT(idText, text, "pinkAboveYellow and pinkAboveSkyblue Flag in wrong order");
            landmarksPercept.numberOfFlags = 0;
          }
          break;
        case Flag::yellowAbovePink:
          if (landmarksPercept.flags[0].angle < landmarksPercept.flags[1].angle)
          {
            //OUTPUT(idText, text, "pinkAboveYellow and yellowAbovePink Flag in wrong order");
            landmarksPercept.numberOfFlags = 0;
          }
          break;
        }
        break;
      }
    }
  }

  // filters impossible relations between flags and goals
  if (landmarksPercept.numberOfFlags != 0)
    if (landmarksPercept.numberOfGoals == 1)
    {
      for (int i = 0;i<landmarksPercept.numberOfFlags;i++)
        if (landmarksPercept.goals[0].color == skyblue)
        {
          // the skyblueAbovePink Flag is on the left side of the blue goal
          if (landmarksPercept.flags[i].type == Flag::skyblueAbovePink)
          {
            if (landmarksPercept.flags[i].angle < (landmarksPercept.goals[0].angle + minAngleBetweenFlagAndGoal))
            {
              //              OUTPUT(idText, text, "skyblueAbovePink flag and blue goal in wrong order");
              landmarksPercept.numberOfFlags = 0;
              landmarksPercept.numberOfGoals = 0;
              obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
              obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
              return;
            }
          }
          else
          {
            // the pinkAboveSkyBlue Flag is on the right side of the blue goal
            if ((landmarksPercept.flags[i].angle + minAngleBetweenFlagAndGoal) > landmarksPercept.goals[0].angle)
            {
              //              OUTPUT(idText, text, "pinkAboveSkyBlue flag and blue goal in wrong order");
              landmarksPercept.numberOfFlags = 0;
              landmarksPercept.numberOfGoals = 0;
              obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
              obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
              return;
            }
          }
        }
        else // yellow goal
        {
          // the yellowAbovePink Flag is on the right side of the yellow goal
          if (landmarksPercept.flags[i].type == Flag::yellowAbovePink)
          {
            if ((landmarksPercept.flags[i].angle + minAngleBetweenFlagAndGoal) > landmarksPercept.goals[0].angle)
            {
              //              OUTPUT(idText, text, "yellowAbovePink flag and yellow goal in wrong order");
              landmarksPercept.numberOfFlags = 0;
              landmarksPercept.numberOfGoals = 0;
              obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
              obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
              return;
            }
          }
          else
          {
            // the pinkAboveYellow Flag is on the left side of the yellow goal
            if (landmarksPercept.flags[i].angle < (landmarksPercept.goals[0].angle + minAngleBetweenFlagAndGoal))
            {
              //              OUTPUT(idText, text, "pinkAboveYellow flag and yellow goal in wrong order");
              landmarksPercept.numberOfFlags = 0;
              landmarksPercept.numberOfGoals = 0;
              obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
              obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
              return;
            }
          }
        }
    }
#endif
  /*
  // First run: find them
  prev = 0;
  for(i = 2; i <= linesPercept.numberOfPoints[LinesPercept::field]; i += 2)
  {
    LinesPercept::LinePoint& pPrev = linesPercept.points[LinesPercept::field][i - 2],
                           & p = linesPercept.points[LinesPercept::field][i];

    // end of line?
    double angle = fabs(((Vector2<int>&) pPrev).angle() - ((Vector2<int>&) p).angle());
    int dist1 = pPrev.abs(),
        dist2 = p.abs();
    if(i == linesPercept.numberOfPoints[LinesPercept::field] ||
       angle > 0.08 ||
       abs(dist1 - dist2) > 2 * fabs(tan(angle / 2) * (dist1 + dist2)))
    {
      if(i - prev < 6)
        while(prev < i)
          linesPercept.points[LinesPercept::field][prev++].angle = LinesPercept::UNDEF;
      prev = i;
    }
  }

  // second run: remove marked points
  prev = 0;
  for(i = 0; i < linesPercept.numberOfPoints[LinesPercept::field]; ++i)
    if(linesPercept.points[LinesPercept::field][i].angle != LinesPercept::UNDEF)
      linesPercept.points[LinesPercept::field][prev++] = linesPercept.points[LinesPercept::field][i];
  linesPercept.numberOfPoints[LinesPercept::field] = prev;
  */
}

bool GT2005ImageProcessor::calcEdgeAngle(
  double& angleInImage,
  double& angleOnField,
  const Vector2<int>& pointInImage, 
  const Vector2<int>& pointOnField,
  double scanAngle,
  const RingBuffer<const unsigned char*,7>& pixelBuffer,
  const bool againstScanline,
  const bool borderOrLine,
  int channel)
{
  // find maximum gradient around point on the scan line
  int indices[] = {3, 4, 2, 5, 1};
  Vector2<double> maxGrad;
  double maxLength = -1;
  for(unsigned int i = 0; i < sizeof(indices) / sizeof(indices[0]); ++i)
  {
    Vector2<int> point = getCoords(pixelBuffer[indices[i]]);
    Vector2<double> grad = Vector2<double>(double(image.image[point.y][channel][point.x]) - image.image[point.y + 1][channel][point.x - 1] ,
      double(image.image[point.y + 1][channel][point.x]) - image.image[point.y][channel][point.x - 1]);
    double length = grad.abs();

    if(length > maxLength)
    {
      Vector2<double> gradRotated = (rotation2x2 * Vector2<double>(grad.x, -grad.y)).normalize();
      gradRotated.y *= -1;
      // filter gradient which are in wrong direction (belong to other side of line)
      double angle = gradRotated.angle();
      bool similarDirection = (angle > scanAngle && angle < scanAngle + pi) || (angle < scanAngle - pi && angle > scanAngle - pi2);
      if(againstScanline != similarDirection)
      {
        maxGrad = grad;
        maxLength = length;
      }
    }
  }

  if(maxLength > 0)
  {
    Vector2<double> gradRotated = (rotation2x2 * Vector2<double>(maxGrad.x, -maxGrad.y)).normalize();
    gradRotated.y *= -1;
    
    // check whether edge direction differs too much from scan direction
    double diff = fabs(scanAngle - gradRotated.angle());
    if(diff > pi)
      diff = pi2 - diff;
    if(diff < pi / 6 || diff > 5 * pi / 6)
    {
      angleInImage = angleOnField = LinesPercept::UNDEF;
      return false;
    }
    Vector2<int> endPoint(int(pointInImage.x + gradRotated.x * 10), int(pointInImage.y + gradRotated.y * 10));
    ARROW(imageProcessor_edges, 
      pointInImage.x,
      pointInImage.y,
      endPoint.x,
      endPoint.y,
      1, 1, (againstScanline ? Drawings::red : Drawings::pink)
    );    

    if (borderOrLine)
    {
      //Vector2<int> pointImage = pointInImage;
     // Vector2<int> pointField = pointOnField;
//      circleFinder.addCandidate(pointField,pointImage);
      Vector2<double> normalToLine = gradRotated;
      normalToLine = normalToLine.rotateLeft();
      Vector2<int> pointOnLine = pointInImage;
      lineFinder.considerLinePoint(pointOnLine, normalToLine);
    }

    Vector2<int> point;
    if(Geometry::calculatePointOnField(endPoint.x, endPoint.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, point))
    {
      angleInImage = normalize((endPoint - pointInImage).angle() - pi_2);
      angleOnField = normalize((point - pointOnField).angle() + pi_2);
      return true;
    }
  }
  angleInImage = angleOnField = LinesPercept::UNDEF;
  return false;
}

void GT2005ImageProcessor::plot(const unsigned char* p,Drawings::Color color)
{
  if(lineColor == Drawings::numberOfColors)
  {
    last = p;
    lineColor = color;
  }
  else if(color != lineColor)
  {
    Vector2<int> p1 = getCoords(last),
                 p2 = getCoords(p);
    LINE(imageProcessor_general,p1.x,p1.y,p2.x,p2.y,0,Drawings::ps_solid,lineColor);
    last = p;
    lineColor = color;
  }
}

bool GT2005ImageProcessor::handleMessage(InMessage& message)
{
  switch(message.getMessageID())
  {
/*
    case idLinesImageProcessorParameters:
    {
      GenericDebugData d;
      message.bin >> d;
      yThreshold = (int)d.data[1];
      vThreshold = (int)d.data[2];
      execute();
      return true;
    }
*/
    case idColorTable64: beaconDetector.analyzeColorTable();
      return true;
  }
  return false;
}

void GT2005ImageProcessor::filterLinesPercept(LinesPercept& percept, int type, const Image& image)
{
  const int maxNumberOfEdgePoints = 200;
  double maxAngle = 0.25; // 8.6°
  double maxDist = 0.187; // 80°

  int i, j;

  // for all lines, calculate how many other lines are similar/near in sense of this special norm
  bool similar[maxNumberOfEdgePoints][maxNumberOfEdgePoints];
  for(i = 0; i < percept.points[type].numberOfPoints; i++)
  {
    Vector2<double> dir1 = Vector2<double>(cos(percept.points[type].pointsOnField[i].angle),sin(percept.points[type].pointsOnField[i].angle));

    //int lineHeightI = Geometry::calculateLineSize(percept.points[type][i], cameraMatrix, image.cameraInfo);
    for(j = i + 1; j < percept.points[type].numberOfPoints; j++)
    {
      // similar/near if distance is small and normal is in the same direction
      bool sim = false;
      // test projection first - to remove unneeded calculations of "expensive" distance
      double angleDiff = fabs(normalize(percept.points[type].pointsOnField[i].angle - percept.points[type].pointsOnField[j].angle));
      if(angleDiff < maxAngle)
      {
        // distance point to line:
        // http://mo.mathematik.uni-stuttgart.de/kurse/kurs8/seite44.html
        // NOTE: this is because in fact, the line.direction is here the *normal* to the real line
        Vector2<double> v1 = Vector2<double>((double)(percept.points[type].pointsOnField[j] - percept.points[type].pointsOnField[i]).x,(double)(percept.points[type].pointsOnField[j] - percept.points[type].pointsOnField[i]).y).normalize();
        double distance1 = fabs(v1 * dir1);
        if(distance1 < maxDist)
        {
          Vector2<double> dir2 = Vector2<double>(cos(percept.points[type].pointsOnField[j].angle),sin(percept.points[type].pointsOnField[j].angle));
          double distance2 = fabs(-v1 * dir2);
          sim = (distance2 < maxDist);
        }
      }
      similar[i][j] = sim;
      similar[j][i] = sim;
      // draw similarity-lines
      /*Vector2<int> p1,p2;
      Geometry::calculatePointInImage(percept.points[type][i], cameraMatrix, image.cameraInfo, p1);
      Geometry::calculatePointInImage(percept.points[type][j], cameraMatrix, image.cameraInfo, p2);
      if(sim) LINE(imageProcessor_edges, 
        p1.x, 
        p1.y, 
        p2.x, 
        p2.y,
        0, 0, 1
      );*/
    }
  }

  // remove points with low number of similar points
  int nextFree = 0;

  // for all points, calculate how many other are similar
  for(i = 0; i < percept.points[type].numberOfPoints; i++)
  {
    int countSim = 0;
    //if(edgePoints[i].belongsToLineNo == -1) // only if point is not yet matched to an edge
    for (j = 0; j < percept.points[type].numberOfPoints; j++)
    {
      //if(edgePoints[j].belongsToLineNo == -1) // only if point is not yet matched to an edge
      if(similar[i][j])
      {
        countSim++;
      }
    }
    if(countSim >= 2) // keep percept
    {
      percept.points[type].pointsInImage[nextFree] = percept.points[type].pointsInImage[i];
      percept.points[type].pointsOnField[nextFree++] = percept.points[type].pointsOnField[i];
    }
  }
  percept.points[type].numberOfPoints = nextFree;
}

---