Modules/ImageProcessor/SlamImageProcessor/SlamImageProcessor.cpp

Go to the documentation of this file.

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

#include "SlamImageProcessor.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "Tools/FieldDimensions.h"
#include "Tools/Math/Geometry.h"
#include "SlamImageProcessorTools.h"
#include "Representations/Perception/JPEGImage.h"

#include "Representations/Perception/SlamPercept.h"

#define slamPercept (*(SlamPercept *)specialPercept.slamData)

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

SlamImageProcessor::SlamImageProcessor(const ImageProcessorInterfaces& interfaces)
: ImageProcessor(interfaces),
beaconDetector(image, cameraMatrix, imageInfo.previousCameraMatrix, imageInfo, colorTable, colorCorrector, robotPose, landmarksPercept, specialPercept),
goalRecognizer(image, cameraMatrix, imageInfo.previousCameraMatrix, colorTable, colorCorrector, obstaclesPercept, landmarksPercept),
ballSpecialist(colorCorrector),
ballClustering(),
lineFinder(colorCorrector, colorTable),
borderFinder(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)
  );
  prevCameraMatrixUninitialized = true;

  count = 0;

  filterWeight = 0.975;

  for (int i = 0; i < NUM_PINK; i++)
  {
    pinkCaracteristic[i].count = 0;
    pinkCaracteristic[i].pixel = 0;
  };
}

void SlamImageProcessor::execute()
{

  INIT_DEBUG_IMAGE(imageProcessorPlayers, image);

  NDECLARE_DEBUGDRAWING ("SlamIMP:maxGreenImage", "drawingOnImage", "shows maximal green point");
  NDECLARE_DEBUGDRAWING ("SlamIMP:horizon", "drawingOnImage", "shows horizon");

  NDECLARE_DEBUGDRAWING ("SlamIMP:maxGreenField", "drawingOnField", "shows maximal green point");
  NDECLARE_DEBUGDRAWING ("SlamIMP:borderPoints", "drawingOnImage", "shows the border points");

  NRECTANGLE ("SlamIMP:maxGreenImage", 0, 0, 10, 10, 1, Drawings::ps_solid, Drawings::red);
  NRECTANGLE ("SlamIMP:maxGreenField", -100, -100, 100, 100, 50, Drawings::ps_solid, Drawings::red);
   

  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);
//  circleFinder.reset();
  lineFinder.reset();
  borderFinder.reset();

  slamPercept.greenLocation.impLengthOfMaxGreen = 0.0;
  slamPercept.greenLocation.impAngleToMaxGreen = 0;
  slamPercept.greenLocation.impAngleToMaxGreenValid = false;

  averageY = 0;
  averageU = 0;
  averageV = 0;

  averageCount = 0;

  slamPercept.imageCharacteristic.numPink = 0;
  slamPercept.imageCharacteristic.angle = cameraMatrix.rotation.getZAngle ();

#ifdef MULTIPLE_BALL_CANDIDATES
    ballClustering.reset();
#endif

#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

  if (prevCameraMatrixUninitialized && cameraMatrix.frameNumber != 0)
  {
    imageInfo.previousCameraMatrix = 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 = 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
  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;
  NARROW("SlamIMP: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();

  averageY /= averageCount;
  averageU /= averageCount;
  averageV /= averageCount;
  
  slamPercept.imageCharacteristic.averageY = averageY;
  slamPercept.imageCharacteristic.averageU = averageU;
  slamPercept.imageCharacteristic.averageV = averageV;
  
  slamPercept.imageCharacteristic.filteredAverageY *= filterWeight;
  slamPercept.imageCharacteristic.filteredAverageY += (1.0 - filterWeight) * averageY;
  slamPercept.imageCharacteristic.filteredAverageU *= filterWeight;
  slamPercept.imageCharacteristic.filteredAverageU += (1.0 - filterWeight) * averageU;
  slamPercept.imageCharacteristic.filteredAverageV *= filterWeight;
  slamPercept.imageCharacteristic.filteredAverageV += (1.0 - filterWeight) * averageV;

  MODIFY ("slamIMP:filteredY", slamPercept.imageCharacteristic.filteredAverageY);
  MODIFY ("slamIMP:filteredU", slamPercept.imageCharacteristic.filteredAverageU);
  MODIFY ("slamIMP:filteredV", slamPercept.imageCharacteristic.filteredAverageV);

  MODIFY ("slamIMP:filterWeight", filterWeight);

  if (slamPercept.imageCharacteristic.averageY >= 70)
    slamPercept.viewAngle = 0;
  else
    slamPercept.viewAngle = 180;

  slamPercept.viewAngleValid = true;

  if (slamPercept.greenLocation.impAngleToMaxGreenValid)
  {
    NCIRCLE ("SlamIMP:maxGreenImage", slamPercept.greenLocation.maxGreenPointInImage.x, slamPercept.greenLocation.maxGreenPointInImage.y, 10, 3, Drawings::ps_solid, Drawings::green);

    NCIRCLE ("SlamIMP:maxGreenField", slamPercept.greenLocation.maxGreenPointOnField.x, slamPercept.greenLocation.maxGreenPointOnField.y, 10, 3, Drawings::ps_solid, Drawings::blue);
  };

//  circleFinder.determineCirclePoint();
  Vector2<double> circleMidPoint;
  lineFinder.execute(linesPercept, cameraMatrix, image, imageInfo, robotPose, false /*circleFinder.getCircle(circleMidPoint)*/, Vector2<double>(0,0)/*circleMidPoint*/);
  borderFinder.execute(linesPercept, cameraMatrix, image, imageInfo, robotPose);

  
  
  DEBUG_DRAWING_FINISHED(circlePoints_image); 
  DEBUG_DRAWING_FINISHED(circlePoints_Field); 
  

#ifdef EDGES
  // 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);
#endif
  
  // clears the multiple ball percept list
  ballSpecialist.resetMultiplePerceptsList(ballPercept);


  // Analyze results
#ifdef MULTIPLE_BALL_CANDIDATES
  if (longestBallRun > 2)
  {
    int numberOfCandidates = ballClustering.getSize();
    for (int i=0; i<numberOfCandidates; i++)
    {
      Vector2<double> temp = ballClustering.getMidpoint(i);
      ballCandidate.x = (int)temp.x;
        ballCandidate.y = (int)temp.y;
        DOT(imageProcessor_ball3, ballCandidate.x, ballCandidate.y, Drawings::black, Drawings::orange);
      ballSpecialist.searchBall(image, colorTable, cameraMatrix, imageInfo.previousCameraMatrix,
                                        ballCandidate.x, ballCandidate.y, ballPercept);

      //CRASH_CHECK(i);
    }
    DEBUG_DRAWING_FINISHED(imageProcessor_multipleBallPercepts);
    DEBUG_DRAWING_FINISHED(multipleBallPerceptsField);
  }
#else
  if (longestBallRun > 2)
  {
    ballSpecialist.searchBall(image, colorTable, cameraMatrix, imageInfo.previousCameraMatrix,
                                    ballCandidate.x, ballCandidate.y, ballPercept);
  }
#endif

  ballSpecialist.forwardPercept(ballPercept);

  // is not needed any more, because fieldLine-percepts now come from recognized lines
  // -- reverted --
  filterPercepts();
  
  landmarksPercept.cameraOffset = cameraMatrix.translation;
  if(imageInfo.horizonInImage)
  {
    beaconDetector.execute();
  }

  slamPercept.imageCharacteristic.numPink = 0;

  if (slamPercept.slamState == SlamPercept::LearnNewFlags)
  goalRecognizer.execute();

  // Drawing stuff
  if(imageInfo.horizonInImage)
  {
    NLINE("SlamIMP: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);

  DEBUG_DRAWING_FINISHED(ballLocatorField);
  DEBUG_DRAWING_FINISHED(percepts_robot);

  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(SlamBeaconDetector::edgeThresholdU);
    SUSANEdgeDetectionLite edgeDetectionV(SlamBeaconDetector::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,"SlamImageProcessor: percept-counting " << endl
    << "\tflags: " << landmarksPercept.numberOfFlags << endl
    << "\tgoals: " << landmarksPercept.numberOfGoals << endl
    << "\tlines: " << linesPercept.numberOfPoints[1] << endl
    << "\tballs: " << ballPercept.ballWasSeen << endl
    << "\tblue:  " << playersPercept.numberOfBluePlayers << endl
    << "\tred:   " << playersPercept.numberOfRedPlayers << endl
    << "\tobst:  " << obstaclesPercept.numberOfPoints
  );*/

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

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

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

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

  N_GENERATE_DEBUG_IMAGE(edgeDetection,
    SUSANEdgeDetectionLite edgeDetectionU(SlamBeaconDetector::edgeThresholdU);
    SUSANEdgeDetectionLite edgeDetectionV(SlamBeaconDetector::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 SlamImageProcessor::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 SlamImageProcessor::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 SlamImageProcessor::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.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;

    /*
    * Slam
    */
    bool startOfGreenFound = false;
    Vector2<int> startOfGreen;

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

    if 
      (
        (actual.x > imageInfo.maxImageCoordinates.x / 2 - 10)
        && (actual.x < imageInfo.maxImageCoordinates.x / 2 + 10)
      )
    {
    averageY += y;
    averageU += u;
    averageV += v;

    averageCount++;
    };      
#ifdef EDGES
      edgeSpecialist.checkPoint(actual, color, cameraMatrix, imageInfo.previousCameraMatrix, 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;
    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);
      colorClassBuffer.add(color);
      
#ifdef EDGES
      edgeSpecialist.checkPoint(actual, color, cameraMatrix, imageInfo.previousCameraMatrix, image);
#endif

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

      if(c3 == gray && colorClassBuffer[4] == gray && colorClassBuffer[5] && colorClassBuffer[6] == gray)
        {
          if(colorClassBuffer[2] == green && colorClassBuffer[1] == green && colorClassBuffer[0] == green)
          {
            NDOT("SlamIMP:borderPoints",actual.x,actual.y,Drawings::blue,Drawings::blue)
            
            // find maximum gradient around point on the scan line
            int indices[] = {3, 4, 2, 5, 1};
            Vector2<double> maxGrad;
            double maxLength = -1;
            int channel = 0;
            bool againstScanline = false;
            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;
                }
              }
             }//for(unsigned int i = 0; i < sizeof(indices) / sizeof(indices[0]); ++i)
            if(maxLength > 0)
            {
              Vector2<double> gradRotated = (rotation2x2 * Vector2<double>(maxGrad.x, -maxGrad.y)).normalize();
              gradRotated.y *= -1;
              Vector2<int> endPoint(int(actual.x + gradRotated.x * 10), int(actual.y + gradRotated.y * 10));
              borderFinder.considerLinePoint(actual,maxGrad);
              NARROW("SlamIMP:borderPoints",actual.x,actual.y,endPoint.x,endPoint.y,1,1,Drawings::red);
            }
          }//if(colorClassBuffer[4] == green && colorClassBuffer[5] == green && colorClassBuffer[6] == green)
        }//if(c3 == gray && colorClassBuffer[2] == gray && colorClassBuffer[1] && colorClassBuffer[0] == gray)


      ++noOrangeCount;
      if (c3 == orange)
      {
        if(noOrangeCount > 1)
        {
          if (orangeCount > longestBallRun)
          {
            longestBallRun = orangeCount;
            ballCandidate = (getCoords(firstOrange) + getCoords(lastOrange)) / 2;
          }
#ifdef MULTIPLE_BALL_CANDIDATES
          if (orangeCount>2)
          {
            ballClustering.addLine(getCoords(firstOrange), getCoords(lastOrange), orangeCount);
          }
#endif
          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;

            if ((greenCount > 5) && (!startOfGreenFound))
            {
              startOfGreenFound = true;

              startOfGreen = getCoords (firstGreen);
            };

            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;

//        if (pinkCount > 10)
        if (pinkCount > 4)
        {
          slamPercept.imageCharacteristic.numPink = 1;
/*          slamPercept.viewAngle = 0 - cameraMatrix.rotation.getZAngle ();
          slamPercept.viewAngleValid = true;*/
        }
            }
            lastPink = p3;
            noPinkCount = 0;
            break;
          case yellow:
            if(noYellowCount > 6)
              yellowCount = 1;
            else
      {
              ++yellowCount;

/*        if (yellowCount > 10)
        {
          specialPercept.viewAngle = -pi - cameraMatrix.rotation.getZAngle ();
          specialPercept.viewAngleValid = true;
        }*/
      }
            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;
    }
#ifdef MULTIPLE_BALL_CANDIDATES
    if (orangeCount>2)
    {
      ballClustering.addLine(getCoords(firstOrange), getCoords(lastOrange), orangeCount);
    }
#endif

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

        /*
        * Slam
        */

        if (firstGroundOnFieldIsOnField)
        {
          double distance = firstGroundOnField.abs ();

          if (distance > slamPercept.greenLocation.lengthOfMaxGreen)
          {
            slamPercept.greenLocation.maxGreenPointInImage = firstGround;
            slamPercept.greenLocation.maxGreenPointOnField.x = (int) firstGroundOnField.x;
            slamPercept.greenLocation.maxGreenPointOnField.y = (int) firstGroundOnField.y;

            slamPercept.greenLocation.impLengthOfMaxGreen = distance;
            slamPercept.greenLocation.impAngleToMaxGreen = firstGroundOnField.angle ();
            slamPercept.greenLocation.impAngleToMaxGreenValid = true;
          };
        };

        /*
        * /Slam
        */

        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)

/*
    if (startOfGreenFound)
    {
      Vector2<double> pointOnField;

      Geometry::calculatePointOnField (startOfGreen.x, startOfGreen.y, cameraMatrix, image.cameraInfo, pointOnField);

      double distance = pointOnField.abs ();

      if (distance > slamPercept.greenLocation.lengthOfMaxGreen)
      {
        slamPercept.greenLocation.maxGreenPointInImage = startOfGreen;
        slamPercept.greenLocation.maxGreenPointOnField.x = (int) pointOnField.x;
        slamPercept.greenLocation.maxGreenPointOnField.y = (int) pointOnField.y;

        slamPercept.greenLocation.impLengthOfMaxGreen = distance;
        slamPercept.greenLocation.impAngleToMaxGreen = pointOnField.angle ();
        slamPercept.greenLocation.impAngleToMaxGreenValid = true;
      };
    };*/
  }
}

void SlamImageProcessor::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 SlamImageProcessor::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, cameraMatrix, imageInfo.previousCameraMatrix, 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, imageInfo.previousCameraMatrix, 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;
  */
}

bool SlamImageProcessor::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 SlamImageProcessor::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 SlamImageProcessor::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 SlamImageProcessor::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.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;
}

---