Modules/ImageProcessor/GT2005ImageProcessor/GT2005PlayerSpecialist.cpp

Go to the documentation of this file.

/**
* @file GT2005FlagSpecialist.cpp
*
* This file contains a class for Image Processing.
* @author <A href=mailto:marc.dassler@web.de>Marc Dassler</A>
*/

#include "Tools/FieldDimensions.h"
#include "Tools/Player.h"
#include "Tools/Math/Common.h"
#include "Modules/ImageProcessor/ImageProcessorTools/ColorCorrector.h"
#include "GT2005PlayerSpecialist.h"
#include "GT2005ImageProcessorTools.h"


GT2005PlayerSpecialist::GT2005PlayerSpecialist(
  const ColorCorrector& colorCorrector,
  const ColorTable& colorTable,
  const Image& image,
  const ImageInfo& imageInfo,
  const LinesPercept& linesPercept,
  const CameraMatrix& cameraMatrix,
  PlayersPercept& playersPercept):
  colorCorrector(colorCorrector),
  colorTable(colorTable),
  image(image),
  imageInfo(imageInfo),
  linesPercept(linesPercept),
  cameraMatrix(cameraMatrix),
  playersPercept(playersPercept)
{
    maxDepth = 0;
}


void GT2005PlayerSpecialist::execute()
{
      clusterRobotLinesPercepts();
      for (int c=0;c<clusteredRedRobots;c++)
      {
        if (averageRedRobotSignaturePoints[c]>1)
        {
          LINE(imageProcessor_robot_detection,
            averageRedRobotSignatureStart[c].x, averageRedRobotSignature[c].y,
            averageRedRobotSignatureStop[c].x, averageRedRobotSignature[c].y, 
            1, Drawings::ps_solid, Drawings::red );
      
          LINE(imageProcessor_robot_detection,
            averageRedRobotSignatureStart[c].x, averageRedRobotSignatureStart[c].y,
            averageRedRobotSignatureStop[c].x, averageRedRobotSignatureStop[c].y, 
            1, Drawings::ps_solid, Drawings::yellow );
          
          NLINE("imageProcessor:robot detection",
            averageRedRobotSignatureStart[c].x, averageRedRobotSignature[c].y,
            averageRedRobotSignatureStop[c].x, averageRedRobotSignature[c].y, 
            1, Drawings::ps_solid, Drawings::red );
      
          NLINE("imageProcessor:robot detection",
            averageRedRobotSignatureStart[c].x, averageRedRobotSignatureStart[c].y,
            averageRedRobotSignatureStop[c].x, averageRedRobotSignatureStop[c].y, 
            1, Drawings::ps_solid, Drawings::yellow );
          
          detectRedRobots(averageRedRobotSignatureStart[c],averageRedRobotSignatureStop[c],averageRedRobotSignature[c]);
        }
      }
      for (int c=0;c<clusteredBlueRobots;c++)
      {
        if (averageBlueRobotSignaturePoints[c]>1)
        {
          LINE(imageProcessor_robot_detection,
            averageBlueRobotSignatureStart[c].x, averageBlueRobotSignature[c].y,
            averageBlueRobotSignatureStop[c].x, averageBlueRobotSignature[c].y, 
            1, Drawings::ps_solid, Drawings::blue );
      
          LINE(imageProcessor_robot_detection,
            averageBlueRobotSignatureStart[c].x, averageBlueRobotSignatureStart[c].y,
            averageBlueRobotSignatureStop[c].x, averageBlueRobotSignatureStop[c].y, 
            1, Drawings::ps_solid, Drawings::yellow );

          NLINE("imageProcessor:robot detection",
            averageBlueRobotSignatureStart[c].x, averageBlueRobotSignature[c].y,
            averageBlueRobotSignatureStop[c].x, averageBlueRobotSignature[c].y, 
            1, Drawings::ps_solid, Drawings::blue );
      
          NLINE("imageProcessor:robot detection",
            averageBlueRobotSignatureStart[c].x, averageBlueRobotSignatureStart[c].y,
            averageBlueRobotSignatureStop[c].x, averageBlueRobotSignatureStop[c].y, 
            1, Drawings::ps_solid, Drawings::yellow );

          
          detectBlueRobots(averageBlueRobotSignatureStart[c],averageBlueRobotSignatureStop[c],averageBlueRobotSignature[c]);
        }
      }


}

void GT2005PlayerSpecialist::detectRedRobots(
     Vector2<int> averageRobotSignatureStart,
     Vector2<int> averageRobotSignatureStop,
     Vector2<int> averageRobotSignature)
{
  // walk the averageRobotSignature and search for white - robotColor - white
  // or search left and right side of the averageRobotSignature

  Vector2<int> firstWhite(1000,0),firstRed(1000,0), lastRed(-1000,0), lastWhite(-1000,0),runner; 
  if (averageRobotSignatureStart != averageRobotSignatureStop)
  {
    BresenhamLineScan avgRobotSigBresenham (averageRobotSignatureStart, averageRobotSignatureStop);
    runner = averageRobotSignatureStart;
    while(runner != averageRobotSignatureStop)
    {
       if ( runner.x >= imageInfo.maxImageCoordinates.x
        || runner.x < 0
        || runner.y >= imageInfo.maxImageCoordinates.y
        || runner.y < 0) 
      {
        OUTPUT(idText, text, "detectRedRobots: Bresenham left image");
        break;
      }
      colorClass inspect = CORRECTED_COLOR_CLASS(runner.x,runner.y,
          image.image[runner.y][0][runner.x],
          image.image[runner.y][1][runner.x],
          image.image[runner.y][2][runner.x],
          colorTable, colorCorrector);


      // TODO: one pixel is not enough to decide this
      if (inspect == white && firstWhite.x == 1000)
            firstWhite = runner;
      if (inspect == red && firstRed.x == 1000)
            firstRed = runner;
      if (inspect == red && lastRed.x < runner.x)
            lastRed = runner;
      if (inspect == white && runner.x > firstWhite.x)
            lastWhite = runner;
       
      avgRobotSigBresenham.getNext(runner);
    }
  }        // fi (averageRobotSignatureStart != averageRobotSignatureStop)
  else
  {
    firstWhite = averageRobotSignatureStart;
  }

  Vector2<unsigned char> cfirstWhite,cfirstRed, clastRed, clastWhite;

  cfirstWhite.x = (char)firstWhite.x;
  cfirstWhite.y = (char)firstWhite.y;

  clastWhite.y = (char)lastWhite.y;
  clastWhite.x = (char)lastWhite.x;

  cfirstRed.y = (char)firstRed.y;
  cfirstRed.x = (char)firstRed.x;

  clastRed.y = (char)lastRed.y;
  clastRed.x = (char)lastRed.x;


  // TODO: only the red playercolor were found
  
  //now take the midpoint of the white parts and start search green below
  double proportion = (averageRobotSignatureStart-averageRobotSignatureStop).abs();
  
  
  Vector2<double> dir;
  dir.x = averageRobotSignatureStop.x-averageRobotSignatureStart.x;
  dir.y = averageRobotSignatureStop.y-averageRobotSignatureStart.y;
  Geometry::Line averageLine = Geometry::Line(averageRobotSignatureStart, dir);
  // if average line angle bigger than 15 degree, we probably observe the roboter at the side
  if (sin(dir.y/proportion) > pi/12)
    proportion *= 11.0/17.0; // robot height to robot length observed at side
  else
    proportion *= 1.0; // robot height to robot width observed at front
  int depth = (int) proportion;
  Vector2<unsigned char> point,lower(0,0);
  double distance;
  if (firstWhite.x != 1000)
  {
    point = findRedFootPointYAxis(cfirstWhite,Vector2<char>(0,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    point = findRedFootPointYAxis(cfirstWhite,Vector2<char>(1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    point = findRedFootPointYAxis(cfirstWhite,Vector2<char>(-1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
  }
  if (lastWhite.x != -1000)
  {      
    point = findRedFootPointYAxis(clastWhite,Vector2<char>(0,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    point = findRedFootPointYAxis(clastWhite,Vector2<char>(1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    point = findRedFootPointYAxis(clastWhite,Vector2<char>(-1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
  }
  if (lower.x!=0)
  {
    Vector2<double> dlower;dlower.x = lower.x;dlower.y = lower.y;
    distance = fabs(Geometry::getDistanceToLine(averageLine,dlower));
  }
  else 
    distance = 0;
  if (firstRed.x != 1000 && proportion-distance>0)
  {
    point = findRedFootPointYAxis(cfirstRed,Vector2<char>(0,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    point = findRedFootPointYAxis(cfirstRed,Vector2<char>(1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    point = findRedFootPointYAxis(cfirstRed,Vector2<char>(-1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
  }
  if (lower.x!=0)
  {
    Vector2<double> dlower;dlower.x = lower.x;dlower.y = lower.y;
    distance = fabs(Geometry::getDistanceToLine(averageLine,dlower));
  }
  else 
    distance = 0;
  if (lastRed.x != -1000 && proportion-distance>0)
  {
    point = findRedFootPointYAxis(clastRed,Vector2<char>(0,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    point = findRedFootPointYAxis(clastRed,Vector2<char>(1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    point = findRedFootPointYAxis(clastRed,Vector2<char>(-1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
  }

  // create percept
  Vector2<double> pointOnField;
  SinglePlayerPercept percept;
  Vector2<int> robotCenter((averageRobotSignatureStop.x-averageRobotSignatureStart.x)/2+averageRobotSignatureStart.x,lower.y);

  // now take the middle of the averageRobotXLine and the y of the lowest point
  if (Geometry::calculatePointOnField(robotCenter.x, robotCenter.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, percept.offsetOnField))
  {
    CIRCLE(imageProcessor_robot_detection, lower.x, lower.y,10,1, Drawings::ps_solid, Drawings::yellow);
    CIRCLE(imageProcessor_robot_detection, robotCenter.x, robotCenter.y,10,1, Drawings::ps_solid, Drawings::red);
    NCIRCLE("imageProcessor:robot detection", lower.x, lower.y,10,1, Drawings::ps_solid, Drawings::yellow);
    NCIRCLE("imageProcessor:robot detection", robotCenter.x, robotCenter.y,10,1, Drawings::ps_solid, Drawings::red);
    percept.directionOnField = 0;           // why = 0?
    percept.offsetOnField = percept.offsetOnField;         // ??
    percept.positionInImage = robotCenter;
    percept.validity = 1;

  Vector2<double> foot;
  if (Geometry::calculatePointOnField(lower.x, lower.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, foot)){
    percept.orientation = atan2(percept.offsetOnField.x - foot.x, percept.offsetOnField.y - foot.y);  
  }else{
    percept.orientation = 0;
  }

    playersPercept.addRedPlayer(percept);
  }
}

void GT2005PlayerSpecialist::detectBlueRobots(
     Vector2<int> averageRobotSignatureStart,
     Vector2<int> averageRobotSignatureStop,
     Vector2<int> averageRobotSignature)
{
  // walk the averageRobotSignature and search for white - robotColor - white
  // or search left and right side of the averageRobotSignature

  Vector2<int> firstWhite(1000,0),firstBlue(1000,0), lastBlue(-1000,0), lastWhite(-1000,0),runner; 
  if (averageRobotSignatureStart != averageRobotSignatureStop)
  {
    BresenhamLineScan avgRobotSigBresenham (averageRobotSignatureStart, averageRobotSignatureStop);
    runner = averageRobotSignatureStart;
    while(runner != averageRobotSignatureStop)
    {
      if ( runner.x >= imageInfo.maxImageCoordinates.x
        || runner.x < 0
        || runner.y >= imageInfo.maxImageCoordinates.y
        || runner.y < 0) 
      {
        OUTPUT(idText, text, "detectBlueRobots: Bresenham left image start "  << averageRobotSignatureStart.x << ":" << averageRobotSignatureStart.y);
        OUTPUT(idText, text, "stop "  << averageRobotSignatureStop.x << ":" << averageRobotSignatureStop.y);
        break;
      }

      colorClass inspect = CORRECTED_COLOR_CLASS(runner.x,runner.y,
          image.image[runner.y][0][runner.x],
          image.image[runner.y][1][runner.x],
          image.image[runner.y][2][runner.x],
          colorTable, colorCorrector);

      // TODO: one pixel is not enough to decide this
      if (inspect == white && firstWhite.x == 1000)
            firstWhite = runner;
      if (inspect == blue && firstBlue.x == 1000)
            firstBlue = runner;
      if (inspect == blue && lastBlue.x < runner.x)
            lastBlue = runner;
      if (inspect == white && runner.x > firstWhite.x)
            lastWhite = runner;
       
      avgRobotSigBresenham.getNext(runner);
    }
  }
  else
  {
    firstWhite = averageRobotSignatureStart;
  }

  Vector2<unsigned char> cfirstWhite,cfirstBlue, clastBlue, clastWhite;

  cfirstWhite.x = (char)firstWhite.x;
  cfirstWhite.y = (char)firstWhite.y;

  clastWhite.y = (char)lastWhite.y;
  clastWhite.x = (char)lastWhite.x;

  cfirstBlue.y = (char)firstBlue.y;
  cfirstBlue.x = (char)firstBlue.x;

  clastBlue.y = (char)lastBlue.y;
  clastBlue.x = (char)lastBlue.x;


  // TODO: only the blue playercolor were found
  
  //now take the midpoint of the white parts and start search green below
  double proportion = (averageRobotSignatureStart-averageRobotSignatureStop).abs();
  
  
  Vector2<double> dir;
  dir.x = averageRobotSignatureStop.x-averageRobotSignatureStart.x;
  dir.y = averageRobotSignatureStop.y-averageRobotSignatureStart.y;
  Geometry::Line averageLine = Geometry::Line(averageRobotSignatureStart, dir);
  // if average line angle bigger than 15 degree, we probably observe the roboter at the side
  if (sin(dir.y/proportion) > pi/12)
    proportion *= 11.0/17.0; // robot height to robot length observed at side
  else
    proportion *= 1.0; // robot height to robot width observed at front
  int depth = (int) proportion;
  Vector2<unsigned char> point,lower(0,0);
  double distance;
  if (firstWhite.x != 1000)
  {
    point = findBlueFootPointYAxis(cfirstWhite,Vector2<char>(0,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    point = findBlueFootPointYAxis(cfirstWhite,Vector2<char>(1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    point = findBlueFootPointYAxis(cfirstWhite,Vector2<char>(-1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
  }
  if (lastWhite.x != -1000)
  {      
    point = findBlueFootPointYAxis(clastWhite,Vector2<char>(0,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    point = findBlueFootPointYAxis(clastWhite,Vector2<char>(1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    point = findBlueFootPointYAxis(clastWhite,Vector2<char>(-1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
  }
  if (lower.x!=0)
  {
    Vector2<double> dlower;dlower.x = lower.x;dlower.y = lower.y;
    distance = fabs(Geometry::getDistanceToLine(averageLine,dlower));
  }
  else 
    distance = 0;
  if (firstBlue.x != 1000 && proportion-distance>0)
  {
    point = findBlueFootPointYAxis(cfirstBlue,Vector2<char>(0,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    point = findBlueFootPointYAxis(cfirstBlue,Vector2<char>(1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    point = findBlueFootPointYAxis(cfirstBlue,Vector2<char>(-1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
  }
  if (lower.x!=0)
  {
    Vector2<double> dlower;dlower.x = lower.x;dlower.y = lower.y;
    distance = fabs(Geometry::getDistanceToLine(averageLine,dlower));
  }
  else 
    distance = 0;
  if (lastBlue.x != -1000 && proportion-distance>0)
  {
    point = findBlueFootPointYAxis(clastBlue,Vector2<char>(0,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    point = findBlueFootPointYAxis(clastBlue,Vector2<char>(1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    point = findBlueFootPointYAxis(clastBlue,Vector2<char>(-1,1),depth);
    if (point.x != 255 && point.y > lower.y)  lower = point;
    DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
  }
 
  // create percept
  Vector2<double> pointOnField;
  SinglePlayerPercept percept;
  Vector2<int> robotCenter((averageRobotSignatureStop.x-averageRobotSignatureStart.x)/2+averageRobotSignatureStart.x,lower.y);
  // now take the middle of the averageRobotXLine and the y of the lowest point
  if (Geometry::calculatePointOnField(robotCenter.x, robotCenter.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, percept.offsetOnField))
  {
    CIRCLE(imageProcessor_robot_detection, lower.x, lower.y,10,1, Drawings::ps_solid, Drawings::yellow);
    CIRCLE(imageProcessor_robot_detection, robotCenter.x, robotCenter.y,10,1, Drawings::ps_solid, Drawings::blue);
    NCIRCLE("imageProcessor:robot detection", lower.x, lower.y,10,1, Drawings::ps_solid, Drawings::yellow);
    NCIRCLE("imageProcessor:robot detection", robotCenter.x, robotCenter.y,10,1, Drawings::ps_solid, Drawings::blue);
    percept.directionOnField = 0;
    percept.offsetOnField = percept.offsetOnField;
    percept.positionInImage = robotCenter;
    percept.validity = 1;
  
  Vector2<double> foot;
  if (Geometry::calculatePointOnField(lower.x, lower.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, foot)){
    percept.orientation = atan2(percept.offsetOnField.x - foot.x, percept.offsetOnField.y - foot.y);  
  }else{
    percept.orientation = 0;
  }

    playersPercept.addBluePlayer(percept);
  }
}

void GT2005PlayerSpecialist::clusterRobotLinesPercepts()
{
  // ---------------- Red robots
  clusteredRedRobots = 0;
  if (linesPercept.points[LinesPercept::redRobot].numberOfPoints > 1)
  {
    int lastStart = 0,currentClusteredPoints = 0;
    averageRedRobotSignature[0] = linesPercept.points[LinesPercept::redRobot].pointsInImage[0];
    averageRedRobotSignatureStart[0] = linesPercept.points[LinesPercept::redRobot].pointsInImage[0];
    averageRedRobotSignaturePoints[0] = 0;
    for(int c = 1;c <= linesPercept.points[LinesPercept::redRobot].numberOfPoints;c++)
    {
      averageRedRobotSignatureStart[clusteredRedRobots] = linesPercept.points[LinesPercept::redRobot].pointsInImage[lastStart];
      int diff = linesPercept.points[LinesPercept::redRobot].pointsInImage[c].x -
                linesPercept.points[LinesPercept::redRobot].pointsInImage[c-1].x;

      if (c == linesPercept.points[LinesPercept::redRobot].numberOfPoints
        || (diff >= 20 && !checkForWhiteLineBetweenRed(linesPercept.points[LinesPercept::redRobot].pointsInImage[c-1],
        linesPercept.points[LinesPercept::redRobot].pointsInImage[c])))
      {
        averageRedRobotSignatureStop[clusteredRedRobots] = linesPercept.points[LinesPercept::redRobot].pointsInImage[c-1];
        lastStart = c;
        averageRedRobotSignature[clusteredRedRobots] /= currentClusteredPoints;
        averageRedRobotSignaturePoints[clusteredRedRobots] = currentClusteredPoints;
        currentClusteredPoints = 0;
        clusteredRedRobots++;
        averageRedRobotSignaturePoints[clusteredRedRobots] = 0;
        averageRedRobotSignature[clusteredRedRobots] = Vector2<int>(0,0);
      }
      if (clusteredRedRobots>=4)
        break;
      averageRedRobotSignature[clusteredRedRobots] += linesPercept.points[LinesPercept::redRobot].pointsInImage[c];
      currentClusteredPoints++;
    } // for(int c = 1;c <= linesPercept.points[LinesPercept::redRobot].numberOfPoints;c++)
  }   // if (linesPercept.points[LinesPercept::redRobot].numberOfPoints > 1)
  else
  {
    if (linesPercept.points[LinesPercept::redRobot].numberOfPoints==1)
    {
      //only one line
      clusteredRedRobots = 1;
      averageRedRobotSignature[0] = linesPercept.points[LinesPercept::redRobot].pointsInImage[0];
      averageRedRobotSignatureStart[0] = averageRedRobotSignature[0];
      averageRedRobotSignatureStop[0] = averageRedRobotSignature[0];
      averageRedRobotSignaturePoints[0] = linesPercept.points[LinesPercept::redRobot].numberOfPoints;
    }
    else
      clusteredRedRobots = 0;

  }    
  // ---------------- Blue robots
  clusteredBlueRobots = 0;
  if (linesPercept.points[LinesPercept::blueRobot].numberOfPoints > 1)
  {
    int lastStart = 0,currentClusteredPoints = 0;
    averageBlueRobotSignature[0] = linesPercept.points[LinesPercept::blueRobot].pointsInImage[0];
    averageBlueRobotSignatureStart[0] = linesPercept.points[LinesPercept::blueRobot].pointsInImage[0];
    averageBlueRobotSignaturePoints[0] = 0;
    for(int c = 1;c <= linesPercept.points[LinesPercept::blueRobot].numberOfPoints;c++)
    {
      averageBlueRobotSignatureStart[clusteredBlueRobots] = linesPercept.points[LinesPercept::blueRobot].pointsInImage[lastStart];
      int diff = linesPercept.points[LinesPercept::blueRobot].pointsInImage[c].x -
                linesPercept.points[LinesPercept::blueRobot].pointsInImage[c-1].x;
      if (c == linesPercept.points[LinesPercept::blueRobot].numberOfPoints
        || (diff >= 20 && !checkForWhiteLineBetweenBlue(linesPercept.points[LinesPercept::blueRobot].pointsInImage[c-1],
        linesPercept.points[LinesPercept::blueRobot].pointsInImage[c])))
      {
        averageBlueRobotSignatureStop[clusteredBlueRobots] = linesPercept.points[LinesPercept::blueRobot].pointsInImage[c-1];
        lastStart = c;
        averageBlueRobotSignature[clusteredBlueRobots] /= currentClusteredPoints;
        averageBlueRobotSignaturePoints[clusteredBlueRobots] = currentClusteredPoints;
        currentClusteredPoints = 0;
        clusteredBlueRobots++;
        averageBlueRobotSignaturePoints[clusteredBlueRobots] = 0;
        averageBlueRobotSignature[clusteredBlueRobots] = Vector2<int>(0,0);
      }
      if (clusteredBlueRobots>=4)
        break;
      averageBlueRobotSignature[clusteredBlueRobots] += linesPercept.points[LinesPercept::blueRobot].pointsInImage[c];
      currentClusteredPoints++;
    }
  }
  else
  {
    if (linesPercept.points[LinesPercept::blueRobot].numberOfPoints == 1)
    {
    //only one blue line
      clusteredBlueRobots = 1;
      averageBlueRobotSignature[0] = linesPercept.points[LinesPercept::blueRobot].pointsInImage[0];
      averageBlueRobotSignatureStart[0] = averageBlueRobotSignature[0];
      averageBlueRobotSignatureStop[0] = averageBlueRobotSignature[0];
      averageBlueRobotSignaturePoints[0] = linesPercept.points[LinesPercept::blueRobot].numberOfPoints;
    }
    else
      clusteredBlueRobots = 0;
  }
}

bool GT2005PlayerSpecialist::checkForWhiteLineBetweenBlue(Vector2<int> start, Vector2<int> stop)
{
  BresenhamLineScan whiteLine (start, stop);
  Vector2<int> begin = start;

  int wrongColor = 0; 
  int overallWhite = 0; 
  int overallWrongColor = 0;
  int overallNoColor = 0;
  int overallAccepted = 0;

  colorClass inspect;
  while(start != stop)
  {
    if ( start.x >= imageInfo.maxImageCoordinates.x
      || start.x < 0
      || start.y >= imageInfo.maxImageCoordinates.y
      || start.y < 0) 
    {
      OUTPUT(idText, text, "Blue Bresenham left image");
      break;
    }
    inspect = CORRECTED_COLOR_CLASS(start.x,start.y,
        image.image[start.y][0][start.x],
        image.image[start.y][1][start.x],
        image.image[start.y][2][start.x],
        colorTable, colorCorrector);
    switch(inspect)
    {
      case blue:
      case skyblue:
        overallAccepted++;
        wrongColor = 0;
        break;
      case white:
        wrongColor = 0;
        overallWhite++;
        break;
      case noColor:
        overallNoColor++;
        wrongColor = 0;
        break;
      default:
        wrongColor++;
        overallWrongColor++;
        break;
    }
    whiteLine.getNext(start);
  }

  if (overallAccepted > overallWhite
    || overallNoColor > (overallAccepted + overallWhite)
    || overallWrongColor > (overallAccepted + overallWhite))
  {
    //LINE(imageProcessor_robot_detection, begin.x, begin.y,
    //   stop.x,stop.y,1,Drawings::white,Drawings::black);
    return false;
  }
  else
  {
    //LINE(imageProcessor_robot_detection, begin.x, begin.y,
    //   stop.x,stop.y,1,Drawings::white,Drawings::blue);
    return true;
  }


}

bool GT2005PlayerSpecialist::checkForWhiteLineBetweenRed(Vector2<int> start, Vector2<int> stop)
{
  BresenhamLineScan whiteLine (start, stop);
  Vector2<int> begin = start;
  int wrongColor = 0; 
  int overallWhite = 0; 
  int overallWrongColor = 0;
  int overallNoColor = 0;
  int overallAccepted = 0;

  colorClass inspect;
  while(start!=stop)
  {
    if ( start.x >= imageInfo.maxImageCoordinates.x
      || start.x < 0
      || start.y >= imageInfo.maxImageCoordinates.y
      || start.y < 0) 
    {
      OUTPUT(idText, text, "Red Bresenham left image");
      break;
    }
    inspect = CORRECTED_COLOR_CLASS(start.x,start.y,
        image.image[start.y][0][start.x],
        image.image[start.y][1][start.x],
        image.image[start.y][2][start.x],
        colorTable, colorCorrector);
    switch(inspect)
    {
      case red:
      case pink:
        overallAccepted++;
        wrongColor = 0;
        break;
      case white:
        wrongColor = 0;
        overallWhite++;
        break;
      case noColor:
        overallNoColor++;
        wrongColor = 0;
        break;
      default:
        wrongColor++;
        overallWrongColor++;
        break;
    }
    whiteLine.getNext(start);
  }

  if (overallAccepted > overallWhite
    || overallNoColor > (overallAccepted + overallWhite)
    || overallWrongColor > (overallAccepted + overallWhite))
  {
  //  LINE(imageProcessor_robot_detection, begin.x, begin.y,
  //     stop.x,stop.y,1,Drawings::white,Drawings::black);
    return false;
  }
  else
  {
  //  LINE(imageProcessor_robot_detection, begin.x, begin.y,
  //     stop.x,stop.y,1,Drawings::white,Drawings::red);
    return true;
  }

}

Vector2<unsigned char> GT2005PlayerSpecialist::findRedFootPointYAxis(
  Vector2<unsigned char> start,
  Vector2<char> direction,
  short depth,
  unsigned char recursionLevel)
{
  if (recursionLevel == 3)
  {
    OUTPUT(idText, text, "Maximum Red Recursion Level reached #" << recursionLevel);
    return start;
  }
  Vector2<unsigned char> runner = start;
  short steps = 0;
  while(true)
  {
    if ( runner.x >= imageInfo.maxImageCoordinates.x
      || runner.x <= 0
      || runner.y >= imageInfo.maxImageCoordinates.y
      || runner.y <= 0) 
        return Vector2<unsigned char>(255,0);
    colorClass inspect = CORRECTED_COLOR_CLASS(runner.x,runner.y,
      image.image[runner.y][0][runner.x],
      image.image[runner.y][1][runner.x],
      image.image[runner.y][2][runner.x],
      colorTable, colorCorrector);

    if (inspect == green)
          break;
    runner.x += direction.x;
    runner.y += direction.y;
    steps++;
    if (--depth > 5)
    {
      runner.x += direction.x;
      runner.y += direction.y;
      --depth;
      steps++;
    }
  }

  LINE(imageProcessor_robot_detection, start.x, start.y,
       runner.x,runner.y,1,Drawings::white,  
       depth>15 ? Drawings::yellow : Drawings::blue);
  NLINE("imageProcessor:robot detection", start.x, start.y,
       runner.x,runner.y,1,Drawings::white,  
       depth>15 ? Drawings::yellow : Drawings::blue);
  maxDepth = max(maxDepth,(int)recursionLevel);
  if (steps==0)
  {
    DOT(imageProcessor_robot_detection, runner.x,runner.y,Drawings::white, Drawings::green);
    return runner;
    NDOT("imageProcessor:robot detection", runner.x,runner.y,Drawings::white, Drawings::green);
    return runner;
  }
  Vector2<unsigned char> down, downLeft, downRight;
  if (!(direction.x == 0 && direction.y == 1))
    down = findRedFootPointYAxis(runner,Vector2<char>(0,1),depth,recursionLevel+1);
  if (!(direction.x == -1 && direction.y == 1))
    downLeft = findRedFootPointYAxis(runner,Vector2<char>(-1,1),depth,recursionLevel+1);
  if (!(direction.x == 1 && direction.y == 1))
    downRight = findRedFootPointYAxis(runner,Vector2<char>(1,1),depth,recursionLevel+1);

 

  if (down.x != 255 && down.y > downLeft.y)
  {
    if (downRight.x != 255 && downRight.y > down.y)
      return downRight;
    else
      return down;
  }
  else
  {
    if (downLeft.x != 255 && downLeft.y > downRight.y)
      return downLeft;
    else
      return downRight;
  } 
}

Vector2<unsigned char> GT2005PlayerSpecialist::findBlueFootPointYAxis(
  Vector2<unsigned char> start,
  Vector2<char> direction,
  short depth,
  unsigned char recursionLevel)
{
  if (recursionLevel == 3)
  {
    OUTPUT(idText, text, "Maximum Blue Recursion Level reached #" << recursionLevel);
    return start;
  }
  Vector2<unsigned char> runner = start;
  short steps = 0;
  while(true)
  {
    if ( runner.x >= imageInfo.maxImageCoordinates.x
      || runner.x <= 0
      || runner.y >= imageInfo.maxImageCoordinates.y
      || runner.y <= 0) 
        return Vector2<unsigned char>(255,0);
    colorClass inspect = CORRECTED_COLOR_CLASS(runner.x,runner.y,
      image.image[runner.y][0][runner.x],
      image.image[runner.y][1][runner.x],
      image.image[runner.y][2][runner.x],
      colorTable, colorCorrector);

    if (inspect == green)
          break;
    runner.x += direction.x;
    runner.y += direction.y;
    steps++;
    if (--depth > 5)
    {
      runner.x += direction.x;
      runner.y += direction.y;
      --depth;
      steps++;
    }
  }

  LINE(imageProcessor_robot_detection, start.x, start.y,
       runner.x,runner.y,1,Drawings::white,  
       depth>15 ? Drawings::yellow : Drawings::blue);
  NLINE("imageProcessor:robot detection", start.x, start.y,
       runner.x,runner.y,1,Drawings::white,  
       depth>15 ? Drawings::yellow : Drawings::blue);
  if (steps==0)
  {
    DOT(imageProcessor_robot_detection, runner.x,runner.y,Drawings::white, Drawings::green);
    NDOT("imageProcessor:robot detection", runner.x,runner.y,Drawings::white, Drawings::green);
    return runner;
  }
  Vector2<unsigned char> down, downLeft, downRight;
  if (!(direction.x == 0 && direction.y == 1))
    down = findBlueFootPointYAxis(runner,Vector2<char>(0,1),depth,recursionLevel+1);
  if (!(direction.x == -1 && direction.y == 1))
    downLeft = findBlueFootPointYAxis(runner,Vector2<char>(-1,1),depth,recursionLevel+1);
  if (!(direction.x == 1 && direction.y == 1))
    downRight = findBlueFootPointYAxis(runner,Vector2<char>(1,1),depth,recursionLevel+1);


  maxDepth = max(maxDepth,(int)recursionLevel);
  if (down.x != 255 && down.y > downLeft.y)
  {
    if (downRight.x != 255 && downRight.y > down.y)
      return downRight;
    else
      return down;
  }
  else
  {
    if (downLeft.x != 255 && downLeft.y > downRight.y)
      return downLeft;
    else
      return downRight;
  } 
}
colorClass GT2005PlayerSpecialist::inspectColorArea(const Vector2<unsigned char>& pos,const Vector2<char>& dir)
{
  // inspects a 2x2 color square and return the popularst color
  if ( pos.x >= imageInfo.maxImageCoordinates.x
      || pos.x <= 0
      || pos.y >= imageInfo.maxImageCoordinates.y
      || pos.y <= 0) 
      return CORRECTED_COLOR_CLASS(pos.x,pos.y,
        image.image[pos.y][0][pos.x],
        image.image[pos.y][1][pos.x],
        image.image[pos.y][2][pos.x],
        colorTable, colorCorrector);

  colorClass inspect;
  int cGreen = 0, cOkay = 0;
  if (dir.x == 0 || dir.y == 0)
  {
    // orthogonal to axis 
    inspect = CORRECTED_COLOR_CLASS(pos.x,pos.y,
      image.image[pos.y][0][pos.x],
      image.image[pos.y][1][pos.x],
      image.image[pos.y][2][pos.x],
      colorTable, colorCorrector);
    if (inspect == green) cGreen++;
    if (inspect == white || inspect == red || inspect == noColor) cOkay++;

    Vector2<int> insPos;
    insPos.x = pos.x + dir.x;
    insPos.y = pos.y + dir.y;
    inspect = CORRECTED_COLOR_CLASS(insPos.x,insPos.y,
      image.image[insPos.y][0][insPos.x],
      image.image[insPos.y][1][insPos.x],
      image.image[insPos.y][2][insPos.x],
      colorTable, colorCorrector);
    if (inspect == green) cGreen++;
    if (inspect == white || inspect == red || inspect == noColor) cOkay++;

    insPos.x = pos.x+dir.x;insPos.y = pos.y+dir.y; insPos.x += dir.y; insPos.y += dir.x;
    inspect = CORRECTED_COLOR_CLASS(insPos.x,insPos.y,
      image.image[insPos.y][0][insPos.x],
      image.image[insPos.y][1][insPos.x],
      image.image[insPos.y][2][insPos.x],
      colorTable, colorCorrector);
    if (inspect == green) cGreen++;
    if (inspect == white || inspect == red || inspect == noColor) cOkay++;

    insPos.x = pos.x+dir.x;insPos.y = pos.y+dir.y; insPos.x -= dir.y; insPos.y -=dir.x;
    inspect = CORRECTED_COLOR_CLASS(insPos.x,insPos.y,
      image.image[insPos.y][0][insPos.x],
      image.image[insPos.y][1][insPos.x],
      image.image[insPos.y][2][insPos.x],
      colorTable, colorCorrector);
    if (inspect == green) cGreen++;
    if (inspect == white || inspect == red || inspect == noColor) cOkay++;
  }
  else
  {
    // diagonal to axis
    inspect = CORRECTED_COLOR_CLASS(pos.x,pos.y,
      image.image[pos.y][0][pos.x],
      image.image[pos.y][1][pos.x],
      image.image[pos.y][2][pos.x],
      colorTable, colorCorrector);
    if (inspect == green) cGreen++;
    if (inspect == white || inspect == red || inspect == noColor) cOkay++;

    Vector2<int> insPos;
    insPos.x = pos.x + dir.x;
    insPos.y = pos.y + dir.y;
    inspect = CORRECTED_COLOR_CLASS(insPos.x,insPos.y,
      image.image[insPos.y][0][insPos.x],
      image.image[insPos.y][1][insPos.x],
      image.image[insPos.y][2][insPos.x],
      colorTable, colorCorrector);
    if (inspect == green) cGreen++;
    if (inspect == white || inspect == red || inspect == noColor) cOkay++;

    insPos.x = pos.x + dir.x;insPos.y = pos.y;
    inspect = CORRECTED_COLOR_CLASS(insPos.x,insPos.y,
      image.image[insPos.y][0][insPos.x],
      image.image[insPos.y][1][insPos.x],
      image.image[insPos.y][2][insPos.x],
      colorTable, colorCorrector);
    if (inspect == green) cGreen++;
    if (inspect == white || inspect == red || inspect == noColor) cOkay++;

    insPos.y = pos.y+dir.y;insPos.x = pos.x;
    inspect = CORRECTED_COLOR_CLASS(insPos.x,insPos.y,
      image.image[insPos.y][0][insPos.x],
      image.image[insPos.y][1][insPos.x],
      image.image[insPos.y][2][insPos.x],
      colorTable, colorCorrector);
    if (inspect == green) cGreen++;
    if (inspect == white || inspect == red || inspect == noColor) cOkay++;
  }
  if (cGreen >= cOkay)
    return green;
  else
    return white;
}

---