Modules/ImageProcessor/GT2004ImageProcessor/GT2004ImageProcessor.cpp

Go to the documentation of this file.

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

#include "GT2004ImageProcessor.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "Tools/FieldDimensions.h"
#include "Tools/Math/Geometry.h"
#include "GT2004ImageProcessorTools.h"

//~ #define EDGES
#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. */

GT2004ImageProcessor::GT2004ImageProcessor(const ImageProcessorInterfaces& interfaces)
: ImageProcessor(interfaces),
beaconDetector(image, cameraMatrix, prevCameraMatrix, imageInfo, colorTable, colorCorrector, landmarksPercept),
goalRecognizer(image, cameraMatrix, prevCameraMatrix, colorTable, colorCorrector, obstaclesPercept, landmarksPercept),
ballSpecialist(colorCorrector)
{
  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 GT2004ImageProcessor::execute()
{
  ColorCorrector::load();
  INIT_DEBUG_IMAGE(imageProcessorPlayers, image);

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

  // reset everything
  landmarksPercept.reset(image.frameNumber);
  linesPercept.reset(image.frameNumber);
  ballPercept.reset(image.frameNumber);
  playersPercept.reset(image.frameNumber);
  obstaclesPercept.reset(image.frameNumber);
#ifdef EDGES
  edgesPercept.reset(image.frameNumber);
  edgeSpecialist.reset();
#endif

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

  static bool prevCameraMatrixUninitialized = true;
  if (prevCameraMatrixUninitialized && cameraMatrix.frameNumber != 0)
  {
    prevCameraMatrix = cameraMatrix;
    prevCmTricot = cmTricot;
    prevCameraMatrixUninitialized = false;
  }

  longestBallRun = 0;

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

  const int visualizationScale = 20;
  Vector3<double> vectorInDirectionOfViewWorldOld;
  vectorInDirectionOfViewWorldOld = prevCameraMatrix.rotation * vectorInDirectionOfViewCamera;
  Vector3<double> cameraMotionVectorWorld = vectorInDirectionOfViewWorld - vectorInDirectionOfViewWorldOld;
  long frameNumber = cameraMatrix.frameNumber, 
    prevFrameNumber = prevCameraMatrix.frameNumber;
  const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
  double timeDiff = (frameNumber - prevFrameNumber) * r.motionCycleTime; // in seconds
  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 additional information about the image
  imageInfo.maxImageCoordinates.x = image.cameraInfo.resolutionWidth - 1;
  imageInfo.maxImageCoordinates.y = image.cameraInfo.resolutionHeight - 1;
  imageInfo.horizon = Geometry::calculateHorizon(cameraMatrix, image.cameraInfo);
  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;
  imageInfo.vertLine.normalizeDirection();

  // Scan through image
  scanColumns();
  scanRows();

#ifdef EDGES
  // get line-point percepts from specialist
  //edgeSpecialist.getEdgePointPercepts(linesPercept, cameraMatrix, prevCameraMatrix, image);
  // get edge percepts from specialist
  edgeSpecialist.getEdgesPercept(edgesPercept, cameraMatrix, prevCameraMatrix, image);
#endif

  // Analyze results
  if (longestBallRun > 2)
    ballSpecialist.searchBall(image, colorTable, cameraMatrix, prevCameraMatrix,
      ballCandidate.x, ballCandidate.y, ballPercept);
  filterPercepts();
  
  landmarksPercept.cameraOffset = cameraMatrix.translation;
  if(imageInfo.horizonInImage)
  {
    beaconDetector.execute();
  }
  goalRecognizer.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);
    colorTable.generateColorClassImage(correctedImage, segmentedImage1ColorClassImage);
  )
  SEND_DEBUG_COLOR_CLASS_IMAGE(segmentedImage1);

  GENERATE_DEBUG_IMAGE(imageProcessorGradients,
    SUSANEdgeDetectionLite edgeDetectionU(GT2004BeaconDetector::edgeThresholdU);
    SUSANEdgeDetectionLite edgeDetectionV(GT2004BeaconDetector::edgeThresholdV);
    Image edgeDetectionImage;
    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++) 
      {
        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 
        {
          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,"GT2004ImageProcessor: 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
  );*/

  prevCameraMatrix = 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));*/
}

void GT2004ImageProcessor::scanColumns()
{
  const int scanLineSpacing(4);
  const double verticalScanLineOffset(15.0);
               //verticalScanLineLength(50.0);
  Geometry::Line scanLine;
  Vector2<double> intersection;
  Vector2<int> topPoint,
               bottomPoint,
               topBorderPoint,
               bottomBorderPoint;
  double bottomBorderDist, topBorderDist;
  int i;
  bool noLines = false,
       skipLine;

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

  
  // scan lines are perpendicular to horizon
  scanLine.direction = imageInfo.vertLine.direction;

  // calculate and scan lines
  for(int dir = -1; dir <= 1; dir += 2)
  {
    for(i = 0; true; i++)
    {
      if (dir == 1 && i == 0) i = 1;
      scanLine.base.x = image.cameraInfo.resolutionWidth / 2 + imageInfo.horizon.direction.x * scanLineSpacing * dir * i;
      scanLine.base.y = image.cameraInfo.resolutionHeight / 2 + imageInfo.horizon.direction.y * scanLineSpacing * dir * i;

      //Does the scan line intersect with the image?
      if(!Geometry::getIntersectionPointsOfLineAndRectangle(
                                          Vector2<int>(0,0),
                                          imageInfo.maxImageCoordinates,
                                          scanLine, topBorderPoint, bottomBorderPoint))
      {
        break;
      }
      Geometry::getIntersectionOfLines(imageInfo.horizon, scanLine, intersection);

      if (fabs(scanLine.direction.y) > 0.001)
      {
        topBorderDist = (topBorderPoint.y - intersection.y) / scanLine.direction.y;
        bottomBorderDist = (bottomBorderPoint.y - intersection.y) / scanLine.direction.y;
      } else {
        topBorderDist = (topBorderPoint.x - intersection.x) / scanLine.direction.x;
        bottomBorderDist = (bottomBorderPoint.x - intersection.x) / scanLine.direction.x;
      }

      skipLine = false;

      // upper boundary for scan lines
      if (topBorderDist > -10)
        topPoint = topBorderPoint;
      else if (bottomBorderDist < -10)
        skipLine = true;
      else
      {
        topPoint.x = (int)(intersection.x - scanLine.direction.x * verticalScanLineOffset);
        topPoint.y = (int)(intersection.y - scanLine.direction.y * verticalScanLineOffset);
        if (topPoint.x < 0) topPoint.x = 0;
        if (topPoint.x >= image.cameraInfo.resolutionWidth) topPoint.x = image.cameraInfo.resolutionWidth - 1;
        if (topPoint.y < 0) topPoint.y = 0;
        if (topPoint.y >= image.cameraInfo.resolutionHeight) topPoint.y = image.cameraInfo.resolutionHeight - 1;
      }
        
      // lower boundaries for scan lines
      switch (i & 3)
      {
        case 1:
        case 3:
          if (bottomBorderDist < 30)
            bottomPoint = bottomBorderPoint;
          else if (topBorderDist > 30)
            skipLine = true;
          else
          {
            bottomPoint.x = (int)(intersection.x + scanLine.direction.x * 40);
            bottomPoint.y = (int)(intersection.y + scanLine.direction.y * 40);
            if (bottomPoint.x < 0) bottomPoint.x = 0;
            if (bottomPoint.x >= image.cameraInfo.resolutionWidth) bottomPoint.x = image.cameraInfo.resolutionWidth - 1;
            if (bottomPoint.y < 0) bottomPoint.y = 0;
            if (bottomPoint.y >= image.cameraInfo.resolutionHeight) bottomPoint.y = image.cameraInfo.resolutionHeight - 1;
          }
          noLines = true;
          break;
        case 2:
          if (bottomBorderDist < 60)
            bottomPoint = bottomBorderPoint;
          else if (topBorderDist > 60)
            skipLine = true;
          else
          {
            bottomPoint.x = (int)(intersection.x + scanLine.direction.x * 60);
            bottomPoint.y = (int)(intersection.y + scanLine.direction.y * 60);
            if (bottomPoint.x < 0) bottomPoint.x = 0;
            if (bottomPoint.x >= image.cameraInfo.resolutionWidth) bottomPoint.x = image.cameraInfo.resolutionWidth - 1;
            if (bottomPoint.y < 0) bottomPoint.y = 0;
            if (bottomPoint.y >= image.cameraInfo.resolutionHeight) bottomPoint.y = image.cameraInfo.resolutionHeight - 1;
          }
          noLines = true;
          break;
        case 0:
        default:
          bottomPoint = bottomBorderPoint;
          noLines = false;
          break;
      }
      if (!skipLine)
        scan(topPoint, bottomPoint, true, noLines);
    }
  }
  // finish clustering
  for(i = 0; i < 3; ++i)
  {
    clusterRobots(0,false,false);
  }
}

void GT2004ImageProcessor::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);
      else
        scan(startPoint, endPoint, false, false);
      scanLine.base += scanOffset;
    }
    else
    {
      return;
    }
  }
}

void GT2004ImageProcessor::scan(const Vector2<int>& start, const Vector2<int>& end,
                                bool vertical, bool noLines)
{
#ifdef EDGES
  edgeSpecialist.resetLine();
#endif

  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.numberOfPoints[i];
    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);
      colorClassBuffer.add(color);
      
#ifdef EDGES
      edgeSpecialist.checkPoint(actual, color, cameraMatrix, prevCameraMatrix, image);
#endif

      // 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;
    double upAngle = 0;
    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> point;
      double angle;
    };
    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);
      colorClassBuffer.add(color);
      
#ifdef EDGES
      edgeSpecialist.checkPoint(actual, color, cameraMatrix, prevCameraMatrix, image);
#endif

      // line point state machine
      const unsigned char* p3 = pixelBuffer[3];
      //UP: y-component increases ////////////////////////////////////////
      if(yBuffer[3] > yBuffer[4] + yThreshold)
      {
        up = p3;
        if(colorClassBuffer[6] == green)
        {
          Vector2<int> coords = getCoords(p3),
                       pointOnField; //position on field, relative to robot
          pixelBufferFromUp.init();
          if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, prevCameraMatrix, 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) // is there green 3 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, prevCameraMatrix, 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 edgeAngle = calcEdgeAngle(coords, pointOnField, scanAngle, pixelBuffer);
                Vector2<int> pointOnField2;
                if(edgeAngle != LinesPercept::UNDEF && pixelBufferFromUp.getNumberOfEntries() > 0 &&
                   Geometry::calculatePointOnField(upCoords.x, upCoords.y, cameraMatrix, prevCameraMatrix, image.cameraInfo, pointOnField2))
                {
                  upAngle = calcEdgeAngle(upCoords, pointOnField2, scanAngle, pixelBufferFromUp, true);
                  if(edgeAngle != LinesPercept::UNDEF && upAngle != LinesPercept::UNDEF)
                  {
                    // only add the closest field line. This makes filtering easier.
                    linesPercept.numberOfPoints[LinesPercept::field] = numberOfPoints[LinesPercept::field];
                    linesPercept.add(LinesPercept::field, pointOnField2, upAngle);
                    linesPercept.add(LinesPercept::field, pointOnField, edgeAngle);
                  }
                }
              }
              else if(height >= 3 && !borderFound)
              {
                switch(c)
                {
                  case skyblue:
                    if(vertical)
                    {
                      borderFound = true;
                      if(getPlayer().getTeamColor() == Player::red && !linesPercept.numberOfPoints[LinesPercept::skyblueGoal])
                        goalAtBorder = pointOnField.abs() < closestBottom;
                      closestBottom = 40000;
                      double edgeAngle = calcEdgeAngle(coords, pointOnField, scanAngle, pixelBuffer, false, 2);
                      if(edgeAngle != LinesPercept::UNDEF)
                        linesPercept.add(LinesPercept::skyblueGoal, pointOnField, edgeAngle);
                      typeOfLinesPercept = LinesPercept::skyblueGoal;
                      pixelsSinceGoal = 0;

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

                      lastRed = lastBlue = 0;
                      redCount = blueCount = 0; 
                    }
                    break;
                  case white:
                    if(height > lineHeight * 3 && (vertical || height > 30))
                    {
                      //borderFound = true;
                      double edgeAngle = calcEdgeAngle(coords, pointOnField, scanAngle, pixelBuffer);
                      if(edgeAngle != LinesPercept::UNDEF)
                      {
                        //linesPercept.add(LinesPercept::border,pointOnField, edgeAngle);
                        borderCandidate.found = true;
                        borderCandidate.point = pointOnField;
                        borderCandidate.angle = edgeAngle;
                      }
                      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;
          }
          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.point, borderCandidate.angle);
    }

    if (orangeCount > longestBallRun)
    {
      longestBallRun = orangeCount;
      ballCandidate = (getCoords(firstOrange) + getCoords(lastOrange)) / 2;
    }

    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.numberOfPoints[goal] == numberOfPoints[goal]) // no goal point found in this column
      {
        Vector2<int> coords = getCoords(pLast),
                     pointOnField;
        if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, prevCameraMatrix, 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.numberOfPoints[i] = 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);
            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, prevCameraMatrix, image.cameraInfo, pointOnField))
          {
            Vector2<int> redCoords = getCoords(lastRed);
            if((redCoords - coords).abs() < Geometry::getSizeByDistance(image.cameraInfo, 100, pointOnField.abs()))
            {
              linesPercept.add(LinesPercept::redRobot,pointOnField);
              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.numberOfPoints[i] = 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);
            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, prevCameraMatrix, image.cameraInfo, pointOnField))
          {
            Vector2<int> blueCoords = getCoords(lastBlue);
            if((blueCoords - coords).abs() < Geometry::getSizeByDistance(image.cameraInfo, 100, pointOnField.abs()))
            {
              linesPercept.add(LinesPercept::blueRobot,pointOnField);
              typeOfLinesPercept = LinesPercept::blueRobot;
              blueFound = true;
            }
          }
        }
      }
      // cluster the robot points found
      clusterRobots(pLast, redFound, blueFound);

      // obstacles found?
      bool addObstaclesPoint = false;
      Vector2<int> newObstaclesNearPoint;
      Vector2<int> newObstaclesFarPoint;
      bool newObstaclesFarPointIsOnImageBorder = false;
      if(count > 90 && cameraMatrix.isValid)
      {
        Vector2<int> firstGroundOnField;
        bool firstGroundOnFieldIsOnField = 
          Geometry::calculatePointOnField(firstGround.x, firstGround.y, cameraMatrix, prevCameraMatrix, image.cameraInfo, firstGroundOnField);

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

        if(groundState == foundBeginOfGround)
        {
          if(firstGroundOnFieldIsOnField)
          {
            addObstaclesPoint = true;
            newObstaclesNearPoint = imageBottomOnField;
            newObstaclesFarPoint = firstGroundOnField;
            newObstaclesFarPointIsOnImageBorder = beginOfGroundIsAtTheTopOfTheImage;
          }
        }
        else if(groundState == foundEndOfGround)
        {
          int lineHeight = Geometry::calculateLineSize(imageBottom, cameraMatrix, image.cameraInfo);
          if(numberOfPixelsSinceLastGround < lineHeight * 4)
          {
            if(firstGroundOnFieldIsOnField)
            {
              addObstaclesPoint = true;
              newObstaclesNearPoint = imageBottomOnField;
              newObstaclesFarPoint = firstGroundOnField;
              newObstaclesFarPointIsOnImageBorder = beginOfGroundIsAtTheTopOfTheImage;
            }
          }
          else if(imageBottomIsOnField)
          {
            addObstaclesPoint = true;
            newObstaclesNearPoint = imageBottomOnField;
            newObstaclesFarPoint = imageBottomOnField;
          }
        }
        else if(groundState == noGroundFound)
        {
          if(imageBottomIsOnField &&
            // the carpet often is not green under the robot
            imageBottomOnField.abs() > 150)
          {
            addObstaclesPoint = true;
            newObstaclesNearPoint = imageBottomOnField;
            newObstaclesFarPoint = imageBottomOnField;
          }
        }
      }// if(count > 90 && cameraMatrix.isValid)
      if(addObstaclesPoint)
      {
        if(
          angleBetweenDirectionOfViewAndGround > -80.0 &&
          !(newObstaclesFarPoint.x == 0 && newObstaclesFarPoint.y == 0)
          )

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

void GT2004ImageProcessor::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][linesPercept.numberOfPoints[LinesPercept::redRobot] - 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, prevCameraMatrix, image.cameraInfo, pointOnField) &&
          closestRed.abs() > pointOnField.abs()))
  {
    if(++noRedCount == 3 && (firstRed != lastRed || noBlueCount > 4))
    {
      SinglePlayerPercept percept;
      percept.direction = (atan2((double)firstRed.y,(double)firstRed.x) + atan2((double)lastRed.y,(double)lastRed.x)) / 2;
      double distance = closestRed.abs();
      percept.offset.x = cos(percept.direction) * distance;
      percept.offset.y = sin(percept.direction) * distance;
      percept.validity = 1;
      playersPercept.addRedPlayer(percept);
    }
  }

  if(blueFound)
  {
    lastBlue = 
      linesPercept.points[LinesPercept::blueRobot][linesPercept.numberOfPoints[LinesPercept::blueRobot] - 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, prevCameraMatrix, image.cameraInfo, pointOnField) &&
          closestBlue.abs() > pointOnField.abs()))
  {
    if(++noBlueCount == 3 && (firstBlue != lastBlue || noRedCount2 > 4))
    {
      SinglePlayerPercept percept;
      percept.direction = (atan2((double)firstBlue.y,(double)firstBlue.x) + atan2((double)lastBlue.y,(double)lastBlue.x)) / 2;
      double distance = closestBlue.abs();
      percept.offset.x = int(cos(percept.direction) * distance);
      percept.offset.y = int(sin(percept.direction) * distance);
      percept.validity = 1;
      playersPercept.addBluePlayer(percept);
    }
  }
}
 
void GT2004ImageProcessor::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.offset.abs(),
           iAngle = atan2(pi.offset.y, pi.offset.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.offset.abs() &&
           fabs(atan2(pj.offset.y, pj.offset.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.numberOfPoints[i] < 3)
      linesPercept.numberOfPoints[i] = 0;

  // Remove outliers in the border
  filterLinesPercept(linesPercept, LinesPercept::border, cameraMatrix, prevCameraMatrix, 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, cameraMatrix, prevCameraMatrix, image);
  /*
  // 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;
  */
}

double GT2004ImageProcessor::calcEdgeAngle(
  const Vector2<int>& pointInImage, 
  const Vector2<int>& pointOnField,
  double scanAngle,
  const RingBuffer<const unsigned char*,7>& pixelBuffer,
  const bool againstScanline,
  int channel) const
{
  // 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)
      return LinesPercept::UNDEF;

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

    Vector2<int> point;
    if(Geometry::calculatePointOnField(endPoint.x, endPoint.y, cameraMatrix, prevCameraMatrix, image.cameraInfo, point))
      return normalize((point - pointOnField).angle() + pi_2);
  }
  return LinesPercept::UNDEF;
}

void GT2004ImageProcessor::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 GT2004ImageProcessor::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 GT2004ImageProcessor::filterLinesPercept(LinesPercept& percept, int type, const CameraMatrix& cameraMatrix, const CameraMatrix& prevCameraMatrix, 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.numberOfPoints[type]; i++)
  {
    Vector2<double> dir1 = Vector2<double>(cos(percept.points[type][i].angle),sin(percept.points[type][i].angle));

    //int lineHeightI = Geometry::calculateLineSize(percept.points[type][i], cameraMatrix, image.cameraInfo);
    for(j = i + 1; j < percept.numberOfPoints[type]; 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][i].angle - percept.points[type][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][j] - percept.points[type][i]).x,(double)(percept.points[type][j] - percept.points[type][i]).y).normalize();
        double distance1 = fabs(v1 * dir1);
        if(distance1 < maxDist)
        {
          Vector2<double> dir2 = Vector2<double>(cos(percept.points[type][j].angle),sin(percept.points[type][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.numberOfPoints[type]; i++)
  {
    int countSim = 0;
    //if(edgePoints[i].belongsToLineNo == -1) // only if point is not yet matched to an edge
    for (j = 0; j < percept.numberOfPoints[type]; 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][nextFree++] = percept.points[type][i];
    }
  }
  percept.numberOfPoints[type] = nextFree;
}

/*
* $Log: GT2004ImageProcessor.cpp,v $
* Revision 1.30  2004/06/30 17:36:58  thomas
* modified obstacle-states to handle white robots while allowing field-lines
*
* Revision 1.29  2004/06/30 09:27:50  nistico
* *** empty log message ***
*
* Revision 1.28  2004/06/29 17:11:27  thomas
* modified to make border-percept better if near border and seeing outside-green and white-wall
*
* Revision 1.27  2004/06/29 10:09:12  thomas
* added new algorithm for filtering border- and field-line-percepts
*
* Revision 1.26  2004/06/28 09:48:14  roefer
* SelfLocator
*
* Revision 1.25  2004/06/22 09:43:17  nistico
* The radial component of the measurement variance is
* also affected by panning velocity now
*
* Revision 1.24  2004/06/22 08:24:38  nistico
* Reintroduced clipping between [-1; 1] before acos()
*
* Revision 1.23  2004/06/21 08:15:57  nistico
* The dot product of normalized vectors (versors) is guaranteed to be within [-1, 1] ;
* the angle was not normalized, however. (-pi, pi)
*
* Revision 1.22  2004/06/20 21:55:36  roefer
* Bug fixed in line point filtering
*
* Revision 1.21  2004/06/19 19:06:31  roefer
* Filtering field lines and border points that might be parts of a robot
*
* Revision 1.20  2004/06/18 12:29:17  nistico
* Added (redundant?) check for division_by_zero
*
* Revision 1.19  2004/06/18 10:24:51  nistico
* Kalman measurement covariance matrix now adjusted also based on
* head panning angular velocity
*
* Revision 1.18  2004/06/18 10:19:15  roefer
* Bug removed
*
* Revision 1.17  2004/06/17 21:13:20  thomas
* modified gradient of line-percepts now have accurate direction
*
* Revision 1.16  2004/06/17 15:52:05  nistico
* Added visualization of camera motion vector on image
* Fixed cameraMatrix.frameNumber=0 problem when playing logfiles
* (the image frame number is copied)
*
* Revision 1.15  2004/06/17 13:34:02  risler
* warning removed
*
* Revision 1.14  2004/06/17 10:50:22  roefer
* Visualization of line orientation fixed
*
* Revision 1.13  2004/06/17 09:24:43  roefer
* Lines percept includes line orientation
*
* Revision 1.12  2004/06/16 18:58:39  roefer
* LinesPercept with angles, does not work yet
*
* Revision 1.11  2004/06/16 13:39:14  thomas
* update edge-specialist
*
* Revision 1.10  2004/06/16 10:50:24  risler
* added border obstacle type
*
* Revision 1.9  2004/06/15 15:40:57  roefer
* edge detection commented out
*
* Revision 1.8  2004/06/15 10:58:26  thomas
* added edge-specialist, edges-percept, debug-drawings etc. (not yet called from image-processor)
*
* Revision 1.7  2004/06/14 23:20:10  spranger
* -changed some functions in Geometry from int to double including ballradius(fieldimensions)
* -maybe all Geometric-functions in Geometry should be as precise as possible, avoiding int to double conversion errors
*
* Revision 1.6  2004/06/13 11:29:35  nistico
* Improved visualization of edges used by BeaconDetector
*
* Revision 1.5  2004/06/05 19:48:45  nistico
* Added one more special situation to BeaconDetector
* imageProcessorGradients now visualized edges used by
* BeaconDetector, for debugging
*
* Revision 1.4  2004/06/05 07:58:21  roefer
* Compensation for motion distortions of images
*
* Revision 1.3  2004/06/01 12:00:33  nistico
* - used Tim's Bresenham for scanForBeaconPart
* - changed edge detection
* - still work in progress
*
* Revision 1.2  2004/05/22 20:44:49  juengel
* Removed manual calibration.
*
* Revision 1.1.1.1  2004/05/22 17:19:46  cvsadm
* created new repository GT2004_WM
*
* Revision 1.4  2004/05/07 15:16:24  nistico
* All geometry calculations are making use of intrinsic functions.
* I updated most of the images processors to make correct use of this.
* Please test if there are any problems, because i'm going to remove the
* old code soon.
*
* Revision 1.3  2004/05/06 16:03:56  nistico
* Supports ColorTable32K through CT32K_LAYOUT switch located into
* GT2004ImageProcessorTools.h
*
* Revision 1.2  2004/05/05 13:27:38  tim
* removed unnecessary lines of code
*
* Revision 1.1  2004/05/04 13:40:19  tim
* added GT2004ImageProcessor
*
*/

---