Modules/ImageProcessor/GT2005ImageProcessor/GT2005CenterCircleFinder.cpp

Go to the documentation of this file.

#include "GT2005CenterCircleFinder.h"
//#define DEBUG_CENTER_CIRCLE_WITH_LOGS


GT2005CenterCircleFinder::GT2005CenterCircleFinder(const ColorCorrector& cCorrector, const ColorTable& cTable):colorCorrector(cCorrector),                                                                                                                    colorTable(cTable)
{
  timeTillLastSeen = 100;
  circleFound = false;
  numberOfCandidates = 0;
  houghStepX = areaOfInterestX/houghDimensionX;
  houghStepY = areaOfInterestY/houghDimensionY;
  maxQuantizationErrorX = houghStepX/2;
  maxQuantizationErrorY = houghStepY/2;
  lineThicknessRateAllowed = 5;
  for(int i = 0; i < houghDimensionX; i++)
  {
    for(int j = 0; j < houghDimensionY; j++)
    {
      houghSpace[i][j] = 0;
      flagY[j] = false;
    }
  }
}

void GT2005CenterCircleFinder::reset()
{
  timeTillLastSeen++;
  circleFound = false;
  numberOfCandidates = 0;
  for(int i = 0; i < houghDimensionX; i++)
  {
    for(int j = 0; j < houghDimensionY; j++)
    {
      houghSpace[i][j] = 0;
      flagY[j] = false;
    }
  }
}

void GT2005CenterCircleFinder::addCandidate(Vector2<int>& candiOnField, Vector2<int>& candiOfImage)
{
   circleFound = false;
   //only candidates, which are inside the area of interest, will be added to the array candidatePoints
  if((candiOnField.x < areaOfInterestX/2) && (candiOnField.x > -areaOfInterestX/2) && 
     (candiOnField.y < areaOfInterestY/2) && (candiOnField.y > -areaOfInterestY/2))
  {
   if(numberOfCandidates < maxNumberOfCandidatesPossible)
   {
       candidatePoints[numberOfCandidates].pointOnField.x = double(candiOnField.x);
       candidatePoints[numberOfCandidates].pointOnField.y = double(candiOnField.y);
       candidatePoints[numberOfCandidates].pointOfImage = candiOfImage;
       numberOfCandidates++;
   }//if(numberOfCandidates <2 maxNumber)
   else
   {
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
      OUTPUT(idText,text,"erhöhe maxNumberOfCandidatesPossible");
#endif
   }
  }/*if((candiOnField.x < areaOfInterstX/2) && (candiOnField.x > -areaOfInterestX/2) && 
        (candiOnField.y < areaOfInterstY/2) && (candiOnField.y > -areaOfInterestY/2) */
}


 
void GT2005CenterCircleFinder::transformArrayToField(int arrayIndex, bool xDirection, int& fieldPosition)
{
  int border;
  int step;
  int dimension;
  if(xDirection)
  {
    border = areaOfInterestX;
    step = houghStepX;
    dimension = houghDimensionX;
  }
  else
  {
    border = areaOfInterestY;
    step = houghStepY;
    dimension = houghDimensionY;
  }
  /* the two borders of the area of interest is x/y = border/2 and x/y= -(border/2). it´s origin (houghSpace[0][0]) is 
   * positioned at x/y =  border/2.  
   * if the arrayIndex is a number between 0 and dimension/2, the field-coordinate is positiv and will be calculated as follows:
   * first, the quantized value for the arrayIndex 0 will be calculated (border/2 - step/2). from that value on, the other quantized value
   * can be calculated through substracting a factor of step 
   *
   * if the arrayIndex is a number between dimension/2 and dimension, the field-coordinate is negativ:
   * starting from the middle of the area of interest, stepping to the outer negative border, the quantized value for arrayIndex dimension/2
   * is -step/2. other quantized values through substracting a factor of step. since one start from the middle of the area of interest,       
   * dimension/2 has to be substracted from arrayIndex
   */
  if(arrayIndex < (dimension/2))
  {
    fieldPosition = border/2 - step/2 - arrayIndex * step; 
  }
  else if((arrayIndex >= (dimension/2)) && (arrayIndex < dimension))
  {
    fieldPosition = - step/2 - (arrayIndex-dimension/2) * step;
  }
}


void GT2005CenterCircleFinder::transformFieldToArray(double fieldPos, bool xDirection, int& arrayIndex)
{
  int border;
  int step;
  int dimension;
  if(xDirection)
  {
    border = areaOfInterestX;
    step = houghStepX;
    dimension = houghDimensionX;
  }
  else
  {
    border = areaOfInterestY;
    step = houghStepY;
    dimension = houghDimensionY;
  }
  
 /* the two borders of the area of interest is x/y = border/2 and x/y= -(border/2). it´s origin (houghSpace[0][0]) is 
  * positioned at x/y =  border/2.
  * if the field-coordinate is a number between 0 and border/2, then the arrayIndex will be a number between 0 and dimension/2:
  * first of all the number (field-coord div step) will be calculated. if the number equals to dimension/2-1, then the corresponding array-index
  * is 0 and if if the number equals to 0, then the corresponding array-index is dimension/2-1. so (field-coord div step) has to be 
  * substracted from (dimension/2-1)
  *
  * if the field-coordinate is a number between 0 and -border/2, then the arrayIndex will be number between dimension/2 and dimension
  * same as above, the number (field-coord div step) will be calculated. if the number equals to dimension/2-1, then the corresponding 
  * array-index is dimension-1, and if the number equals to 0, then the corresponding array-index is dimension/2. so dimension/2 has to be 
  * added to (field-coord div step).
  */
  if((fieldPos >= 0) && (fieldPos < (border/2)))
  {
    arrayIndex = (dimension/2 - 1) - (int(fieldPos) / step);
  }
  else if((fieldPos < 0) && (fieldPos > -(border/2)))
  {
    fieldPos *= -1;
    arrayIndex = (int(fieldPos) / step) + (dimension/2);
  }

}


void GT2005CenterCircleFinder::determineCirclePoint(const CameraMatrix& cameraMatrix, const Image& image, const RobotPose& robotPose,                                                               const CameraMatrix& prevCameraMatrix)
 {
   //Hough Transformation,
   //(x - xc)^2 + (y - yc)^2 = r^2
   int xc = 0;
   int xc1 = 0; //needed, because the sqrt of a value has got two results
   int yc = 0;

   //variables to determine the max entry
   int maxEntry = 0; //max value that occur (it is possible that more than one cell has got his number of entries)
   int absMaxEntry = 0; //the absolute maximum value (only one hough space cell has got this number of entries)
   bool existAbsMaxEntry = false; //does an absolute max value exist?
   Vector2<double> absMaxFieldCoord(0,0);
   multipleCandidate maxEntryFieldCoord[maxNumberOfCandidatesMaxEntry];
   int NumberOfEntriesMaxEntry = 0;
   

   //fill HoughSpace(2-dimensional)
   for(int i = 0; i < numberOfCandidates; i++)
   {
       for(int y = 0; y < houghDimensionY; y++)
       {
         transformArrayToField(y,false,yc);
         double help = double(sqr(radius) - sqr(candidatePoints[i].pointOnField.y - yc)); // help = (x - xc)^2
         if(help > 0)
         {
            help = sqrt(help); // help = x - xc
            xc = int(candidatePoints[i].pointOnField.x - help);
            xc1 = int(help + candidatePoints[i].pointOnField.x);

            int arrayIndex;
            if((xc < (areaOfInterestX/2)) && (xc > (-areaOfInterestX/2)))
            {
              transformFieldToArray(xc,true,arrayIndex),
              houghSpace[arrayIndex][y] += 1;
              flagY[y] = true;
              //determine maxEntry and absMaxEntry
              if(houghSpace[arrayIndex][y] > maxEntry)
              {
                NumberOfEntriesMaxEntry = 1; //skip 0, becausevone new entry will be inserted in this loop
                if(houghSpace[arrayIndex][y] > absMaxEntry)
                {
                  maxEntry = absMaxEntry;
                  maxEntryFieldCoord[0].pointOnField = absMaxFieldCoord;
                  maxEntryFieldCoord[0].count = 0;
                  maxEntryFieldCoord[0].avgDistance = 0;
                  absMaxEntry = houghSpace[arrayIndex][y];
                  existAbsMaxEntry = true;
                  absMaxFieldCoord = Vector2<double>(xc,yc);
                }
                else if(houghSpace[arrayIndex][y] == absMaxEntry)
                {
                  existAbsMaxEntry = false;
                  maxEntry = absMaxEntry;
                  maxEntryFieldCoord[0].pointOnField = absMaxFieldCoord;
                  maxEntryFieldCoord[0].count = 0;
                  maxEntryFieldCoord[0].avgDistance = 0;
                }
                else if(houghSpace[arrayIndex][y] < absMaxEntry)
                {
                  maxEntry = houghSpace[arrayIndex][y];
                  maxEntryFieldCoord[0].pointOnField = Vector2<double>(xc, yc);
                  maxEntryFieldCoord[0].count = 0;
                  maxEntryFieldCoord[0].avgDistance = 0;
                }
              }//if(houghSpace[arrayIndex][y] > maxEntry)
              else if(houghSpace[arrayIndex][y] == maxEntry)
              {
                if(NumberOfEntriesMaxEntry < maxNumberOfCandidatesMaxEntry)
                {
                  maxEntryFieldCoord[NumberOfEntriesMaxEntry].pointOnField = Vector2<double>(xc, yc);
                  maxEntryFieldCoord[NumberOfEntriesMaxEntry].count = 0;
                  maxEntryFieldCoord[NumberOfEntriesMaxEntry].avgDistance = 0;
                  NumberOfEntriesMaxEntry++;
                }
                else
                {
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
                  OUTPUT(idText,text,"erhöhe maxNumberOfCandidatesMaxEntry"); 
#endif 
                }
              }//else if(houghSpace[arrayIndex][y] == maxEntry)
            }//if((xc < (areaOfInterestX/2)) && (xc > (-areaOfInterestX/2)))

            if((xc1 < (areaOfInterestX/2)) && (xc1 > (-areaOfInterestX/2)))
            {
              transformFieldToArray(xc1,true,arrayIndex),
              houghSpace[arrayIndex][y] += 1;
              flagY[y] = true;
              //determine maxEntry and absMaxEntry
              if(houghSpace[arrayIndex][y] > maxEntry)
              {
                NumberOfEntriesMaxEntry = 1;
                if(houghSpace[arrayIndex][y] > absMaxEntry)
                {
                  maxEntry = absMaxEntry;
                  maxEntryFieldCoord[0].pointOnField = absMaxFieldCoord;
                  maxEntryFieldCoord[0].count = 0;
                  maxEntryFieldCoord[0].avgDistance = 0;
                  absMaxEntry = houghSpace[arrayIndex][y];
                  existAbsMaxEntry = true;
                  absMaxFieldCoord = Vector2<double>(xc1,yc);
                }
                else if(houghSpace[arrayIndex][y] == absMaxEntry)
                {
                  existAbsMaxEntry = false;
                  maxEntry = absMaxEntry;
                  maxEntryFieldCoord[0].pointOnField = absMaxFieldCoord;
                  maxEntryFieldCoord[0].count = 0;
                  maxEntryFieldCoord[0].avgDistance = 0;
                }
                else if(houghSpace[arrayIndex][y] < absMaxEntry)
                {
                  maxEntry = houghSpace[arrayIndex][y];
                  maxEntryFieldCoord[0].pointOnField = Vector2<double>(xc1, yc);
                  maxEntryFieldCoord[0].count = 0;
                  maxEntryFieldCoord[0].avgDistance = 0;
                }
              }//if(houghSpace[arrayIndex][y] > maxEntry)
              else if(houghSpace[arrayIndex][y] == maxEntry)
              {
                if(NumberOfEntriesMaxEntry < maxNumberOfCandidatesMaxEntry)
                {
                  maxEntryFieldCoord[NumberOfEntriesMaxEntry].pointOnField = Vector2<double>(xc1, yc);
                  maxEntryFieldCoord[NumberOfEntriesMaxEntry].count = 0;
                  NumberOfEntriesMaxEntry++;
                }
                else
                {
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
                  OUTPUT(idText,text,"erhöhe maxNumberOfCandidatesMaxEntry"); 
#endif
                }
              }//else if(houghSpace[arrayIndex][y] == maxEntry)
            }//if((xc1 < (areaOfInterestX/2)) && (xc1 > (-areaOfInterestX/2)))
         }//if(help1 > 0)
 
       }//for(int y = 0; y < houghDimension; y++)

   }//for(int i = 0; i < numberOfCandidates; i++)
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
   OUTPUT(idText,text,"maxEntry, abs: " << maxEntry << ", " << absMaxEntry << ", " << existAbsMaxEntry);
#endif

   //evaluation
   if(maxEntry >= mEntry)
   {
     if(existAbsMaxEntry)
     {
       checkCircleMidpoint(absMaxFieldCoord, absMaxEntry, cameraMatrix, image, robotPose, prevCameraMatrix);                         }
     else 
     {
       checkCircleMidpoint2(maxEntry, NumberOfEntriesMaxEntry, maxEntryFieldCoord, cameraMatrix, image, robotPose, prevCameraMatrix);                                      
     }//else 
    }//if((existMaxEntry)&&(absMaxEntry >= aMaxEntry) && (absMaxEntry < max))

    if(!circleFound && timeTillLastSeen < 60)
    {
      Drawings::Color c;
      if (timeTillLastSeen<2)
      {
        c = Drawings::yellow;
      }
      else if (timeTillLastSeen<10)
      {
        c = Drawings::yellowOrange;
      }
      else if (timeTillLastSeen<30)
      {
        c = Drawings::orange;
      }
      else
      {
        c = Drawings::red;
      }
      CIRCLE(circlePoints_Field, midPointLastSeenField.x, midPointLastSeenField.y, 180, 3,Drawings::ps_solid, c)
    }
 }



 bool GT2005CenterCircleFinder::getCircle(Vector2<double>& mid)
 {
   if(circleFound)
   {
     mid = midPoint;
     return(true);
   }
   else
     return(false);
 }



 void GT2005CenterCircleFinder::checkCircleMidpoint(Vector2<double> candidate, int absMaxEntry, const CameraMatrix& cameraMatrix,                                                                  const Image& image, const RobotPose& robotPose, const CameraMatrix& prevCameraMatrix)
 {
   bool circleMidPoint = false;
   Vector2<int> candidateInImage;
   Vector2<int> candidateOnField(int(candidate.x), int(candidate.y));
   Geometry::calculatePointInImage(candidateOnField,cameraMatrix,image.cameraInfo,candidateInImage);
   if(absMaxEntry >= enoughPros)
     circleMidPoint = true;
   else
   {
     int count = 0;
     double distance = 0;
     double maxQuantizationError = max(maxQuantizationErrorX,maxQuantizationErrorY);
     for(int i = 0; i < numberOfCandidates; i++)
     {
       distance = sqrt(sqr(candidatePoints[i].pointOnField.x - candidate.x) + sqr(candidatePoints[i].pointOnField.y - candidate.y));
       if((distance > (radius - (maxQuantizationError*2))) && (distance < (radius + (maxQuantizationError*2))))
       {
         count++;
       }
     }
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
   OUTPUT(idText,text,"midpointtest1: " << count);
#endif
     if((count >= mEntry) && (count < enoughPros))
     {
       // scan in four direction and look for white
       circleMidPoint = scanSurrounding(candidateInImage, candidate, cameraMatrix, image, robotPose, prevCameraMatrix);
     }
     else if(count >= enoughPros)
       circleMidPoint = true;
   }//else
   
 /*  if(circleMidPoint == true)
   {
     midPoint = candidate;
     circleFound = true;
    
     //drawings of the mid-point
     Vector2<double> fieldCoord = Geometry::relative2FieldCoord(robotPose, candidate.x, candidate.y);
     CIRCLE(circlePoints_Field, fieldCoord.x, fieldCoord.y, 150, 30,Drawings::ps_dash, Drawings::black);
     //CIRCLE(circlePoints_Field, candidate.x, candidate.y, 150, 30,Drawings::ps_dash, Drawings::black);
     CIRCLE(circlePoints_image, candidateInImage.x, candidateInImage.y, 3, 1,Drawings::ps_dash, Drawings::black);
   }
   */

   Vector2<double> fieldCoord = Geometry::relative2FieldCoord(robotPose, candidate.x, candidate.y);
   if(circleMidPoint == true)
   {
     midPoint = fieldCoord;
     circleFound = true;
     timeTillLastSeen = 0;
     CIRCLE(circlePoints_Field, fieldCoord.x, fieldCoord.y, 180, 3,Drawings::ps_solid, Drawings::pink);
     midPointLastSeenField = fieldCoord;
   }
   
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
   OUTPUT(idText, text, "");
   OUTPUT(idText, text, ""); 
#endif
 }



 void GT2005CenterCircleFinder::checkCircleMidpoint2(int maxEntry, int NumberOfEntriesMaxEntry,                                                                                                      multipleCandidate maxEntryFieldCoord[maxNumberOfCandidatesMaxEntry],                                                                           const CameraMatrix& cameraMatrix, const Image& image, const RobotPose& robotPose,                                                              const CameraMatrix& prevCameraMatrix)
 { 
   bool circleMidPoint = false;
   double distance = 0;
   int absMax = 0;
   bool existAbsMax = false;
   int position = 0;
   double minAvgDistance = 2000; //just for initialization
   double maxQuantizationError = max(maxQuantizationErrorX,maxQuantizationErrorY);
 
    
   if(maxEntry >= mEntry2)
   {
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
     OUTPUT(idText, text, "midpoint2: maxEntry > mEntry2");
#endif
     //calculate the average distance between the candidate
     for(int i = 0; i < NumberOfEntriesMaxEntry; i++)
     {
       for(int j = i + 1; j < NumberOfEntriesMaxEntry; j++)
       {
         distance = sqrt(sqr(maxEntryFieldCoord[i].pointOnField.x - maxEntryFieldCoord[j].pointOnField.x) +                                                              sqr(maxEntryFieldCoord[i].pointOnField.y - maxEntryFieldCoord[j].pointOnField.y));
         maxEntryFieldCoord[i].avgDistance += distance;
         maxEntryFieldCoord[j].avgDistance += distance;
       }//for(int j = 0; j < NumberOfEntriesMaxEntry;j++)
       maxEntryFieldCoord[i].avgDistance = maxEntryFieldCoord[i].avgDistance/(NumberOfEntriesMaxEntry - 1);
       if(maxEntryFieldCoord[i].avgDistance < minAvgDistance)
       {
         minAvgDistance = maxEntryFieldCoord[i].avgDistance;
         position = i;
       }
     }//for(int i = 0; i < NumberOfEntriesMaxEntry; i++)


    /* ///////LÖSCHEN!!!
     Vector2<int> candidateInImage;
     Vector2<int> candidateOnField((int)maxEntryFieldCoord[position].pointOnField.x,                                                                                               (int)maxEntryFieldCoord[position].pointOnField.y); 
     Geometry::calculatePointInImage(candidateOnField ,cameraMatrix,image.cameraInfo,candidateInImage); 
     CIRCLE(circlePoints_image, candidateInImage.x, candidateInImage.y, 3, 1,Drawings::ps_dash, Drawings::blue);
     ////LÖSCHEN!!!*/

#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
     OUTPUT(idText, text, "midpoint2: minAvgDistance = " << minAvgDistance);
#endif
     if(minAvgDistance <= acceptedAvgDistance)
     {
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
       OUTPUT(idText, text, "midpoint2: Cluster");
#endif
       circleMidPoint = true;
     }
    
   }//if(maxEntry > enoughPros)
   if(circleMidPoint == false)
   {
     for(int j = 0; j < NumberOfEntriesMaxEntry; j++)
     {
       for(int i = 0; i < numberOfCandidates; i++)
       {
         distance = sqrt(sqr(candidatePoints[i].pointOnField.x - maxEntryFieldCoord[j].pointOnField.x) +                                                               sqr(candidatePoints[i].pointOnField.y - maxEntryFieldCoord[j].pointOnField.y));  
         if((distance > (radius - (maxQuantizationError*2))) && (distance < (radius + (maxQuantizationError*2))))
         {
           maxEntryFieldCoord[j].count++;
         }
       }//for(int i = 0; i < numberOfCandidates; i++)

       if(maxEntryFieldCoord[j].count > absMax)
       {
         existAbsMax = true;
         absMax = maxEntryFieldCoord[j].count;
         position = j;
       }
       else if(maxEntryFieldCoord[j].count == absMax)
       {
         existAbsMax = false;
       }
     }//for(int j = 0; j < NumberOfEntriesMaxEntry; j++)

     Vector2<int> candidateInImage;
     Vector2<int> candidateOnField((int)maxEntryFieldCoord[position].pointOnField.x,                                                                                               (int)maxEntryFieldCoord[position].pointOnField.y); 
     Geometry::calculatePointInImage(candidateOnField ,cameraMatrix,image.cameraInfo,candidateInImage); 
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
     OUTPUT(idText, text, "midpoint2:    existAbsMax, absMax: " << existAbsMax << ", " << absMax);
#endif
     if(existAbsMax)
     {
       if(maxEntryFieldCoord[position].count >= enoughPros)          
         circleMidPoint = true;
       else if((maxEntryFieldCoord[position].count < enoughPros) && (maxEntryFieldCoord[position].count >= mEntry))
       {
         circleMidPoint = scanSurrounding(candidateInImage, maxEntryFieldCoord[position].pointOnField, cameraMatrix, image, robotPose,                                                   prevCameraMatrix);
       }
     }//if(existAbsMax)
     else if(absMax >= mEntry3)
     {
       for(int i = 0; (i < NumberOfEntriesMaxEntry) && (circleMidPoint == false); i++)
       {
         if(maxEntryFieldCoord[i].count == absMax)
         {
           candidateOnField = Vector2<int>((int)maxEntryFieldCoord[i].pointOnField.x, (int)maxEntryFieldCoord[i].pointOnField.y);
           Geometry::calculatePointInImage(candidateOnField ,cameraMatrix,image.cameraInfo,candidateInImage);
           circleMidPoint = scanSurrounding(candidateInImage, maxEntryFieldCoord[i].pointOnField, cameraMatrix, image, robotPose,                                                          prevCameraMatrix);
           if(circleMidPoint)
           {
             position = i;
           }
         }//if(maxEntryFieldCoord[i].count = absMax)
       }//for(int i = 0; (i < NumberOfEntriesMaxEntry) && (circleMidPoint == false); i++)
     }//else if(absMax >= mEntry3)
   }// if(circleMidPoint == false)

 /*   if(circleMidPoint == true)
   {
     midPoint = maxEntryFieldCoord[position].pointOnField;
     circleFound = true;
       
     //drawings of the mid-point
     Vector2<double> fieldCoord = Geometry::relative2FieldCoord(robotPose, maxEntryFieldCoord[position].pointOnField.x,                                                                                                    maxEntryFieldCoord[position].pointOnField.y);
     Vector2<int> candidateInImage; 
     Vector2<int> candidateOnField((int)maxEntryFieldCoord[position].pointOnField.x,                                                                                               (int)maxEntryFieldCoord[position].pointOnField.y);
     Geometry::calculatePointInImage(candidateOnField ,cameraMatrix,image.cameraInfo,candidateInImage);
     CIRCLE(circlePoints_Field, fieldCoord.x, fieldCoord.y, 150, 30,Drawings::ps_dash, Drawings::yellowOrange);
     CIRCLE(circlePoints_image, candidateInImage.x, candidateInImage.y, 3, 1,Drawings::ps_dash, Drawings::yellowOrange);
   }*/
 
    Vector2<double> fieldCoord = Geometry::relative2FieldCoord(robotPose, maxEntryFieldCoord[position].pointOnField.x,                                                                                                    maxEntryFieldCoord[position].pointOnField.y);
    Vector2<int> candidateInImage; 
    Vector2<int> candidateOnField((int)maxEntryFieldCoord[position].pointOnField.x,                                                                                               (int)maxEntryFieldCoord[position].pointOnField.y);
    Geometry::calculatePointInImage(candidateOnField ,cameraMatrix,image.cameraInfo,candidateInImage);
   if(circleMidPoint == true)
   {
     timeTillLastSeen = 0;
     midPoint = fieldCoord;
     circleFound = true;
     CIRCLE(circlePoints_Field, fieldCoord.x, fieldCoord.y, 180, 3,Drawings::ps_solid, Drawings::pink);
     midPointLastSeenField = fieldCoord;
   } 
   

#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    OUTPUT(idText, text, "");
    OUTPUT(idText, text, ""); 
#endif
 }  


bool GT2005CenterCircleFinder::scanSurrounding(Vector2<int> start, Vector2<double> startOnField, const CameraMatrix& cameraMatrix,                                                            const Image& image, const RobotPose& robotPose, const CameraMatrix& prevCameraMatrix)
{
  int positiveInNumberOfDirection = 0;
  Vector2<int> current;
  Vector2<int> end;
  Vector2<int> step;
  int difference = 0;
  bool currentWasSet = false;
   bool positiv = false;
  double distance = 5000;
  double positivDistance = 0;
  int numberOfPositivDistance = 0;
  double distanceArray[numberOfScanLines];
  int numberOfEntriesDistanceArray = 0;


  //North
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
  OUTPUT(idText, text,"");
  OUTPUT(idText, text, "NORTH:");
#endif
  
  if((start.x >= 0) && (start.x <= image.cameraInfo.resolutionWidth) && (start.y > 0))
  {
    if(start.y > image.cameraInfo.resolutionHeight)
    {
      if((start.y - scanLength) < image.cameraInfo.resolutionHeight)
      {
        current = Vector2<int>(start.x, image.cameraInfo.resolutionHeight);
        currentWasSet = true;
      }
    }
    else
    {
      current = start;
      currentWasSet = true;
    }//else
    if(currentWasSet)
    {
      if((start.y - scanLength) < 0)
        end = Vector2<int> (start.x,0);
      else
        end = Vector2<int> (start.x, start.y - scanLength);

       step = Vector2<int>(0,-1);
       scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix, distance, positiv);
       if(positiv)
       {
         positivDistance = max(distance,positivDistance);
         numberOfPositivDistance++;
       }
       else
       {
         distanceArray[numberOfEntriesDistanceArray] = distance;
         numberOfEntriesDistanceArray++;
       }
    }
  }//if((start.x >= 0) && (start.x <= image.cameraInfo.resolutionWidth) && (start.y > 0))

  
 
    //South
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    OUTPUT(idText, text,"");
    OUTPUT(idText, text, "SOUTH:");
#endif
    currentWasSet = false;
    positiv = false;
    distance = 5000;
  
    if((start.x >= 0) && (start.x <= image.cameraInfo.resolutionWidth) && (start.y < image.cameraInfo.resolutionHeight))
    {
      if(start.y < 0)
      {
        if((start.y + scanLength) > 0)
        {
          current = Vector2<int>(start.x, 0);
          currentWasSet = true;
        }
      }
      else
      {
        current = start;
        currentWasSet = true;
      }
      if(currentWasSet)
      {
        if((start.y + scanLength) > image.cameraInfo.resolutionHeight)
         end = Vector2<int> (start.x,image.cameraInfo.resolutionHeight);
        else
         end = Vector2<int> (start.x, start.y + scanLength);  
        step = Vector2<int> (0,1);
        scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix, distance,positiv);
        if(positiv)
       {
         positivDistance = max(distance,positivDistance);
         numberOfPositivDistance++;
       }
       else
       {
         distanceArray[numberOfEntriesDistanceArray] = distance;
         numberOfEntriesDistanceArray++;
       }
      }
    }//if((start.x >= 0) && (start.x <= image.cameraInfo.resolutionWidth) && (start.y <= image.cameraInfo.resolutionHeight))
  
  //West
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    OUTPUT(idText, text,"");
    OUTPUT(idText, text, "WEST:");
#endif
    currentWasSet = false;
    positiv = false;
    distance = 5000;
  
    if((start.y  >= 0) && (start.y <= image.cameraInfo.resolutionHeight) && (start.x > 0))
    {
      if(start.x > image.cameraInfo.resolutionWidth)
      {
        if((start.x - scanLength) < image.cameraInfo.resolutionWidth)
        {
          current = Vector2<int>(image.cameraInfo.resolutionWidth, start.y);
          currentWasSet = true;
        }
      }
      else
      {
        current = start;
        currentWasSet = true;
      }
      if(currentWasSet)
      {
        if((start.x - scanLength) < 0)
         end = Vector2<int> (0, start.y);
        else
         end = Vector2<int> (start.x - scanLength, start.y);  
        step = Vector2<int> (-1,0);
        scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix, distance,positiv);
        if(positiv)
       {
         positivDistance = max(distance,positivDistance);
         numberOfPositivDistance++;
       }
       else
       {
         distanceArray[numberOfEntriesDistanceArray] = distance;
         numberOfEntriesDistanceArray++;
       }
      }
    }//if((start.y  >= 0) && (start.y <= image.cameraInfo.resolutionHeight) && (start.x > 0)

  
  //East
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    OUTPUT(idText, text,"");
    OUTPUT(idText, text, "EAST:");
#endif
    currentWasSet = false;
    positiv = false;
    distance = 5000;

  
    if((start.y  >= 0) && (start.y <= image.cameraInfo.resolutionHeight) && (start.x < image.cameraInfo.resolutionWidth))
    {
      if(start.x < 0)
      {
        if((start.x + scanLength) > 0)
        {
          current = Vector2<int>(0, start.y);
          currentWasSet = true;
        }
      }
      else
      {
        current = start;
        currentWasSet = true;
      }
      if(currentWasSet)
      {
        if((start.x + scanLength) > image.cameraInfo.resolutionWidth)
          end = Vector2<int> (image.cameraInfo.resolutionWidth, start.y);
        else
          end = Vector2<int> (start.x + scanLength, start.y); 
       step = Vector2<int> (1,0);
       scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix, distance, positiv);
       if(positiv)
       {
         positivDistance = max(distance,positivDistance);
         numberOfPositivDistance++;
       }
       else
       {
         distanceArray[numberOfEntriesDistanceArray] = distance;
         numberOfEntriesDistanceArray++;
       }
      }
    }//if((start.y  >= 0) && (start.y <= image.cameraInfo.resolutionHeight) && (start.x < image.cameraInfo.resolutionWidth))
   

  //NORTHWEST
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    OUTPUT(idText, text,"");
    OUTPUT(idText, text, "NORTHWEST:");
#endif
    currentWasSet = false;
    positiv = false;
    distance = 5000;
  
    if((start.x > 0) && (start.y > 0))
    {
    if((start.y > image.cameraInfo.resolutionHeight) && (start.x <= image.cameraInfo.resolutionWidth))
      {
        if(start.y - scanLength < image.cameraInfo.resolutionHeight) 
        {
          difference = start.y - image.cameraInfo.resolutionHeight;
          if((start.x - difference > 0) && (start.x - difference <= image.cameraInfo.resolutionWidth))
          {
            current = Vector2<int>(start.x - difference, image.cameraInfo.resolutionHeight);
            currentWasSet = true;
          }
        }   
      }
      else if((start.x > image.cameraInfo.resolutionWidth) && (start.y <= image.cameraInfo.resolutionHeight))
      {
        if(start.x - scanLength < image.cameraInfo.resolutionWidth)
        {
          difference = start.x - image.cameraInfo.resolutionWidth;
          if((start.y - difference > 0) && (start.y - difference <= image.cameraInfo.resolutionHeight))
          {
            current = Vector2<int>(image.cameraInfo.resolutionWidth, start.y - difference);
            currentWasSet = true;
          }
        } 
      }
      else if((start.y > image.cameraInfo.resolutionHeight) && (start.x > image.cameraInfo.resolutionWidth))
      {
        if(start.y - image.cameraInfo.resolutionHeight == start.x - image.cameraInfo.resolutionWidth)
        {
          if((start.y - scanLength < image.cameraInfo.resolutionHeight) && (start.x - scanLength < image.cameraInfo.resolutionWidth))
          {
            current = Vector2<int>(image.cameraInfo.resolutionWidth, image.cameraInfo.resolutionHeight);
            currentWasSet = true;
          }
        }
        else if(start.y - image.cameraInfo.resolutionHeight < start.x - image.cameraInfo.resolutionWidth)
        {
          if(start.x - scanLength < image.cameraInfo.resolutionWidth)
          {
            difference = start.x - image.cameraInfo.resolutionWidth;
            if((start.y - difference > 0) && (start.y - difference < image.cameraInfo.resolutionHeight))
            {
              current = Vector2<int>(image.cameraInfo.resolutionWidth, start.y - difference);
              currentWasSet = true;
            }
          }
        }
        else if(start.y - image.cameraInfo.resolutionHeight > start.x - image.cameraInfo.resolutionWidth)
        {
          if(start.y - scanLength < image.cameraInfo.resolutionHeight) 
          {
            difference = start.y - image.cameraInfo.resolutionHeight;
            if((start.x - difference > 0) && (start.x - difference < image.cameraInfo.resolutionWidth))
            {
              current = Vector2<int>(start.x - difference, image.cameraInfo.resolutionHeight);
              currentWasSet = true;
            }
          }    
        }
      }
      else if((start.y <= image.cameraInfo.resolutionHeight) && (start.x <= image.cameraInfo.resolutionWidth))
      {
        current = start;
        currentWasSet = true;
      }
      if(currentWasSet)
      {
        if((start.y - scanLength < 0) && (start.x - scanLength >= 0))
        {
          difference = abs(start.y - scanLength);
          end = Vector2<int>(start.x - scanLength + difference,0);
        }
        else if((start.y - scanLength < 0) && (start.x - scanLength < 0))
        {
          if(abs(start.x - scanLength) == abs(start.y - scanLength))
          {
            end = Vector2<int>(0,0);
          }
          else if(abs(start.x - scanLength) < abs(start.y - scanLength))
          {
            difference = abs(start.y - scanLength);
            end = Vector2<int>(start.x - scanLength + difference, 0);
          }
          else if(abs(start.x - scanLength) > abs(start.y - scanLength))
          {
            difference = abs(start.x - scanLength);
            end = Vector2<int>(0, start.y - scanLength + difference);
          }
        }
        else if((start.y - scanLength >= 0) && (start.x - scanLength < 0))
        {
          difference = abs(start.x - scanLength);
          end = Vector2<int>(0,start.y - scanLength + difference);
        }
        else
        {
          end = Vector2<int>(start.x - scanLength, start.y + scanLength);
        }
         step = Vector2<int>(-1,-1);
         scan(current, startOnField, end, step, positiveInNumberOfDirection,cameraMatrix, image, robotPose, prevCameraMatrix,distance, positiv);
         if(positiv)
       {
         positivDistance = max(distance,positivDistance);
         numberOfPositivDistance++;
       }
       else
       {
         distanceArray[numberOfEntriesDistanceArray] = distance;
         numberOfEntriesDistanceArray++;
       }
      }
    }


  //SOUTHWEST
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    OUTPUT(idText, text,"");
    OUTPUT(idText, text, "SOUTHWEST:");
#endif
    currentWasSet = false;
    positiv = false;
    distance = 5000;
  
    if((start.x > 0) && (start.y < image.cameraInfo.resolutionHeight))
    {
    if((start.y < 0) && (start.x <= image.cameraInfo.resolutionWidth))
      {
        if(start.y + scanLength > 0) 
        {
          difference = abs(start.y);
          if((start.x - difference > 0) && (start.x - difference <= image.cameraInfo.resolutionWidth))
          {
            current = Vector2<int>(start.x - difference,0);
            currentWasSet = true;
          }
        }   
      }
      else if((start.x > image.cameraInfo.resolutionWidth) && (start.y >= 0))
      {
        if(start.x - scanLength < image.cameraInfo.resolutionWidth)
        {
          difference = start.x - image.cameraInfo.resolutionWidth;
          if((start.y + difference > 0) && (start.y + difference <= image.cameraInfo.resolutionHeight))
          {
            current = Vector2<int>(image.cameraInfo.resolutionWidth, start.y + difference);
            currentWasSet = true;
          }
        } 
      }
      else if((start.y < 0) && (start.x > image.cameraInfo.resolutionWidth))
      {
        if(abs(start.y) == start.x - image.cameraInfo.resolutionWidth)
        {
          if((start.y + scanLength > 0) && (start.x - scanLength < image.cameraInfo.resolutionWidth))
          {
            current = Vector2<int>(image.cameraInfo.resolutionWidth, image.cameraInfo.resolutionHeight);
            currentWasSet = true;
          }
        }
        else if(abs(start.y) < start.x - image.cameraInfo.resolutionWidth)
        {
          if(start.x - scanLength < image.cameraInfo.resolutionWidth)
          {
            difference = start.x - image.cameraInfo.resolutionWidth;
            if((start.y + difference > 0) && (start.y + difference < image.cameraInfo.resolutionHeight))
            {
              current = Vector2<int>(image.cameraInfo.resolutionWidth, start.y + difference);
              currentWasSet = true;
            }
          }
        }
        else if(abs(start.y) > start.x - image.cameraInfo.resolutionWidth)
        {
          if(start.y + scanLength > 0) 
          {
            difference = abs(start.y);
            if((start.x - difference > 0) && (start.x - difference < image.cameraInfo.resolutionWidth))
            {
              current = Vector2<int>(start.x - difference, 0);
              currentWasSet = true;
            }
          }    
        }
      }
      else if((start.y >= 0) && (start.x <= image.cameraInfo.resolutionWidth))
      {
        current = start;
        currentWasSet = true;
      }
      if(currentWasSet)
      {
        if((start.y + scanLength > image.cameraInfo.resolutionHeight) && (start.x - scanLength >= 0))
        {
          difference = image.cameraInfo.resolutionHeight - start.y;
          end = Vector2<int>(start.x - difference,image.cameraInfo.resolutionHeight);
        }
        else if((start.y + scanLength > image.cameraInfo.resolutionHeight) && (start.x - scanLength < 0))
        {
          if(abs(start.x - scanLength) == start.y + scanLength - image.cameraInfo.resolutionHeight )
          {
            end = Vector2<int>(0,image.cameraInfo.resolutionHeight);
          }
          else if(abs(start.x - scanLength) <  start.y + scanLength - image.cameraInfo.resolutionHeight)
          {
            difference = image.cameraInfo.resolutionHeight - start.y;
            end = Vector2<int>(start.x - difference, image.cameraInfo.resolutionHeight);
          }
          else if(abs(start.x - scanLength) >  start.y + scanLength - image.cameraInfo.resolutionHeight)
          {
            difference = start.x;
            end = Vector2<int>(0, start.y + difference);
          }
        }
        else if((start.y + scanLength <= image.cameraInfo.resolutionHeight) && (start.x - scanLength < 0))
        {
          difference = start.x;
          end = Vector2<int>(0,start.y + difference);
        }
        else
        {
          end = Vector2<int>(start.x - scanLength, start.y + scanLength);
        }
         step = Vector2<int>(-1,1);
         scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix,distance,positiv);
         if(positiv)
       {
         positivDistance = max(distance,positivDistance);
         numberOfPositivDistance++;
       }
       else
       {
         distanceArray[numberOfEntriesDistanceArray] = distance;
         numberOfEntriesDistanceArray++;
       }

      }
    }


  //SOUTHEAST
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    OUTPUT(idText, text,"");
    OUTPUT(idText, text, "SOUTHEAST:");
#endif
    currentWasSet = false;
    positiv = false;
    distance = 5000;
  
    if((start.x < image.cameraInfo.resolutionWidth) && (start.y < image.cameraInfo.resolutionHeight))
    {
    if((start.y < 0) && (start.x >= 0))
      {
        if(start.y + scanLength > 0) 
        {
          difference = abs(start.y);
          if((start.x + difference > 0) && (start.x + difference <= image.cameraInfo.resolutionWidth))
          {
            current = Vector2<int>(start.x + difference,0);
            currentWasSet = true;
          }
        }   
      }
      else if((start.x < 0) && (start.y >= 0))
      {
        if(start.x + scanLength > 0)
        {
          difference = abs(start.x);
          if((start.y + difference > 0) && (start.y + difference <= image.cameraInfo.resolutionHeight))
          {
            current = Vector2<int>(0, start.y + difference);
            currentWasSet = true;
          }
        } 
      }
      else if((start.y < 0) && (start.x < 0))
      {
        if(abs(start.y) == abs(start.x))
        {
          if((start.y + scanLength > 0) && (start.x + scanLength > 0))
          {
            current = Vector2<int>(0,0);
            currentWasSet = true;
          }
        }
        else if(abs(start.y) > abs(start.x))
        {
          if(start.y + scanLength > 0)
          {
            difference = abs(start.y);
            if((start.x + difference > 0) && (start.x + difference < image.cameraInfo.resolutionWidth))
            {
              current = Vector2<int>(start.x + difference, 0);
              currentWasSet = true;
            }
          }
        }
        else if(abs(start.y) < abs(start.x))
        {
          if(start.x + scanLength > 0) 
          {
            difference = abs(start.x);
            if((start.y + difference > 0) && (start.y + difference < image.cameraInfo.resolutionHeight))
            {
              current = Vector2<int>(0, start.y + difference);
              currentWasSet = true;
            }
          }    
        }
      }
      else if((start.y >= 0) && (start.x >= 0))
      {
        current = start;
        currentWasSet = true;
      }
      if(currentWasSet)
      {
        if((start.y + scanLength > image.cameraInfo.resolutionHeight) && (start.x + scanLength <= image.cameraInfo.resolutionWidth))
        {
          difference = image.cameraInfo.resolutionHeight - start.y;
          end = Vector2<int>(start.x + difference,image.cameraInfo.resolutionHeight);
        }
        else if((start.y + scanLength > image.cameraInfo.resolutionHeight) && (start.x + scanLength > image.cameraInfo.resolutionWidth))
        {
          if(start.x - image.cameraInfo.resolutionWidth == start.y - image.cameraInfo.resolutionHeight )
          {
            end = Vector2<int>(image.cameraInfo.resolutionWidth,image.cameraInfo.resolutionHeight);
          }
          else if(start.x - image.cameraInfo.resolutionWidth < start.y - image.cameraInfo.resolutionHeight)
          {
            difference = image.cameraInfo.resolutionHeight - start.y;
            end = Vector2<int>(start.x + difference, image.cameraInfo.resolutionHeight);
          }
          else if(start.x - image.cameraInfo.resolutionWidth > start.y - image.cameraInfo.resolutionHeight)
          {
            difference = image.cameraInfo.resolutionWidth - start.x;
            end = Vector2<int>(image.cameraInfo.resolutionWidth, start.y + difference);
          }
        }
        else if((start.y + scanLength <= image.cameraInfo.resolutionHeight) && (start.x + scanLength > image.cameraInfo.resolutionWidth))
        {
          difference = image.cameraInfo.resolutionWidth - start.x;
          end = Vector2<int>(image.cameraInfo.resolutionWidth,start.y + difference);
        }
        else
        {
          end = Vector2<int>(start.x + scanLength, start.y + scanLength);
        }
         step = Vector2<int>(1,1);
         scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix,distance,positiv);
         if(positiv)
       {
         positivDistance = max(distance,positivDistance);
         numberOfPositivDistance++;
       }
       else
       {
         distanceArray[numberOfEntriesDistanceArray] = distance;
         numberOfEntriesDistanceArray++;
       }
      }
    }
    

  //NORTHEAST
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    OUTPUT(idText, text,"");
    OUTPUT(idText, text, "NORTHEAST:");
#endif
    currentWasSet = false;
    positiv = false;
    distance = 5000;
  
    if((start.x < image.cameraInfo.resolutionWidth) && (start.y > 0))
    {
      if((start.y > image.cameraInfo.resolutionHeight) && (start.x >= 0))
      {
        if(start.y - scanLength < image.cameraInfo.resolutionHeight) 
        {
          difference = start.y - image.cameraInfo.resolutionHeight;
          if((start.x + difference > 0) && (start.x + difference <= image.cameraInfo.resolutionWidth))
          {
            current = Vector2<int>(start.x + difference, image.cameraInfo.resolutionHeight);
            currentWasSet = true;
          }
        }   
      }
      else if((start.x < 0) && (start.y <= image.cameraInfo.resolutionHeight))
      {
        if(start.x + scanLength > 0)
        {
          difference = abs(start.x);
          if((start.y - difference > 0) && (start.y - difference <= image.cameraInfo.resolutionHeight))
          {
            current = Vector2<int>(0, start.y - difference);
            currentWasSet = true;
          }
        } 
      }
      else if((start.y > image.cameraInfo.resolutionHeight) && (start.x < 0))
      {
        if(start.y - image.cameraInfo.resolutionHeight == abs(start.x))
        {
          if((start.y - scanLength < image.cameraInfo.resolutionHeight) && (start.x + scanLength > 0))
          {
            current = Vector2<int>(0,image.cameraInfo.resolutionHeight);
            currentWasSet = true;
          }
        }
        else if(start.y - image.cameraInfo.resolutionHeight < abs(start.x))
        {
          if(start.x + scanLength > 0)
          {
            difference = abs(start.x);
            if((start.y - difference > 0) && (start.y - difference < image.cameraInfo.resolutionHeight))
            {
              current = Vector2<int>(0, start.y - difference);
              currentWasSet = true;
            }
          }
        }
        else if(start.y - image.cameraInfo.resolutionHeight > abs(start.x))
        {
          if(start.y - scanLength < image.cameraInfo.resolutionHeight) 
          {
            difference = start.y - image.cameraInfo.resolutionHeight;
            if((start.x + difference > 0) && (start.x + difference < image.cameraInfo.resolutionWidth))
            {
              current = Vector2<int>(start.x + difference,image.cameraInfo.resolutionHeight);
              currentWasSet = true;
            }
          }    
        }
      }
      else if((start.y <= image.cameraInfo.resolutionHeight) && (start.x >= 0))
      {
        current = start;
        currentWasSet = true;
      }
      if(currentWasSet)
      {
        if((start.y - scanLength < 0) && (start.x + scanLength <= image.cameraInfo.resolutionWidth))
        {
          difference = start.y;
          end = Vector2<int>(start.x + difference,0);
        }
        else if((start.y - scanLength < 0) && (start.x + scanLength > image.cameraInfo.resolutionWidth))
        {
          if(start.x - image.cameraInfo.resolutionWidth == abs(start.y))
          {
            end = Vector2<int>(image.cameraInfo.resolutionWidth,0);
          }
          else if(start.x - image.cameraInfo.resolutionWidth < abs(start.y))
          {
            difference = abs(start.y);
            end = Vector2<int>(start.x + difference, 0);
          }
          else if(start.x - image.cameraInfo.resolutionWidth > abs(start.y))
          {
            difference = start.x - image.cameraInfo.resolutionWidth;
            end = Vector2<int>(image.cameraInfo.resolutionWidth, start.y - difference);
          }
        }
        else if((start.y - scanLength >= 0) && (start.x + scanLength > image.cameraInfo.resolutionWidth))
        {
          difference = image.cameraInfo.resolutionWidth - start.x;
          end = Vector2<int>(image.cameraInfo.resolutionWidth,start.y - difference);
        }
        else
        {
          end = Vector2<int>(start.x + scanLength, start.y - scanLength);
        }
         step = Vector2<int>(1,-1);
         scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix,distance,positiv);
         if(positiv)
       {
         positivDistance = max(distance,positivDistance);
         numberOfPositivDistance++;
       }
       else
       {
         distanceArray[numberOfEntriesDistanceArray] = distance;
         numberOfEntriesDistanceArray++;
       }
      }
    }
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
   OUTPUT(idText, text, "found positives = " << positiveInNumberOfDirection);
#endif
   if(positiveInNumberOfDirection >= minNumberOfPositives)
     return(true);
  /* else if(positiveInNumberOfDirection >= minNumberOfPositivesForDistanceCheck)
   {
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
     OUTPUT(idText, text, "positive Distance = " << positivDistance);
#endif
     for(int i = 0; i < numberOfEntriesDistanceArray; i++)
     {
       if(distanceArray[i] <= positivDistance)
         positiveInNumberOfDirection++;
       
     }
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
     OUTPUT(idText, text, "found positives2 = " << positiveInNumberOfDirection);
#endif
     if(positiveInNumberOfDirection >= minNumberOfPositives)
       return(true);
     else
       return(false);
   }*/
   else 
     return(false);
}

 void GT2005CenterCircleFinder::scan(Vector2<int> current, Vector2<double> startOnField, Vector2<int> end,                                                                           Vector2<int> step, int& positiveInNumberOfDirection, const CameraMatrix& cameraMatrix,                                                          const Image& image, const RobotPose& robotPose, const CameraMatrix& prevCameraMatrix,                                                           double& endDistance, bool& positiv)
 {
   unsigned char y,u,v;
   Vector2<int> current2 = current;
   bool useCurrent2 = false;
   bool onlyWhite = false;
   int countOnlyWhite = 0;
   colorClass currentColorClass = yellow;
   //drawings of the scan lines
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
   LINE(circlePoints_image, current.x, current.y, end.x, end.y,1,1,Drawings::yellow);
#endif
   //scanning
   //check if startpixel is white
   y = colorCorrector.correct(current.x, current.y, 0, image.image[current.y][0][current.x]);
   u = colorCorrector.correct(current.x, current.y, 1, image.image[current.y][1][current.x]);
   v = colorCorrector.correct(current.x, current.y, 2, image.image[current.y][2][current.x]);
   currentColorClass = COLOR_CLASS(y, u, v, colorTable);
 
   while((currentColorClass == white) && (current != end))
   {
      current += step;
      y = colorCorrector.correct(current.x, current.y, 0, image.image[current.y][0][current.x]);
      u = colorCorrector.correct(current.x, current.y, 1, image.image[current.y][1][current.x]);
      v = colorCorrector.correct(current.x, current.y, 2, image.image[current.y][2][current.x]);
      currentColorClass = COLOR_CLASS(y, u, v, colorTable);

      if((current == end) && (currentColorClass == white))
        onlyWhite = true;

   }
   if(onlyWhite)
   {
     if(countOnlyWhite <= onlyWhiteAllowed)
     {
       positiveInNumberOfDirection++;
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
       CIRCLE(circlePoints_image, current.x, current.y, 3, 1,Drawings::ps_dash, Drawings::red);
       OUTPUT(idText, text, "positiv: ");
       OUTPUT(idText, text, "countOnlyWhite: " << countOnlyWhite);
#endif
       countOnlyWhite++;
       positiv = true;
     }
   }
   else if((currentColorClass != white) && (current != end))//check the distance between current and start
   {
     Vector2<int> currentOnField;
     if(Geometry::calculatePointOnField(current.x,current.y,cameraMatrix,prevCameraMatrix,image.cameraInfo,currentOnField))
       {
        double distance = sqrt(sqr(startOnField.x - currentOnField.x) + sqr(startOnField.y - currentOnField.y));
        if(distance < maximalDistanceAllowedBetweenMidAndBorder) 
        {
          bool whiteFound = false;
          bool greenFound = false;
          int whiteCount = 0;
          while((current != end) && !(whiteFound))
          {
            y = colorCorrector.correct(current.x, current.y, 0, image.image[current.y][0][current.x]);
            u = colorCorrector.correct(current.x, current.y, 1, image.image[current.y][1][current.x]);
            v = colorCorrector.correct(current.x, current.y, 2, image.image[current.y][2][current.x]);
            currentColorClass = COLOR_CLASS(y, u, v, colorTable);
            if(currentColorClass == white)
            {
              if(greenFound)
               whiteCount++;
            }
            else if(currentColorClass == green)
            {
              if(!greenFound)
              {
                greenFound = true;
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
                OUTPUT(idText, text, "greenFound1 ");
#endif
              }
              else
              {
                whiteCount = 0;
              }
            }
            else
            {
              whiteCount = 0;
            }
            if(whiteCount == neededWhiteCount)
              whiteFound = true;
            current += step;
          }//while((current != end) && (whiteCount != neededWhiteCount)
          if(whiteFound)
          {
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
            OUTPUT(idText, text, "whiteFound ");
#endif
            //check distance
            if(Geometry::calculatePointOnField(current.x,current.y,cameraMatrix,prevCameraMatrix,image.cameraInfo,currentOnField))
            {
              double distance = sqrt(sqr(startOnField.x - currentOnField.x) + sqr(startOnField.y - currentOnField.y));
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
              OUTPUT(idText, text, "distance: "  << distance);
#endif
              if(distance < maximalDistanceAllowedBetweenMidAndBorder)
              {
                if(current != end)
                {
                  useCurrent2 = true;
                  current2 = current;
                  current2 += step;
                  y = colorCorrector.correct(current.x, current.y, 0, image.image[current.y][0][current.x]);
                  u = colorCorrector.correct(current.x, current.y, 1, image.image[current.y][1][current.x]);
                  v = colorCorrector.correct(current.x, current.y, 2, image.image[current.y][2][current.x]);
                  currentColorClass = COLOR_CLASS(y, u, v, colorTable);
                  while((current2 != end) && (currentColorClass != green))
                  {
                    current2 += step;
                    y = colorCorrector.correct(current.x, current.y, 0, image.image[current.y][0][current.x]);
                    u = colorCorrector.correct(current.x, current.y, 1, image.image[current.y][1][current.x]);
                    v = colorCorrector.correct(current.x, current.y, 2, image.image[current.y][2][current.x]);
                    currentColorClass = COLOR_CLASS(y, u, v, colorTable);
                  }
                  if(currentColorClass == green)
                  {
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
                    OUTPUT(idText, text, "greenFound2 ");
#endif
                    positiveInNumberOfDirection++;
                    positiv = true;
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
                    LINE(circlePoints_image, current.x, current.y, current2.x, current2.y, 1, 20, Drawings::pink);
#endif
                  }
                }
              }
            }
          }//if(whiteFound)
        }//if(distance < maximalDistanceAllowedBetweenMidAndBorder) 
      }// if(Geometry::calculatePointOnField(current.x,current.y,cameraMatrix,prevCameraMatrix,image.cameraInfo,currentOnField))
   }//else if((currentColorClass != white) && (current != end))//check the distance between current and start
   if((!positiv) && ((current.x == 0) || (current.x == image.cameraInfo.resolutionWidth) ||(current.y == 0) ||                                                      (current.y == image.cameraInfo.resolutionHeight) || (current2.x == 0) || (current2.x == image.cameraInfo.resolutionWidth) ||                      (current2.y == 0) || (current2.y == image.cameraInfo.resolutionHeight)))
  {
    Vector2<int> currentOnField;
    if(useCurrent2)
    { 
      if(Geometry::calculatePointOnField(current2.x,current2.y,cameraMatrix,prevCameraMatrix,image.cameraInfo,currentOnField))
      {
        endDistance = sqrt(sqr(startOnField.x - currentOnField.x) + sqr(startOnField.y - currentOnField.y)); 
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
        OUTPUT(idText, text, "border-distance: " << endDistance);
        CIRCLE(circlePoints_image, current2.x, current2.y, 3, 1,Drawings::ps_dash, Drawings::pink);
#endif
      }
    }
    else
    {
      if(Geometry::calculatePointOnField(current.x,current.y,cameraMatrix,prevCameraMatrix,image.cameraInfo,currentOnField))
      {
        endDistance = sqrt(sqr(startOnField.x - currentOnField.x) + sqr(startOnField.y - currentOnField.y)); 
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
        OUTPUT(idText, text, "border-distance: " << endDistance);
        CIRCLE(circlePoints_image, current.x, current.y, 3, 1,Drawings::ps_dash, Drawings::pink);
#endif
      }
    }
  }
  else if(!positiv)
  {
    endDistance = 5000;
  }
  else if(positiv)
  {
    Vector2<int> currentOnField;
    if(Geometry::calculatePointOnField(current.x,current.y,cameraMatrix,prevCameraMatrix,image.cameraInfo,currentOnField))
    {
      endDistance = sqrt(sqr(startOnField.x - currentOnField.x) + sqr(startOnField.y - currentOnField.y)); 
#ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
      OUTPUT(idText, text, "border-distance-positive: " << endDistance);
#endif
    }
  }
}

 

---