Modules/ImageProcessor/VLCImageProcessor/VLCImageProcessor.cpp

Go to the documentation of this file.

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

#include "VLCImageProcessor.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "Tools/FieldDimensions.h"
#include "Tools/Math/Geometry.h"
#include "VLCImageProcessorTools.h"
#include "Representations/Perception/JPEGImage.h"
#include "Tools/Streams/InStreams.h"

// for VLC
#include "Tools/Location.h"
//

// Parameters for the scanning grid
const double VLCImageProcessor::verticalScanLineTopAboveHorizon = 15.0;
const double VLCImageProcessor::verticalScanLineLength13 = 55.0;
const double VLCImageProcessor::verticalScanLineLength2 = 75.0;
const double VLCImageProcessor::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 VLCImageProcessor::minAngleBetweenFlagAndGoal = 0.05;


VLCImageProcessor::VLCImageProcessor(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),
ballClustering(),
lineFinder(colorCorrector, colorTable),
circleFinder(cameraMatrix, image)
{
  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)
  );

  // --- for VLC
  averageYBuffer.init();

  // instantiate the 4 colortables with this ugly code copied from the ColorTable64Mod.cpp
  colorTableVeryDark = new ColorTable64();
  InBinaryFile stream1(getLocation().getModelFilename("verydark.c64"));
  colorTableVeryDark->setFormat(ColorTable64::CT64);
  if (stream1.exists())
  {
    stream1 >> *colorTableVeryDark;
  }else{
    //OUTPUT(idText, text, "load verydark.c64 FAILED");
    //colorTableVeryDark = &((ColorTable64)colorTable);
  }

  colorTableDark = new ColorTable64();
  InBinaryFile stream2(getLocation().getModelFilename("dark.c64"));
  colorTableDark->setFormat(ColorTable64::CT64);
  if (stream2.exists())
  {
    stream2 >> *colorTableDark;
  }else{
    //OUTPUT(idText, text, "load dark.c64 FAILED");
    //colorTableDark = &((ColorTable64)colorTable);
  }
  
  colorTableMedium = new ColorTable64();
  InBinaryFile stream3(getLocation().getModelFilename("medium.c64"));
  colorTableMedium->setFormat(ColorTable64::CT64);
  if (stream3.exists())
  {
    stream3 >> *colorTableMedium;
  }else{
    //OUTPUT(idText, text, "load medium.c64 FAILED");
    //colorTableMedium = &((ColorTable64)colorTable);
  }
  
  colorTableBright = new ColorTable64();
  InBinaryFile stream4(getLocation().getModelFilename("bright.c64"));
  colorTableBright->setFormat(ColorTable64::CT64);
  if (stream4.exists())
  {
    stream4 >> *colorTableBright;
  }else{
    //OUTPUT(idText, text, "load bright.c64 FAILED");
    //colorTableBright = &((ColorTable64)colorTable);
  }

  // read the thresholds
  vd2d=d2m=m2b=0.0;  

  //read port
  InConfigFile file(getLocation().getModelFilename("vlc.cfg"));
  // format: 
  // VeryDark2Dark x.x
  // Dark2Medium x.x
  // Medium2Bright x.x
  // spaces and nl are ignored
  if(file.exists())
  {
    char buf[250];
    while(!file.eof())
    {
      file >> buf;
      if ( strcmp(buf,"VeryDark2Dark")==0 )
      {
        file >> vd2d;
        // OUTPUT(idText, text, "VeryDark2Dark-Threshold = " << vd2d);
      }
      else if ( strcmp(buf,"Dark2Medium")==0 )
      {
        file >> d2m;
        // OUTPUT(idText, text, "Dark2Medium-Threshold = " << d2m);
      }
      else if ( strcmp(buf,"Medium2Bright")==0 )
      {
        file >> m2b;
        // OUTPUT(idText, text, "Medium2Bright-Threshold = " << m2b);
      }
    }
  }

  // just for initialisation
  bestColorTable = colorTableMedium;

  averageY = 100;
  numberOfScannedPixels = 1;

  // --- end (for VLC)
}

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

  xFactor = yFactor = image.cameraInfo.focalLength;

  
  // for testing 
  for (int j = 0; j<15; j+=2)
  {
    RobotPose base;
    base.translation.x=0;
    base.translation.y=0;
    base.rotation=0;
    RobotPose testPose,testPose2;
    Pose2D movement(fromDegrees(5),300,100);
    Pose2D movement2(0,300,100);
    base.calculateMovement(movement,j,testPose);
    Vector2<double> temp(testPose.translation.x + 100 * cos(testPose.rotation),testPose.translation.y + 100 * sin(testPose.rotation));
    ARROW(lineCrossingsField, testPose.translation.x, testPose.translation.y, temp.x, temp.y, 3, Drawings::ps_solid, Drawings::orange);
    base.calculateMovement(movement2,j,testPose2);
    Vector2<double> temp2(testPose2.translation.x + 100 * cos(testPose2.rotation),testPose2.translation.y + 100 * sin(testPose2.rotation));
    ARROW(lineCrossingsField, testPose2.translation.x, testPose2.translation.y, temp2.x, temp2.y, 3, Drawings::ps_solid, Drawings::red);
  }
  //*/

  // for VLC
  averageY /= numberOfScannedPixels;
  averageU /= numberOfScannedPixels;
  averageV /= numberOfScannedPixels;
  OUTPUT(idText,text,"Statistics:\t" << averageY << "\t" << averageU << "\t" << averageV);
  if (averageY<255.0)
  { // the first value may be fucked up and then messes up the first 5 frames (ok, who cares about five frames, but this way its totally correct)
    averageYBuffer.add(averageY);
  }


  // decide which colorTable to use in this frame (based on the statistics of the last one)
  int numberOfEntrys = averageYBuffer.getNumberOfEntries();
  double smoothedY = 0;
  for (int x=0; x<numberOfEntrys; x++)
  {
    smoothedY += averageYBuffer[x];
  }
  if (numberOfEntrys>0)
  {
    smoothedY /= numberOfEntrys;
  }

  if (smoothedY<vd2d)
  {
    bestColorTable = colorTableVeryDark;
    //OUTPUT(idText,text,"VLCImageProcessor: very dark");
  }
  else if (smoothedY<d2m)
  {
    bestColorTable = colorTableDark;
    //OUTPUT(idText,text,"VLCImageProcessor: dark");
  }
  else if (smoothedY<m2b)
  {
    bestColorTable = colorTableMedium;
    //OUTPUT(idText,text,"VLCImageProcessor: medium");
  }
  else
  {
    bestColorTable = colorTableBright;
    //OUTPUT(idText,text,"VLCImageProcessor: bright");
  }

  // reset statistics
  averageY = 0;
  averageU = 0;
  averageV = 0;

  beaconDetector.setBestColorTable(bestColorTable);
  //goalRecognizer.setBestColorTable(bestColorTable);
  goalSpecialistB.setBestColorTable(bestColorTable);
  goalSpecialistY.setBestColorTable(bestColorTable);
  lineFinder.setBestColorTable(bestColorTable);
  // end (for VLC) 
  
  numberOfScannedPixels = 0;

  // 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;

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


  // Scan through image
  scanColumns();

  // finish clustering (??)
  for(int i = 0; i < 3; ++i)
  {
    clusterRobots(0,false,false);
  }

  scanRows();

  // Signalize end of scanning to specialists
  goalSpecialistY.notifyAboutFinish();
  goalSpecialistB.notifyAboutFinish();
  
  
  circleFinder.determineCirclePoint();
  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, *bestColorTable, 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, *bestColorTable, 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();
  
  landmarksPercept.cameraOffset = cameraMatrix.translation;
  if(imageInfo.horizonInImage)
  {
    beaconDetector.execute();
  }

  // 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 );
  }
  DEBUG_DRAWING_FINISHED(imageProcessor_horizon);
  DEBUG_DRAWING_FINISHED(imageProcessor_scanLines);
  DEBUG_DRAWING_FINISHED(imageProcessor_general);
  DEBUG_DRAWING_FINISHED(imageProcessor_edges);

  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);
    (*bestColorTable).generateColorClassImage(correctedImage, segmentedImage1ColorClassImage);
  )
  SEND_DEBUG_COLOR_CLASS_IMAGE(segmentedImage1);

  GENERATE_DEBUG_IMAGE(imageProcessorGradients,
    SUSANEdgeDetectionLite edgeDetectionU(VLCBeaconDetector::edgeThresholdU);
    SUSANEdgeDetectionLite edgeDetectionV(VLCBeaconDetector::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,"VLCImageProcessor: 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(colorCorrected,
    colorCorrectedImage = image;
    colorCorrector.correct(colorCorrectedImage);
  );
  N_SEND_DEBUG_IMAGE_AS_JPEG(colorCorrected)

  N_GENERATE_DEBUG_IMAGE(colorCorrected2,
    colorCorrected2Image = image;
    colorCorrector.correct(colorCorrected2Image);
  );
  N_SEND_DEBUG_IMAGE(colorCorrected2);

  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(VLCBeaconDetector::edgeThresholdU);
    SUSANEdgeDetectionLite edgeDetectionV(VLCBeaconDetector::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 VLCImageProcessor::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 VLCImageProcessor::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 VLCImageProcessor::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, (*bestColorTable));
      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;
    bool borderFound = false; // there can be only one border point per scan line

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

    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, (*bestColorTable));
      colorClassBuffer.add(color);

      // for VLC
      averageY += y;
      averageU += u;
      averageV += v;
      //

      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(height < 3 * lineHeight && state == greenAndUP &&
                 (colorClassBuffer[5] == white ||
                  colorClassBuffer[4] == white))
              {
                double edgeAngleImage, edgeAngleField;
                Vector2<int> pointOnField2;
                if(pixelBufferFromUp.getNumberOfEntries() > 0 &&
                   calcEdgeAngle(edgeAngleImage, edgeAngleField, coords, pointOnField, scanAngle, pixelBuffer) &&
                   Geometry::calculatePointOnField(upCoords.x, upCoords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField2))
                {
                  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(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 || height > 30))
                    {
                      //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 > 90 && cameraMatrix.isValid)
        {
          DEBUG_IMAGE_SET_PIXEL_BLACK(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
          colorClass color = colorClassBuffer[0];
          if(groundState == noGroundFound)
          {
            DEBUG_IMAGE_SET_PIXEL_YELLOW(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
            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);
            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);
            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 ? 4 : 1.5))
                {
                  DEBUG_IMAGE_SET_PIXEL_PINK(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
                  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 robot point found?
      if(redCount > blueCount && (firstGreen && redCount > 2 || redCount > 20))
      { // drop border and goal points
        for(i = 1; i < LinesPercept::numberOfLineTypes; ++i)
          linesPercept.points[i].numberOfPoints = numberOfPoints[i];
        if(redCount > 20)
        { // if robot is close, use upper boundary of tricot to calculate distance
          Vector2<int> coords = getCoords(firstRed),
                       pointOnField; //position on field, relative to robot
          if(Geometry::calculatePointOnField(coords.x, coords.y, cmTricot, prevCmTricot, image.cameraInfo, pointOnField) &&
             pointOnField.abs() < 500)
          {
            DOT(imageProcessor_general, coords.x, coords.y, Drawings::red, Drawings::white);
            linesPercept.add(LinesPercept::redRobot,pointOnField,coords);
            typeOfLinesPercept = LinesPercept::redRobot;
            redFound = true;
          }
        }
        if(!redFound)
        { // otherwise, use foot point
          Vector2<int> coords = getCoords(firstGreen),
                       pointOnField; //position on field, relative to robot
          if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
          {
            Vector2<int> redCoords = getCoords(lastRed);
            if((redCoords - coords).abs() < Geometry::getSizeByDistance(image.cameraInfo, 100, pointOnField.abs()))
            {
              linesPercept.add(LinesPercept::redRobot,pointOnField,coords);
              typeOfLinesPercept = LinesPercept::redRobot;
              redFound = true;
            }
          }
        }
      }
      // blue robot point found?
      else if(blueCount > redCount && (firstGreen && blueCount > 2 || blueCount > 10))
      { // drop border and goal points
        for(i = 1; i < LinesPercept::numberOfLineTypes; ++i)
          linesPercept.points[i].numberOfPoints = numberOfPoints[i];
        if(blueCount > 10)
        { // if robot is close, use upper boundary of tricot to calculate distance
          Vector2<int> coords = getCoords(firstBlue),
                       pointOnField; //position on field, relative to robot
          if(Geometry::calculatePointOnField(coords.x, coords.y, cmTricot, prevCmTricot, image.cameraInfo, pointOnField) &&
             pointOnField.abs() < 500)
          {
            DOT(imageProcessor_general, coords.x, coords.y, Drawings::pink, Drawings::white);
            linesPercept.add(LinesPercept::blueRobot,pointOnField,coords);
            typeOfLinesPercept = LinesPercept::blueRobot;
            blueFound = true;
          }
        }
        if(!blueFound)
        { // otherwise, use foot point
          Vector2<int> coords = getCoords(firstGreen),
                       pointOnField; //position on field, relative to robot
          if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
          {
            Vector2<int> blueCoords = getCoords(lastBlue);
            if((blueCoords - coords).abs() < Geometry::getSizeByDistance(image.cameraInfo, 100, pointOnField.abs()))
            {
              linesPercept.add(LinesPercept::blueRobot,pointOnField,coords);
              typeOfLinesPercept = LinesPercept::blueRobot;
              blueFound = true;
            }
          }
        }
      }
      // cluster the robot points found
      clusterRobots(pLast, redFound, blueFound);

      // obstacles found?
      bool addObstaclesPoint = false;
      ObstaclesPercept::Segment s;
      if(count > 90 && 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)
          {
            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)
            {
              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);
          }
        }
        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);
          }
        }
      }// 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; 
            break;
          case LinesPercept::yellowGoal: 
            if(pixelsBetweenGoalAndObstacle < 15) 
              s.obstacleType = ObstaclesPercept::goal; 
            break;
          case LinesPercept::blueRobot:
            if(getPlayer().getTeamColor() == Player::blue)
              s.obstacleType = ObstaclesPercept::teammate;
            else 
              s.obstacleType = ObstaclesPercept::opponent;
            break;
          case LinesPercept::redRobot: 
            if(getPlayer().getTeamColor() == Player::red) 
              s.obstacleType = ObstaclesPercept::teammate;
            else 
              s.obstacleType = ObstaclesPercept::opponent;
            break;
          //case LinesPercept::border:
          //  s.obstacleType = ObstaclesPercept::border;
          }

          obstaclesPercept.add(s);
        }
      }
    }//if(vertical)
  }
}

void VLCImageProcessor::clusterRobots(const unsigned char* bottomPoint,
                                         bool redFound, bool blueFound)
{
  Vector2<int> coords = getCoords(bottomPoint),
               pointOnField;
  int noRedCount2 = noRedCount;

  if(redFound)
  {
    lastRed = 
      linesPercept.points[LinesPercept::redRobot].pointsOnField[linesPercept.points[LinesPercept::redRobot].numberOfPoints - 1];
    if(noRedCount > 2)
      firstRed = closestRed = lastRed;
    else if(closestRed.abs() > lastRed.abs())
      closestRed = lastRed;
    noRedCount = 0;
  }
  // count number of columns without robot points, but ignore columns that are
  // not long enough to contain the robot. Drop single robot points of the other 
  // robot color.
  else if(noRedCount > 2 || !bottomPoint ||
          (Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField) &&
          closestRed.abs() > pointOnField.abs()))
  {
    if(++noRedCount == 3 && (firstRed != lastRed || noBlueCount > 4))
    {
      SinglePlayerPercept percept;
      percept.directionOnField = (atan2((double)firstRed.y,(double)firstRed.x) + atan2((double)lastRed.y,(double)lastRed.x)) / 2;
      double distance = closestRed.abs();
      percept.offsetOnField.x = cos(percept.directionOnField) * distance;
      percept.offsetOnField.y = sin(percept.directionOnField) * distance;
      percept.positionInImage = coords;
      percept.validity = 1;
      playersPercept.addRedPlayer(percept);
    }
  }

  if(blueFound)
  {
    lastBlue = 
      linesPercept.points[LinesPercept::blueRobot].pointsOnField[linesPercept.points[LinesPercept::blueRobot].numberOfPoints - 1];
    if(noBlueCount > 2)
      firstBlue = closestBlue = lastBlue;
    else if(closestBlue.abs() > lastBlue.abs())
      closestBlue = lastBlue;
    noBlueCount = 0;
  }
  // count number of columns without robot points, but ignore columns that are
  // not long enough to contain the robot. Drop single robot points of the other 
  // robot color.
  else if(noBlueCount > 2 || !bottomPoint ||
          (Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField) &&
          closestBlue.abs() > pointOnField.abs()))
  {
    if(++noBlueCount == 3 && (firstBlue != lastBlue || noRedCount2 > 4))
    {
      SinglePlayerPercept percept;
      percept.directionOnField = (atan2((double)firstBlue.y,(double)firstBlue.x) + atan2((double)lastBlue.y,(double)lastBlue.x)) / 2;
      double distance = closestBlue.abs();
      percept.offsetOnField.x = int(cos(percept.directionOnField) * distance);
      percept.offsetOnField.y = int(sin(percept.directionOnField) * distance);
      percept.positionInImage = coords;
      percept.validity = 1;
      playersPercept.addBluePlayer(percept);
    }
  }
}
 
void VLCImageProcessor::filterPercepts()
{
  // Close robots produce far ghost robots. Remove them.
  int i;
  for(i = 0; i < playersPercept.numberOfRedPlayers + playersPercept.numberOfBluePlayers; ++i)
  {
    SinglePlayerPercept& pi = i < playersPercept.numberOfRedPlayers 
                      ? playersPercept.redPlayers[i]
                      : playersPercept.bluePlayers[i - playersPercept.numberOfRedPlayers];
    double iDist = pi.offsetOnField.abs(),
           iAngle = atan2(pi.offsetOnField.y, pi.offsetOnField.x),
           ignoreAngle = atan2(150, iDist);
    for(int j = 0; j < playersPercept.numberOfRedPlayers + playersPercept.numberOfBluePlayers; ++j)
      if(i != j)
      {
        SinglePlayerPercept& pj = j < playersPercept.numberOfRedPlayers 
                            ? playersPercept.redPlayers[j]
                            : playersPercept.bluePlayers[j - playersPercept.numberOfRedPlayers];
        if(iDist < pj.offsetOnField.abs() &&
           fabs(atan2(pj.offsetOnField.y, pj.offsetOnField.x) - iAngle) < ignoreAngle)
        {
          if(j < playersPercept.numberOfRedPlayers)
            pj = playersPercept.redPlayers[--playersPercept.numberOfRedPlayers];
          else
            pj = playersPercept.bluePlayers[--playersPercept.numberOfBluePlayers];
          if(i > j)
          {
            i = j; // check pi again
            break;
          }
          else
            --j; // check pj again
        }
      }
  }

  // If there are too few line points of a certain type, remove them all, they may be misreadings
  for(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 VLCImageProcessor::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 VLCImageProcessor::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 VLCImageProcessor::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 VLCImageProcessor::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;
}

---