Modules/ImageProcessor/VLCImageProcessor/VLCRobotSpecialist.cpp

Go to the documentation of this file.

/**
* @file VLCRobotSpecialist.cpp
*
* This file contains a class for Image Processing.
*
* @author Tobias Wegner
*/

#include "VLCRobotSpecialist.h"

#ifdef RS_DEBUG
#ifndef APERIOS1_3_2
#pragma message( "compiling VLCRobotSpecialist with debugging" )
#endif
#endif

#ifdef RS_DEBUG_CONSOLE
#ifndef APERIOS1_3_2
#pragma message( "compiling VLCRobotSpecialist with console outputs" )
#endif
#endif

//#define RS_UNIFORM_SCANLINES
#define RS_RADIAL_ON_IMAGE

//dirty hack, prevents from compiler errors
#define min(a, b)(((a) < (b))?(a):(b))
#define max(a, b)(((a) > (b))?(a):(b))

VLCRobotSpecialist::VLCRobotSpecialist
(
 const ColorCorrector& colorCorrector_,
 const Image& image_, 
 const ImageInfo& imageInfo_,
 const ColorTable& colorTable_,
 const CameraMatrix& cameraMatrix_,
 const CameraMatrix& prevCameraMatrix_,
 LinesPercept& linesPercept_,
 PlayersPercept& playersPercept_,
 const RobotPose& robotPose_,
 const List<GreenChangeEvent>& greenChangeEvents_)
 :
colorCorrector(colorCorrector_),
image(image_),
imageInfo(imageInfo_),
colorTable(colorTable_),
cameraMatrix(cameraMatrix_),
prevCameraMatrix(prevCameraMatrix_),
linesPercept(linesPercept_),
playersPercept(playersPercept_),
robotPose(robotPose_),
greenChangeEvents(greenChangeEvents_)
{
}

void VLCRobotSpecialist::reset ()
{
  //reset candidates
  topIndex = RS_NUM_CANDIDATES / 2 + 1;
  bottomIndex = RS_NUM_CANDIDATES / 2;
}

void VLCRobotSpecialist::scan ()
{
  //some types
  Geometry::Line scanLine;
  Geometry::Line clipLine;

#ifdef RS_RADIAL_ON_IMAGE
  Vector2<int>  startPoint  = Vector2<int>(image.cameraInfo.resolutionWidth / 2, image.cameraInfo.resolutionHeight - 1);
#else
  Vector2<int>  startPoint;

  Geometry::calculatePointInImage (Vector2<int>(475, 0), cameraMatrix, CameraInfo::CameraInfo (), startPoint);

  startPoint.y = startPoint.y;
#endif
  Vector2<int>  endPoint;

  //reset everything
  reset ();

  //calculation scan line
  scanLine.base.x = startPoint.x;
  scanLine.base.y = startPoint.y;

  int i = -1;
    double startAngle = -pi_2;
    double stopAngle = pi_2;
    double stepAngle = (stopAngle - startAngle) / RS_NUM_SCAN_LINES;

  for (double angle = startAngle; angle <= stopAngle; angle += stepAngle)
  {
    CRASH_CHECK(i); i++;

    //resetting start point
    startPoint.x = (int) scanLine.base.x;
    startPoint.y = (int) scanLine.base.y;

    //calculation of radial scanline directions
#ifdef RS_RADIAL_ON_IMAGE
    scanLine.direction.x = sin (angle);
    scanLine.direction.y = cos (angle);
#else
    Vector2<int>directionOnField;
    Vector2<int>directionInImage;

    directionOnField.x = (int) (cos (angle) * 1000000);
    directionOnField.y = (int) (-sin (angle) * 1000000 + 475);

    //projecting to field
    Geometry::calculatePointInImage (directionOnField, cameraMatrix, CameraInfo::CameraInfo (), directionInImage);

    scanLine.direction.x = directionInImage.x;
    scanLine.direction.y = directionInImage.y;
#endif
    scanLine.normalizeDirection ();

    //intersecting with horizon
    clipLine = imageInfo.horizon;

    if ((scanLine.direction * clipLine.direction) == 1)
      continue;

    Geometry::getIntersectionOfLines(clipLine, scanLine, endPoint);

    //clipping, test if intersection lies in image
    if (((endPoint.x < 0) || (endPoint.x >= image.cameraInfo.resolutionWidth)) || ((endPoint.y < 0) || (endPoint.y >= image.cameraInfo.resolutionHeight)))
    {
      //intersection is outside of image, so calculate new new intersection
      if (endPoint.x < 0)
      {
        //intersection was to left, so intersect with left boundary
        clipLine.base = Vector2<double> (0, 0);
        clipLine.direction = Vector2<double> (0, 1);

        Geometry::getIntersectionOfLines(clipLine, scanLine, endPoint);
      }
      else
        if (endPoint.x >= image.cameraInfo.resolutionWidth)
        {
          //intersection lies too right, so intersect with right boundary
          clipLine.base = Vector2<double> (image.cameraInfo.resolutionWidth, 0);
          clipLine.direction = Vector2<double> (0, 1);

          Geometry::getIntersectionOfLines(clipLine, scanLine, endPoint);
        };

      if (endPoint.y < 0)
      {
        //intersection lies too high above, so intersect upper left boundary
        clipLine.base = Vector2<double> (0, 0);
        clipLine.direction = Vector2<double> (1, 0);

        Geometry::getIntersectionOfLines(clipLine, scanLine, endPoint);
      }

      if (endPoint.y >= image.cameraInfo.resolutionHeight)
        continue;                               //intersection is below image, so no scanning needed
    };

    if (((startPoint.x < 0) || (startPoint.x >= image.cameraInfo.resolutionWidth)) || ((startPoint.y < 0) || (startPoint.y >= image.cameraInfo.resolutionHeight)))
    {
      //intersection is outside of image, so calculate new new intersection
      if (startPoint.x < 0)
      {
        //intersection was to left, so intersect with left boundary
        clipLine.base = Vector2<double> (0, 0);
        clipLine.direction = Vector2<double> (0, 1);

        Geometry::getIntersectionOfLines(clipLine, scanLine, startPoint);
      }
      else
        if (startPoint.x >= image.cameraInfo.resolutionWidth)
        {
          //intersection lies too right, so intersect with right boundary
          clipLine.base = Vector2<double> (image.cameraInfo.resolutionWidth, 0);
          clipLine.direction = Vector2<double> (0, 1);

          Geometry::getIntersectionOfLines(clipLine, scanLine, startPoint);
        };

      if (startPoint.y >= image.cameraInfo.resolutionHeight)
      {
        //intersection lies too high above, so intersect upper left boundary
        clipLine.base = Vector2<double> (0, image.cameraInfo.resolutionHeight - 1);
        clipLine.direction = Vector2<double> (1, 0);

        Geometry::getIntersectionOfLines(clipLine, scanLine, startPoint);
      }

      if (startPoint.y < 0)
        continue;                               //intersection is below image, so no scanning needed
    };

#ifndef RS_UNIFORM_SCANLINES
    switch (i % 4)
    {
    case 0: { //full line
          break;
        };
    case 1: { //half line
        };
    case 3: { //half line
          //only half size scan line
          startPoint.x = (startPoint.x + endPoint.x) / 2;
          startPoint.y = (startPoint.y + endPoint.y) / 2;

          break;
        };
    case 2: {
          //only quarter size scan line
          startPoint.x = (startPoint.x + 3 * endPoint.x) / 4;
          startPoint.y = (startPoint.y + 3 * endPoint.y) / 4;

          break;
        };
    };
#endif
    scan (startPoint, endPoint);
  };

  clusterCandidates ();

  return;
}

void VLCRobotSpecialist::scan (Vector2<int> startPoint)
{
  unsigned char y, u, v;
  colorClass    lastColor = noColor;

  Vector2<int>  endPoint;
  Geometry::Line  scanLine;

  //calculating line perpendicular to horizon
  scanLine.base.x = startPoint.x;
  scanLine.base.y = startPoint.y;
  scanLine.direction.x = - imageInfo.horizon.direction.y;
  scanLine.direction.y = imageInfo.horizon.direction.x;

  //calculating endpoint for bresenham
  Geometry::getIntersectionOfLines (imageInfo.horizon, scanLine, endPoint);

  // init Bresenham
  BresenhamLineScan bresenhamScanLine = BresenhamLineScan (startPoint, endPoint);
  Vector2<int>    actual    = startPoint;

  Vector2<int> footPoint = startPoint;

  for (int i = 0; i <= bresenhamScanLine.numberOfPixels; ++i)
  {
    CRASH_CHECK(i);

    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]);
    }
      colorClass color = COLOR_CLASS(y, u, v);

    if ((color == green) || (color == orange))
    {
      footPoint = actual;
    };

    if ((color == blue) || (color == red))
    {
      if ((color != lastColor) || (color == noColor))
      {
        //other robot found
        RobotCandidate  candidate;

        candidate.footPoint = footPoint;
        candidate.candidateStart = actual;
        candidate.color = color;

        addCandidate (candidate);

        lastColor = color;
      }
    }

    bresenhamScanLine.getNext (actual);
  } 

#ifdef RS_DEBUG
  LINE(imageProcessor_robot_detection,
    startPoint.x, startPoint.y, endPoint.x, endPoint.y,
    1, Drawings::ps_solid, Drawings::blue );
#endif

  return;
}

void VLCRobotSpecialist::scan (Vector2<int> startPoint, Vector2<int> endPoint)
{
  // init Bresenham
  BresenhamLineScan scanLine  = BresenhamLineScan (startPoint, endPoint);
  Vector2<int>    actual    = startPoint;

  unsigned char y, u, v;

  Vector2<int>  lastGreenPosition;
  int       lastGreenCount    = -1;
  bool      searchForGreen    = false;

  // test whether or not startpoint has green color
  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]);
  }
  colorClass color = COLOR_CLASS(y, u, v);

  if (color != green)
    return;                             //dont know, where green stopped, so better leave

  double threshold = RS_NGP_TH_START;

  for (int i = 0; i <= scanLine.numberOfPixels; ++i)
  {
    CRASH_CHECK(i);

    // <copied from imp>
    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]);
    }
      colorClass color = COLOR_CLASS(y, u, v);
    //<copied end>

#ifndef RS_DEBUG_DRAW_RADIAL_SCANLINES_ONLY
    //search for obstacles
    if ((color == green) || (color == noColor) || (searchForGreen))
    {
      if (color == green)
      {
#ifdef RS_FIXED_NO_GREEN_THRESHOLD
        if (i - lastGreenCount < threshold)
#else
        if (i - lastGreenCount < 2 + (1.0 - ((double) i / (double) scanLine.numberOfPixels)) * 7.5)
#endif
          lastGreenCount = -1;

        searchForGreen = false;
      }
    }
    else
    {
      //check how many not green pixels are on the line
      if (lastGreenCount < 0)
      {
        lastGreenCount = i;
        lastGreenPosition = actual;
      }

#ifdef RS_FIXED_NO_GREEN_THRESHOLD
      if (i - lastGreenCount >= RS_NO_GREEN_PIXEL_THRESHOLD)
#else
      if (i - lastGreenCount >= threshold)
#endif
      {
        //obstacle found
        scan (lastGreenPosition);
          
        searchForGreen = true;
      }
    }
#endif

    scanLine.getNext (actual);

    threshold -= (RS_NGP_TH_START - RS_NGP_TH_STOP) / scanLine.numberOfPixels;
  };

#ifdef RS_DEBUG
  LINE(imageProcessor_robot_detection,
    startPoint.x, startPoint.y, endPoint.x, endPoint.y,
    1, Drawings::ps_solid, Drawings::blue );
#endif

  return;
}

void VLCRobotSpecialist::addCandidate (RobotCandidate candidate)
{
  int index;

  //a lineid > 0 means, that we are scanning left to right, so insert right to preserve order
  if (candidate.lineID > 0)
    index = topIndex;
  else
    index = bottomIndex;

  //test index inside array boundary
  if ((index > 0) && (index < RS_NUM_CANDIDATES))
  {
    //yes is, so save
    candidates[index] = candidate;

    //move borders
    if (candidate.lineID > 0)
            topIndex++;
    else
      bottomIndex--;
  }
  else
  {
    //no, outside, we had too many candidates
    RECTANGLE (imageProcessor_robot_detection, candidate.footPoint.x - 2, candidate.footPoint.y - 2, candidate.footPoint.x + 2, candidate.footPoint.y + 2, 4, Drawings::ps_solid, Drawings::gray);
  }

  //do debug drawings
#ifdef  RS_DEBUG
  {
    if (candidate.color == blue)
    {
#ifdef RS_DEBUG_SHOW_START_RUN
      CIRCLE (imageProcessor_robot_detection, candidate.candidateStart.x, candidate.candidateStart.y, 1, 1, Drawings::ps_dot, Drawings::blue);
#endif

#ifdef RS_DEBUG_SHOW_RUNS
      LINE (imageProcessor_robot_detection, candidate.candidateStart.x, candidate.candidateStart.y, candidate.candidateStop.x, candidate.candidateStop.y, 1, Drawings::ps_solid, Drawings::blue);
#endif
    }
    else
    {
#ifdef RS_DEBUG_SHOW_START_RUN
      CIRCLE (imageProcessor_robot_detection, candidate.candidateStart.x, candidate.candidateStart.y, 1, 1, Drawings::ps_dot, Drawings::red);
#endif

#ifdef RS_DEBUG_SHOW_RUNS
      LINE (imageProcessor_robot_detection, candidate.candidateStart.x, candidate.candidateStart.y, candidate.candidateStop.x, candidate.candidateStop.y, 1, Drawings::ps_solid, Drawings::red);
#endif
    }
  }
#endif
}

bool VLCRobotSpecialist::findNearestFootPoint (Vector2<int> &footPoint)
{
  enum Gradient
  {
    equal,
    up,
    down
  };

  Gradient  leftGradient = equal, rightGradient = equal;

  GreenChangeEvent  lastLeft, lastRight;
  int         delta;

  int         orgLineID;

  bool        nextLeftStepFound = false;
  bool        nextRightStepFound  = false;

  Vector2<int>    maxLeftPoint;
  int         maxLeftDistance;

  Vector2<int>    maxRightPoint;
  int         maxRightDistance;

  GreenChangeEvent tmpEvent;

  int i = 0;

  //first loop throug all feet, to find one fitting to candidates foot
  while (footIterator)
  {
    CRASH_CHECK(i); i++;

    GreenChangeEvent event = greenChangeEvents[footIterator];

    //only look for start events
    if (event.startEvent)
      if (event.positionOnImage == footPoint)
      { 
        //found start point for search
        orgLineID = event.lineID;

        //init iterators
        maxLeftPoint = footPoint;
        maxLeftDistance = event.distanceToHorizon;

        maxRightPoint = footPoint;
        maxRightDistance = event.distanceToHorizon;

        List<GreenChangeEvent>::Pos leftIter = footIterator;
        List<GreenChangeEvent>::Pos rightIter = footIterator;

        lastLeft = event;
        lastRight = event;

        leftIter--;
        rightIter++;

        int j = 0;

        //loop while both iterators are inside the list boundary
        while (leftIter | rightIter)
        {
          CRASH_CHECK(j); j++;
          
          //look left, so find next left foot
          int k = 0;

          while (leftIter)
          {
            CRASH_CHECK(k); k++;
            event = greenChangeEvents[leftIter];

            //check line id
            if (lastLeft.lineID > ((GreenChangeEvent)greenChangeEvents[leftIter]).lineID + 1)
            {
              //new line id is 2 less than last ones, so we went too far and have to go back
              leftIter++;

              //do we have a start event?
              if (!((GreenChangeEvent)greenChangeEvents[leftIter]).startEvent || 
                (((GreenChangeEvent)greenChangeEvents[leftIter]).lineID == lastLeft.lineID))
              {
                //no, or we are on the last line, there is one line missing, draw orange
#ifdef RS_DEBUG_FEET
                LINE (imageProcessor_robot_detection, lastLeft.positionOnImage.x, lastLeft.positionOnImage.y, greenChangeEvents[leftIter].positionOnImage.x, greenChangeEvents[leftIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::orange);
#endif

                //end search
                leftIter = NULL;

                continue;
              }
              else
              {
                //yes, start event so continue search
                nextLeftStepFound = true;
              };
            };

            //are we too far from original line?
            if (orgLineID - event.lineID > RS_FOOT_THRESHOLD)
            { 
              //yes, search went to far away
#ifdef RS_DEBUG_FEET
              LINE (imageProcessor_robot_detection, lastLeft.positionOnImage.x, lastLeft.positionOnImage.y, greenChangeEvents[leftIter].positionOnImage.x, greenChangeEvents[leftIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::white);
#endif
              //so end search
              leftIter = NULL;

              continue;
            };

            if ((((GreenChangeEvent)greenChangeEvents[leftIter]).distanceToHorizon <= lastLeft.distanceToHorizon) && 
              (((GreenChangeEvent)greenChangeEvents[leftIter]).lineID) < lastLeft.lineID)
            { 
              //current event is closer to horizon than last one, and on a smaler scanline
              if (!((GreenChangeEvent)greenChangeEvents[leftIter]).startEvent)
              {
                //not a startevent, so go one step back
                leftIter++;

                tmpEvent = greenChangeEvents[leftIter];

                if (lastLeft.lineID == ((GreenChangeEvent)greenChangeEvents[leftIter]).lineID)
                {
                  //back on old scanline, so draw pink debug drawing
#ifdef RS_DEBUG_FEET
                  LINE (imageProcessor_robot_detection, lastLeft.positionOnImage.x, lastLeft.positionOnImage.y, greenChangeEvents[leftIter].positionOnImage.x, greenChangeEvents[leftIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::red);
#endif

                  leftIter = NULL;  //switched back, error

                  continue;
                }
                else
                  if (!((GreenChangeEvent)greenChangeEvents[leftIter]).startEvent)
                  {
                    //start event expected, so draw black
#ifdef RS_DEBUG_FEET
                    LINE (imageProcessor_robot_detection, lastLeft.positionOnImage.x, lastLeft.positionOnImage.y, greenChangeEvents[leftIter].positionOnImage.x, greenChangeEvents[leftIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::black);
#endif

                    leftIter = NULL;  //switched back, error

                    continue;
                  }
              }

              nextLeftStepFound = true;
            }

            if (nextLeftStepFound)
            {
              //check gradient
              delta = lastLeft.distanceToHorizon - ((GreenChangeEvent)greenChangeEvents[leftIter]).distanceToHorizon;

              if (delta > RS_FOOT_GRADIENT)
              { //up
                switch (leftGradient)
                {
                case up:
                  {
                    leftGradient = up;

                    break;
                  };
                case equal:
                case down:
                  {
                    footPoint = maxLeftPoint;

#ifdef RS_DEBUG_FEET
                    CIRCLE (imageProcessor_robot_detection, footPoint.x, footPoint.y, 2, 1, Drawings::ps_solid, Drawings::pink);
#endif
  
                    return true;
                  };
                }
              }
              else
              {
                if (delta < -RS_FOOT_GRADIENT)
                { //down
                  leftGradient = down;
                }
                else
                {
                  leftGradient = equal;
                }
              }

#ifdef RS_DEBUG_FEET
              LINE (imageProcessor_robot_detection, lastLeft.positionOnImage.x, lastLeft.positionOnImage.y, greenChangeEvents[leftIter].positionOnImage.x, greenChangeEvents[leftIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::pink);
#endif

              lastLeft = greenChangeEvents[leftIter];

              if (maxLeftDistance < lastLeft.distanceToHorizon)
              {
                maxLeftDistance = lastLeft.distanceToHorizon;

                maxLeftPoint = lastLeft.positionOnImage;
              };

              nextLeftStepFound = false;

              leftIter--;

              break;
            };

            leftIter--;
          };

          //look right
          //find next foot right
          k = 0;
          while (rightIter)
          {
            CRASH_CHECK(k); k++;
            event = greenChangeEvents[rightIter];

            if (lastRight.lineID + 1 < ((GreenChangeEvent)greenChangeEvents[rightIter]).lineID)
            {
              //new line id is 2 less than last ones, so go one back
              rightIter--;

              if (!((GreenChangeEvent)greenChangeEvents[rightIter]).startEvent || 
                (((GreenChangeEvent)greenChangeEvents[rightIter]).lineID == lastRight.lineID))
              {
                //was one line too far, draw orange
#ifdef RS_DEBUG_FEET
                LINE (imageProcessor_robot_detection, lastRight.positionOnImage.x, lastRight.positionOnImage.y, greenChangeEvents[rightIter].positionOnImage.x, greenChangeEvents[rightIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::orange);
#endif

                rightIter = NULL;

                continue;
              }
              else
                nextRightStepFound = true;
            };

            if (event.lineID - orgLineID > RS_FOOT_THRESHOLD)
            { 
              //search went to far away
#ifdef RS_DEBUG_FEET
              LINE (imageProcessor_robot_detection, lastRight.positionOnImage.x, lastRight.positionOnImage.y, greenChangeEvents[rightIter].positionOnImage.x, greenChangeEvents[rightIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::white);
#endif

              rightIter = NULL; //one line too far

              continue;
            };

            if ((((GreenChangeEvent)greenChangeEvents[rightIter]).distanceToHorizon >= lastRight.distanceToHorizon) && 
              (((GreenChangeEvent)greenChangeEvents[rightIter]).lineID) > lastRight.lineID)
            { 
              //current event is closer to horizon than last one, and on a smaler scanline
              if (!((GreenChangeEvent)greenChangeEvents[rightIter]).startEvent)
              {
                //not a startevent, so go one step back
                rightIter--;

                tmpEvent = greenChangeEvents[rightIter];

                if (lastRight.lineID == ((GreenChangeEvent)greenChangeEvents[rightIter]).lineID)
                {
                  //back on old scanline, so draw pink debug drawing
#ifdef RS_DEBUG_FEET
                  LINE (imageProcessor_robot_detection, lastRight.positionOnImage.x, lastRight.positionOnImage.y, greenChangeEvents[rightIter].positionOnImage.x, greenChangeEvents[rightIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::red);
#endif

                  rightIter = NULL; //switched back, error

                  continue;
                }
                else
                  if (!((GreenChangeEvent)greenChangeEvents[rightIter]).startEvent)
                  {
                    //start event expected, so draw black
#ifdef RS_DEBUG_FEET
                    LINE (imageProcessor_robot_detection, lastRight.positionOnImage.x, lastRight.positionOnImage.y, greenChangeEvents[rightIter].positionOnImage.x, greenChangeEvents[rightIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::black);
#endif

                    rightIter = NULL; //switched back, error

                    continue;
                  }
              }

              nextRightStepFound = true;
            }

            if (nextRightStepFound)
            {
              //check gradient
              delta = lastRight.distanceToHorizon - ((GreenChangeEvent)greenChangeEvents[rightIter]).distanceToHorizon;

              if (delta > RS_FOOT_GRADIENT)
              { //up
                switch (rightGradient)
                {
                case up:
                  {
                    rightGradient = up;

                    break;
                  };
                case equal:
                case down:
                  {
                    footPoint = maxRightPoint;

#ifdef RS_DEBUG_FEET
                    CIRCLE (imageProcessor_robot_detection, footPoint.x, footPoint.y, 2, 1, Drawings::ps_solid, Drawings::pink);
#endif
  
                    return true;
                  };
                }
              }
              else
              {
                if (delta < -RS_FOOT_GRADIENT)
                { //down
                  rightGradient = down;
                }
                else
                {
                  rightGradient = equal;
                }
              }

#ifdef RS_DEBUG_FEET
              LINE (imageProcessor_robot_detection, lastRight.positionOnImage.x, lastRight.positionOnImage.y, greenChangeEvents[rightIter].positionOnImage.x, greenChangeEvents[rightIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::pink);
#endif

              lastRight = greenChangeEvents[rightIter];

              if (maxRightDistance < lastRight.distanceToHorizon)
              {
                maxRightDistance = lastRight.distanceToHorizon;

                maxRightPoint = lastRight.positionOnImage;
              };

              nextRightStepFound = false;

              break;
            };

            rightIter++;
          };
        }

        footPoint.x = -1;
        footPoint.y = -1;

        return false;
      }

    footIterator++;
  };

  footIterator = greenChangeEvents.getFirst ();

  footPoint.x = -1;
  footPoint.y = -1;

  return false;
};

void VLCRobotSpecialist::clusterCandidates (colorClass color)
{
  //init
  RobotCandidate *candidate = NULL, *lastCandidate = NULL;

  int element = bottomIndex + 1;

  CandidateCluster  robotCluster;

  robotCluster.startPosition = -1;
  robotCluster.stopPosition = -1;

#ifdef RS_DEBUG_CONSOLE
  OUTPUT (idText, text, "--------");
#endif

#ifdef  RS_DEBUG_SHOW_GREENCHANGE
  //print greenchange events
  dbg_PrintGreenchanges ();
#endif

  footIterator = greenChangeEvents.getFirst ();

  if (element == topIndex)
  {
    //no element found, so return
    return;
  }

  bool firstLoop = true;

  int i = 0;
  while (element < topIndex)
  {
    CRASH_CHECK(i); i++;

    //get next candidate
    candidate = &candidates[element];

    //check color
    if (candidate->color != color)
    {
      //wrong color, so continue
      element++;

      continue;
    }

    //do we find a foot point ?
    if (!findNearestFootPoint (candidate->footPoint))
    {
      //no, so go on
//      candidate->color = noColor; 
      
      element++;

#ifdef RS_DEBUG
      CIRCLE (imageProcessor_robot_detection, candidate->candidateStart.x, candidate->candidateStart.y, 3, 1, Drawings::ps_solid, Drawings::orange);
#endif

      continue;
    };

    //are we in the first loop ?
    if (firstLoop)
    {
      //yes, so init cluster
      robotCluster.startPosition = element;
      robotCluster.stopPosition = element;

      element++;
      lastCandidate = candidate;

      //not in first loop anymore
      firstLoop = false;

      continue;
    };

    //calculate the distance on field between last and actual footpoint
    int distance = distanceOnField (lastCandidate->footPoint, candidate->footPoint, cameraMatrix, CameraInfo::CameraInfo ());

    //generate colorspace between them
    ColorSpace colorSpace = generateColorspace (lastCandidate->candidateStart, candidate->candidateStart);

    //normalize colorspace
    colorSpace.Normalize ();

    //check colorspace and distance
    if (
        ((colorSpace.colorCount[green] >= 0.1) && (candidate->lineID - lastCandidate->lineID >= 3))
        || (distance > 400)
        || ((candidate->color == red) && (colorSpace.colorCount[blue] >= 0.1))
        || ((candidate->color == blue) && (colorSpace.colorCount[red] >= 0.1)) 
      )
    {
      //new percept found, so reset everything
#ifdef RS_DEBUG

#ifdef RS_DEBUG_SHOW_CLUSTER_INFO
      if (colorSpace.colorCount[green] >= 0.1)
        OUTPUT (idText, text, "clustering : too much green found: " << colorSpace.colorCount[green] << "; line " << candidate->lineID);

      if (distance > 400)
        OUTPUT (idText, text, "clustering : too far away: " << distance << " ; line " << candidate->lineID);

      if ((candidate->color == red) && (colorSpace.colorCount[blue] >= 0.1))
        OUTPUT (idText, text, "clustering : wrong color found : blue; line " << candidate->lineID);

      if ((candidate->color == blue) && (colorSpace.colorCount[red] >= 0.1))
        OUTPUT (idText, text, "clustering : wrong color found : red; line " << candidate->lineID);
#endif

#endif
      //generate percept for last cluster
      if (lastCandidate->color == red || lastCandidate->color == blue)
        generatePercept (robotCluster);

      //start new cluster
      robotCluster.startPosition = element;
      robotCluster.stopPosition = element;
    }
    else
    {
      //increase cluster
      robotCluster.stopPosition = element;

#ifdef  RS_DEBUG
      //debug output, show connection between points
      if (candidate->color == blue)
      {
#ifdef RS_DEBUG_SHOW_CONNECTIONS
        LINE (imageProcessor_robot_detection, lastCandidate->candidateStart.x, lastCandidate->candidateStart.y,
          candidate->candidateStart.x, candidate->candidateStart.y, 2, Drawings::ps_dot, Drawings::blue);
#endif
      }
      else
      {
#ifdef RS_DEBUG_SHOW_CONNECTIONS
        LINE (imageProcessor_robot_detection, lastCandidate->candidateStart.x, lastCandidate->candidateStart.y,
          candidate->candidateStart.x, candidate->candidateStart.y, 2, Drawings::ps_dot, Drawings::red);
#endif
      };
#endif
    }

    //go on to next candidate
    element++;
    lastCandidate = candidate;
  };

  //there is still a cluster left
  if (robotCluster.stopPosition != -1)
    generatePercept (robotCluster);

  return;
}

void VLCRobotSpecialist::generatePercept (CandidateCluster  cluster)
{
  int       element;

  Vector2<int>  average = Vector2<int>(0, 0);
  Vector2<int>  filter  = Vector2<int>(0, 0);
  VLCRectangle  body;
  VLCRectangle  foot;

  int       count = 0;

  int       varianz = 0;

  double      aspectRatio = 0;

  RobotCandidate  candidate;
  RobotCandidate  lastCandidate;

  RobotDirection  robotDirection;

  int       longestTrikotRun = 0;

//debugging variables
#ifdef RS_DEBUG_CONSOLE
  int       dbg_i;
#endif

  //init percept information
  PerceptInfo   perceptInfo;

  perceptInfo.minBounding = Vector2<int>(1024, 1024);
  perceptInfo.maxBounding = Vector2<int>(0, 0);
  perceptInfo.bottomLine  = Vector2<int>(0, 0);
  perceptInfo.flags = 0;

  //only one sample, so throw away
//  if (cluster.stopPosition - cluster.startPosition < 1)
//    return;
  perceptInfo.lines = cluster.stopPosition - cluster.startPosition + 1;

  //generate boundings
  count = generateBoundings (cluster, body, foot, filter);

  //do
  if (count > 0)
  {
    filter.x /= count;
    filter.y /= count;

    varianz = foot.y2 - foot.y1;

    int threshold = filter.y - (int) (varianz * 0.1);

    count = 0;

    lastCandidate.lineID = -1;

    //calculate bottom line
    int i = 0;
    for (element = cluster.startPosition; element <= cluster.stopPosition; element++)
    {
      CRASH_CHECK(i); i++;

      candidate = candidates[element];

      if (longestTrikotRun < (candidate.candidateStart.y - candidate.candidateStop.y))
        longestTrikotRun = candidate.candidateStart.y - candidate.candidateStop.y;

      if (candidate.footPoint.y >= threshold)
      {
        average.x += candidate.footPoint.x;
        average.y += candidate.footPoint.y;

        if (lastCandidate.lineID == -1)
          lastCandidate = candidate;

        perceptInfo.bottomLine.x += candidate.footPoint.x - lastCandidate.footPoint.x;
        perceptInfo.bottomLine.y += candidate.footPoint.y - lastCandidate.footPoint.y;

        count++;
        lastCandidate = candidate;

#ifdef  RS_DEBUG
        CIRCLE (imageProcessor_robot_detection, candidate.footPoint.x, candidate.footPoint.y, 1, 1, Drawings::ps_solid, Drawings::black);
#endif
      }
      else
      {
#ifdef  RS_DEBUG
        CIRCLE (imageProcessor_robot_detection, candidate.footPoint.x, candidate.footPoint.y, 1, 1, Drawings::ps_solid, Drawings::dark_gray);
#endif
      }
    }

    if (count > 0)
    {
      //calculation average from footpoints
      average.x /= count;
      average.y /= count;

      //calclulate bounding box
      {
        perceptInfo.minBounding.x = min (body.x1, body.x1);
        perceptInfo.minBounding.y = min (body.y1, body.y1);
        perceptInfo.maxBounding.x = max (body.x2, body.x2);
        perceptInfo.maxBounding.y = max (body.y2, average.y);
      }

      unsigned char y, u, v, ly = 0;
      //grow bounding box
#ifdef RS_GROW_BB
      {
        int xcoord, ycoord;

        xcoord = minBounding.x;
        ycoord = (int)((double)((maxBounding.y - minBounding.y) * 0.8) + minBounding.y);

        int k = 0;
        while (xcoord > 0)
        {
          CRASH_CHECK(k); k++;
          // <copied from imp>
          if (xcoord < 0 || xcoord >= image.cameraInfo.resolutionWidth || ycoord < 0 || ycoord >= image.cameraInfo.resolutionHeight)
            y = u = v = 0;
          else
          {
            y = colorCorrector.correct(xcoord, ycoord, 0, image.image[ycoord][0][xcoord]);
            u = colorCorrector.correct(xcoord, ycoord, 1, image.image[ycoord][1][xcoord]);
            v = colorCorrector.correct(xcoord, ycoord, 2, image.image[ycoord][2][xcoord]);
          }
          colorClass color = COLOR_CLASS(y, u, v);
          //<copied end>

          if (color == green)
            break;

#ifdef RS_DEBUG
          DOT (imageProcessor_robot_detection, xcoord, ycoord, ColorClasses::colorClassToDrawingsColor (color), ColorClasses::colorClassToDrawingsColor (color));
#endif

          xcoord--;
        };

        if (xcoord != 0)
          minBounding.x = xcoord + 1;

        xcoord = maxBounding.x;

        k = 0;
        while (xcoord < image.cameraInfo.resolutionWidth)
        {
          CRASH_CHECK(k); k++;
          // <copied from imp>
          if (xcoord < 0 || xcoord >= image.cameraInfo.resolutionWidth || ycoord < 0 || ycoord >= image.cameraInfo.resolutionHeight)
            y = u = v = 0;
          else
          {
            y = colorCorrector.correct(xcoord, ycoord, 0, image.image[ycoord][0][xcoord]);
            u = colorCorrector.correct(xcoord, ycoord, 1, image.image[ycoord][1][xcoord]);
            v = colorCorrector.correct(xcoord, ycoord, 2, image.image[ycoord][2][xcoord]);
          }
          colorClass color = COLOR_CLASS(y, u, v);
          //<copied end>

          if (color == green)
            break;

#ifdef RS_DEBUG
          DOT (imageProcessor_robot_detection, xcoord, ycoord, ColorClasses::colorClassToDrawingsColor (color), ColorClasses::colorClassToDrawingsColor (color));
#endif

          xcoord++;
        };

        if (xcoord != image.cameraInfo.resolutionWidth)
          maxBounding.x = xcoord - 1;
      }
#endif
      //test for size
      {
        Vector2<int>  fieldPosition;
        Vector2<int>  vec2;

        Geometry::calculatePointOnField(perceptInfo.minBounding.x, perceptInfo.maxBounding.y, cameraMatrix, CameraInfo::CameraInfo (), fieldPosition);
        Geometry::calculatePointOnField(perceptInfo.maxBounding.x, perceptInfo.maxBounding.y, cameraMatrix, CameraInfo::CameraInfo (), vec2);

        vec2.x -= fieldPosition.x;
        vec2.y -= fieldPosition.y;

        if ((vec2.abs() < 100) || (vec2.abs() > 500))
          perceptInfo.flags |= MSHPPF::wrongSize;
      }

      //generate colorspace
#define NUMSUBSPACES  3
      ColorSpace    colorSpace;
      ColorSpace    subSpaces[NUMSUBSPACES];
      int       subSpace;
      {
        int xcoord, ycoord;

        int k = 0;
        for (xcoord = perceptInfo.minBounding.x; xcoord < perceptInfo.maxBounding.x; xcoord += max ((perceptInfo.maxBounding.x - perceptInfo.minBounding.x) / 9, 1))
        {
          CRASH_CHECK(k); k++;
          subSpace = (int)((xcoord - perceptInfo.minBounding.x) / (double)((perceptInfo.maxBounding.x - perceptInfo.minBounding.x) / (double)NUMSUBSPACES));

          int l = 0;
          for (ycoord = perceptInfo.minBounding.y; ycoord < perceptInfo.maxBounding.y; ycoord += max ((perceptInfo.maxBounding.y - perceptInfo.minBounding.y) / 10, 1))
          {
            CRASH_CHECK(l); l++;
            // <copied from imp>
            if (xcoord < 0 || xcoord >= image.cameraInfo.resolutionWidth || ycoord < 0 || ycoord >= image.cameraInfo.resolutionHeight)
              y = u = v = 0;
            else
            {
              y = colorCorrector.correct(xcoord, ycoord, 0, image.image[ycoord][0][xcoord]);
              u = colorCorrector.correct(xcoord, ycoord, 1, image.image[ycoord][1][xcoord]);
              v = colorCorrector.correct(xcoord, ycoord, 2, image.image[ycoord][2][xcoord]);
            }
            colorClass color = COLOR_CLASS(y, u, v);
            //<copied end>

            if (ycoord == perceptInfo.minBounding.y)
              ly = y;

            subSpaces[subSpace].AddSample (color, y, y - ly);
          };
        };

        for (subSpace = 0; subSpace < NUMSUBSPACES; subSpace++)
        {
          CRASH_CHECK(subSpace);

          colorSpace += subSpaces[subSpace];
        };
      }

      //normalize
      {
        for (subSpace = 0; subSpace < NUMSUBSPACES; subSpace++)
        {
          CRASH_CHECK(subSpace);

          subSpaces[subSpace].Normalize ();

#ifdef RS_DEBUG
//        RECTANGLE (imageProcessor_robot_detection, minBounding.x
#endif
#ifdef RS_DEBUG_CONSOLE
          OUTPUT (idText, text, "y subspace " << subSpace << " : " << subSpaces[subSpace].y);
#endif
        };

        colorSpace.Normalize ();
      }

      //
      //show statistics on console
      {
#ifdef  RS_DEBUG_CONSOLE
        for (dbg_i = 0; dbg_i < numOfColors; dbg_i++)
        {
          CRASH_CHECK(dbg_i);

          OUTPUT (idText, text, ColorClasses::getColorName ((colorClass) dbg_i) << " : " << colorSpace.colorCount[dbg_i]);
        };

        OUTPUT (idText, text, "brightness : " << colorSpace.y);
        OUTPUT (idText, text, "delta brightness : " << colorSpace.dY);
#endif
      }

      //
      //do some filtering
      {
        unsigned int filterFlags = filterPercepts (colorSpace, candidate.color);

        if (filterFlags)
          perceptInfo.flags |= filterFlags;
      }


      //
      //calculate aspect ratio of bounding box
      {
        if (perceptInfo.maxBounding.y != perceptInfo.minBounding.y)
          aspectRatio = (perceptInfo.maxBounding.x - perceptInfo.minBounding.x) / (perceptInfo.maxBounding.y - perceptInfo.minBounding.y);
        else
          aspectRatio = 0;

        if (aspectRatio < 0)
        {
          //higher than wide, so robot seems to be seen from front or back
          robotDirection.AddSample (0);
          robotDirection.AddSample (pi);
        }
        else
        {
          //wider than high, so robot seems to be seen from side
          robotDirection.AddSample (pi_2);
          robotDirection.AddSample (-pi_2);
        }
      }
      //
      //calculate direction from color dispersion
      {
        if (colorSpace.colorCount[red] > 0.15)
        {
          //much red, so robot seems to be seen from back
          robotDirection.AddSample (0, 10);

#ifdef  RS_DEBUG_CONSOLE
          OUTPUT (idText, text, "robot seen from back");
#endif
        }
      }

      //
      //calculate direction from brightness
      {
        if ((subSpaces[0].y < subSpaces[1].y) && (subSpaces[0].y < subSpaces[2].y))
        { //robot seen from front / right
          robotDirection.AddSample (pi + pi_4, 2);
        }

        if ((subSpaces[1].y < subSpaces[0].y) && (subSpaces[1].y < subSpaces[2].y))
        { //robot seen from front or back
          robotDirection.AddSample (pi, 2);
          robotDirection.AddSample (0, 1);
          
          if (colorSpace.y < 75)
          {
            //quite dark, so robot seems to be seen from front
            robotDirection.AddSample (pi, 2);

#ifdef  RS_DEBUG_CONSOLE
            OUTPUT (idText, text, "robot seen from front");
#endif
          };
        }

        if ((subSpaces[2].y < subSpaces[0].y) && (subSpaces[2].y < subSpaces[1].y))
        { //robot seen from front / left
          robotDirection.AddSample (pi - pi_4, 2);
        }
      }

      //generate percept
      {
        perceptInfo.position  = average;
        perceptInfo.direction = robotDirection.getDirection ();
        perceptInfo.validity  = robotDirection.getReliability ();
        perceptInfo.color   = candidate.color;
        perceptInfo.count   = count;

        addPercept (perceptInfo); 
      }
    }
    else
      if (longestTrikotRun > 25)// && (candidate.candidateStop != Vector2<int>(0, 0)))
      { //robot is very near
        perceptInfo.position  = Vector2<int> (candidate.candidateStop.x, image.cameraInfo.resolutionHeight);
        perceptInfo.direction = 0;
        perceptInfo.validity  = 0.75;
        perceptInfo.color   = candidate.color;
        perceptInfo.count   = 0;

        addPercept (perceptInfo); 
      }
  };
}

int VLCRobotSpecialist::noColorPixelsBetween (Vector2<int> startPoint, Vector2<int> endPoint, colorClass noColor)
{
  // init Bresenham
  BresenhamLineScan scanLine  = BresenhamLineScan (startPoint, endPoint);
  Vector2<int>    actual    = startPoint;

  unsigned char y, u, v;

  int result = 0;

  for (int i = 0; i <= scanLine.numberOfPixels; ++i)
  {
    CRASH_CHECK(i);
    // <copied from imp>
    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]);
    }
      colorClass color = COLOR_CLASS(y, u, v);
    //<copied end>

    if (color == noColor)
      result++;

    // Bresenham
    scanLine.getNext (actual);
  };

  return result;
}

ColorSpace VLCRobotSpecialist::generateColorspace (Vector2<int> startPoint, Vector2<int> endPoint)
{
  ColorSpace colorSpace;

  // init Bresenham
  BresenhamLineScan scanLine  = BresenhamLineScan (startPoint, endPoint);
  Vector2<int>    actual    = startPoint;

  unsigned char y, u, v, ly = 0;

  int i;

  for (i = 0; i < numOfColors; i++)
  {
    CRASH_CHECK(i);

    colorSpace.colorCount[i] = 0;
  }

  for (i = 0; i <= scanLine.numberOfPixels; ++i)
  {
    CRASH_CHECK(i);

    // <copied from imp>
    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]);
    }
      colorClass color = COLOR_CLASS(y, u, v);
    //<copied end>

    if (i == 0)
      ly = y;

    colorSpace.colorCount[color]++;
    colorSpace.y += y;
    colorSpace.dY += y - ly;

#ifdef RS_DEBUG_SHOW_COLORSPACE_INFO
    LINE(imageProcessor_scanLines, actual.x, actual.y, actual.x, actual.y, 1, Drawings::ps_solid, ColorClasses::colorClassToDrawingsColor (color));
#endif

    // Bresenham
    scanLine.getNext (actual);
  };

  return colorSpace;
}

unsigned int VLCRobotSpecialist::filterPercepts (ColorSpace colorSpace, colorClass candidateColor)
{
  unsigned int result = 0;

  if ((colorSpace.colorCount[red] < 0.01) && (colorSpace.colorCount[blue] < 0.01))
  { //too few trickot found, so asume as noise
#ifdef  RS_DEBUG_CONSOLE
    OUTPUT (idText, text, "too few trickot found, so asume as noise");
    OUTPUT (idText, text, "");
#endif

    result |= MSHPPF::fewTrickot;
  }

  if ((candidateColor == red) && (colorSpace.colorCount[white] > 0.5))
  { //too much white, so asume something was seen on the wall
#ifdef  RS_DEBUG_CONSOLE
    OUTPUT (idText, text, "too much white, so asume something was seen on the wall");
    OUTPUT (idText, text, "");
#endif

    result |= MSHPPF::tooMuchWhite;
  }

  if (colorSpace.colorCount[green] > 0.5)
  { //too much green, so asume a ball was seen on the field
#ifdef  RS_DEBUG_CONSOLE
    OUTPUT (idText, text, "too much green, so asume a ball was seen on the field");
    OUTPUT (idText, text, "");
#endif

    result |= MSHPPF::tooMuchGreen;
  }

  if (colorSpace.colorCount[skyblue] > 0.1)
  { //too much skyblue, so it seems to be a goal, or landmark
#ifdef  RS_DEBUG_CONSOLE
    OUTPUT (idText, text, "too much skyblue, so it seems to be a goal, or landmark");
    OUTPUT (idText, text, "");
#endif

    result |= MSHPPF::tooMuchSkyBlue;
  }

  if (colorSpace.colorCount[orange] > 0.2)
  { //too much orange, so it seems to be a ball
#ifdef  RS_DEBUG_CONSOLE
    OUTPUT (idText, text, "too much orange, so it seems to be a ball");
    OUTPUT (idText, text, "");
#endif

    result |= MSHPPF::tooMuchOrange;
  }

  if (colorSpace.dY < 35)
  { //too few brightness changes
#ifdef  RS_DEBUG_CONSOLE
    OUTPUT (idText, text, "too few brightness changes");
    OUTPUT (idText, text, "");
#endif

    result |= MSHPPF::fewBrightnessChange;
  }

  return result;
}

void VLCRobotSpecialist::addPercept (PerceptInfo &perceptInfo)
{
  Vector2<int>  fieldPosition;

  MSHSinglePlayerPercept percept;

  percept.flags = perceptInfo.flags;
  percept.lines = perceptInfo.lines;

  //calculate positions from footpoints relative to own robot
  Geometry::calculatePointOnField(perceptInfo.position.x, perceptInfo.position.y, cameraMatrix, CameraInfo::CameraInfo (), fieldPosition);

  /* translate from robot to playing field space*/
  Pose2D pose = Pose2D (fieldPosition);

  Pose2D poseOnField = robotPose + pose;

  percept.offset = poseOnField.translation;

  /* filtering should be done in playerslocator */
  if (
    (percept.offset.x > xPosOpponentGoal) || 
    (percept.offset.x < -xPosOpponentGoal) || 
    (percept.offset.y > yPosLeftSideline) || 
    (percept.offset.y < -yPosLeftSideline)
    )
  {
    //percept is outside
    percept.flags |= MSHPPF::outOfField;

#ifdef  RS_DEBUG_CONSOLE
    OUTPUT (idText, text, "");
    OUTPUT (idText, text, "percept outside field");
    OUTPUT (idText, text, "");
#endif
  }

  percept.direction = robotPose.getAngle () + perceptInfo.direction;
  percept.validity = perceptInfo.validity;


#ifdef  RS_DEBUG_CONSOLE
  OUTPUT (idText, text, "robot at position : " << fieldPosition.x << " , " << fieldPosition.y);
  OUTPUT (idText, text, "distance : " << fieldPosition.abs ());
  OUTPUT (idText, text, "");
#endif

#ifdef  RS_DEBUG
  ARROW (percepts_robot, percept.offset.x, percept.offset.y, percept.offset.x + cos (percept.direction) * 200, percept.offset.y - sin (percept.direction) * 200, 1, Drawings::ps_solid, Drawings::black);
#endif

  if (perceptInfo.color == blue)
  {
    playersPercept.addBluePlayer (percept);

#ifdef  RS_DEBUG


#ifdef  RS_DEBUG_SHOW_BOUNDINGBOX
    RECTANGLE (imageProcessor_robot_detection, perceptInfo.minBounding.x, perceptInfo.minBounding.y, perceptInfo.maxBounding.x, perceptInfo.maxBounding.y, 1, Drawings::ps_dot, Drawings::blue);
#endif

    perceptInfo.gradient = 0;
  
    if (perceptInfo.bottomLine.x != 0)
      perceptInfo.gradient = (((double) perceptInfo.bottomLine.y) / ((double) perceptInfo.bottomLine.x)) / ((double)perceptInfo.count) * 2.0;

    Vector2<int>  start;
    Vector2<int>  stop;

    start.x = perceptInfo.minBounding.x;
    start.y = perceptInfo.position.y + ((int)((perceptInfo.minBounding.x - perceptInfo.position.x) * perceptInfo.gradient));

    stop.x = perceptInfo.maxBounding.x;
    stop.y = perceptInfo.position.y + ((int)((perceptInfo.maxBounding.x - perceptInfo.position.x) * perceptInfo.gradient));

#ifdef RS_DEBUG_SHOW_PERCEPT
    LINE (imageProcessor_robot_detection, start.x, start.y, stop.x, stop.y, 1, Drawings::ps_dot, Drawings::blue);

    CIRCLE (imageProcessor_robot_detection, perceptInfo.position.x, perceptInfo.position.y, 0, 1, Drawings::ps_solid, Drawings::blue);
    CIRCLE (percepts_robot, percept.offset.x, percept.offset.y, 50, 10, Drawings::ps_solid, Drawings::blue);
#endif
#endif
  }
  else
  {
    playersPercept.addRedPlayer (percept);

#ifdef  RS_DEBUG
#ifdef  RS_DEBUG_SHOW_BOUNDINGBOX
    RECTANGLE (imageProcessor_robot_detection, perceptInfo.minBounding.x, perceptInfo.minBounding.y, perceptInfo.maxBounding.x, perceptInfo.maxBounding.y, 1, Drawings::ps_dot, Drawings::red);
#endif

    perceptInfo.gradient = 0;
  
    if (perceptInfo.bottomLine.x != 0)
      perceptInfo.gradient = (((double) perceptInfo.bottomLine.y) / ((double) perceptInfo.bottomLine.x)) / ((double)perceptInfo.count) * 2.0;

    Vector2<int>  start;
    Vector2<int>  stop;

    start.x = perceptInfo.minBounding.x;
    start.y = perceptInfo.position.y + ((int)((perceptInfo.minBounding.x - perceptInfo.position.x) * perceptInfo.gradient));

    stop.x = perceptInfo.maxBounding.x;
    stop.y = perceptInfo.position.y + ((int)((perceptInfo.maxBounding.x - perceptInfo.position.x) * perceptInfo.gradient));

#ifdef RS_DEBUG_SHOW_PERCEPT
    LINE (imageProcessor_robot_detection, start.x, start.y, stop.x, stop.y, 1, Drawings::ps_dot, Drawings::red);

    CIRCLE (imageProcessor_robot_detection, perceptInfo.position.x, perceptInfo.position.y, 0, 1, Drawings::ps_solid, Drawings::orange);
    CIRCLE (percepts_robot, percept.offset.x, percept.offset.y, 50, 10, Drawings::ps_solid, Drawings::red);
#endif
#endif
  };  

  return;
};


void VLCRobotSpecialist::dbg_PrintGreenchanges (void)
{
  List<GreenChangeEvent>::Pos iter = greenChangeEvents.getFirst ();

  int i = 0;
  while (iter)
  {
    CRASH_CHECK(i); i++;
    GreenChangeEvent event = greenChangeEvents[iter];

    if (event.startEvent)
    {
      CIRCLE (imageProcessor_robot_detection, event.positionOnImage.x, event.positionOnImage.y, 2, 1, Drawings::ps_dot, Drawings::gray);
    }
    else
    {
      CIRCLE (imageProcessor_robot_detection, event.positionOnImage.x, event.positionOnImage.y, 2, 1, Drawings::ps_dot, Drawings::dark_gray);
    }

    iter++;
  };
};

int VLCRobotSpecialist::distanceOnField (Vector2<int> vec1, Vector2<int> vec2, CameraMatrix cameraMatrix, CameraInfo cameraInfo)
{
  Vector2<int>  pointOnField1, pointOnField2;

  Geometry::calculatePointOnField (vec1.x, vec1.y, cameraMatrix, cameraInfo, pointOnField1);

  Geometry::calculatePointOnField (vec2.x, vec2.y, cameraMatrix, cameraInfo, pointOnField2);

  pointOnField2 -= pointOnField1;

  return pointOnField2.abs ();
}

int VLCRobotSpecialist::generateBoundings (CandidateCluster cluster, VLCRectangle &body, VLCRectangle &foot, Vector2<int> &footAVG)
{
  RobotCandidate  candidate;

  int       element = 0;
  int       count = 0;

  //init values
  body.x1 = 1024; body.x2 = 0; body.y1 = 1024; body.y2 = 0;
  foot.x1 = 1024; foot.x2 = 0; foot.y1 = 1024; foot.y2 = 0;

  //do the stuff
  for (element = (int) cluster.startPosition; element <= (int)cluster.stopPosition; element++)
  {
    CRASH_CHECK(element);

    candidate = candidates[element];

    if (body.x2 < candidate.candidateStart.x)
      body.x2 = candidate.candidateStart.x;

    if (body.x1 > candidate.candidateStart.x)
      body.x1 = candidate.candidateStart.x;

    if (body.y2 < candidate.candidateStart.y)
      body.y2 = candidate.candidateStart.y;

    if (body.y1 > candidate.candidateStart.y)
      body.y1 = candidate.candidateStart.y;

    if ((candidate.footPoint.x != -1) &&
      (candidate.footPoint.y != -1)
      )
    {
      if (foot.x2 < candidate.footPoint.x)
        foot.x2 = candidate.footPoint.x;

      if (foot.x1 > candidate.footPoint.x)
        foot.x1 = candidate.footPoint.x;

      if (foot.y2 < candidate.footPoint.y)
        foot.y2 = candidate.footPoint.y;

      if (foot.y1 > candidate.footPoint.y)
        foot.y1 = candidate.footPoint.y;

      footAVG.x += candidate.footPoint.x;
      footAVG.y += candidate.footPoint.y;
    };

    count++;
  };

  return count;
}

#undef min
#undef max

---