Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/ImageProcessor/GT2005ImageProcessor/GT2005CenterCircleFinder.cpp

Go to the documentation of this file.

    00001
    00002 #include "GT2005CenterCircleFinder.h"
    00003 //#define DEBUG_CENTER_CIRCLE_WITH_LOGS
    00004
    00005
    00006 GT2005CenterCircleFinder::GT2005CenterCircleFinder(const ColorCorrector& cCorrector, const ColorTable& cTable):colorCorrector(cCorrector),                                                                                                                    colorTable(cTable)
    00007 {
    00008   timeTillLastSeen = 100;
    00009   circleFound = false;
    00010   numberOfCandidates = 0;
    00011   houghStepX = areaOfInterestX/houghDimensionX;
    00012   houghStepY = areaOfInterestY/houghDimensionY;
    00013   maxQuantizationErrorX = houghStepX/2;
    00014   maxQuantizationErrorY = houghStepY/2;
    00015   lineThicknessRateAllowed = 5;
    00016   for(int i = 0; i < houghDimensionX; i++)
    00017   {
    00018     for(int j = 0; j < houghDimensionY; j++)
    00019     {
    00020       houghSpace[i][j] = 0;
    00021       flagY[j] = false;
    00022     }
    00023   }
    00024 }
    00025
    00026 void GT2005CenterCircleFinder::reset()
    00027 {
    00028   timeTillLastSeen++;
    00029   circleFound = false;
    00030   numberOfCandidates = 0;
    00031   for(int i = 0; i < houghDimensionX; i++)
    00032   {
    00033     for(int j = 0; j < houghDimensionY; j++)
    00034     {
    00035       houghSpace[i][j] = 0;
    00036       flagY[j] = false;
    00037     }
    00038   }
    00039 }
    00040
    00041 void GT2005CenterCircleFinder::addCandidate(Vector2<int>& candiOnField, Vector2<int>& candiOfImage)
    00042 {
    00043    circleFound = false;
    00044    //only candidates, which are inside the area of interest, will be added to the array candidatePoints
    00045   if((candiOnField.x < areaOfInterestX/2) && (candiOnField.x > -areaOfInterestX/2) &&
    00046      (candiOnField.y < areaOfInterestY/2) && (candiOnField.y > -areaOfInterestY/2))
    00047   {
    00048    if(numberOfCandidates < maxNumberOfCandidatesPossible)
    00049    {
    00050        candidatePoints[numberOfCandidates].pointOnField.x = double(candiOnField.x);
    00051        candidatePoints[numberOfCandidates].pointOnField.y = double(candiOnField.y);
    00052        candidatePoints[numberOfCandidates].pointOfImage = candiOfImage;
    00053        numberOfCandidates++;
    00054    }//if(numberOfCandidates <2 maxNumber)
    00055    else
    00056    {
    00057 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00058       OUTPUT(idText,text,"erhöhe maxNumberOfCandidatesPossible");
    00059 #endif
    00060    }
    00061   }/*if((candiOnField.x < areaOfInterstX/2) && (candiOnField.x > -areaOfInterestX/2) &&
    00062         (candiOnField.y < areaOfInterstY/2) && (candiOnField.y > -areaOfInterestY/2) */
    00063 }
    00064
    00065
    00066
    00067 void GT2005CenterCircleFinder::transformArrayToField(int arrayIndex, bool xDirection, int& fieldPosition)
    00068 {
    00069   int border;
    00070   int step;
    00071   int dimension;
    00072   if(xDirection)
    00073   {
    00074     border = areaOfInterestX;
    00075     step = houghStepX;
    00076     dimension = houghDimensionX;
    00077   }
    00078   else
    00079   {
    00080     border = areaOfInterestY;
    00081     step = houghStepY;
    00082     dimension = houghDimensionY;
    00083   }
    00084   /* 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
    00085    * positioned at x/y =  border/2.
    00086    * if the arrayIndex is a number between 0 and dimension/2, the field-coordinate is positiv and will be calculated as follows:
    00087    * first, the quantized value for the arrayIndex 0 will be calculated (border/2 - step/2). from that value on, the other quantized value
    00088    * can be calculated through substracting a factor of step
    00089    *
    00090    * if the arrayIndex is a number between dimension/2 and dimension, the field-coordinate is negativ:
    00091    * starting from the middle of the area of interest, stepping to the outer negative border, the quantized value for arrayIndex dimension/2
    00092    * is -step/2. other quantized values through substracting a factor of step. since one start from the middle of the area of interest,
    00093    * dimension/2 has to be substracted from arrayIndex
    00094    */
    00095   if(arrayIndex < (dimension/2))
    00096   {
    00097     fieldPosition = border/2 - step/2 - arrayIndex * step;
    00098   }
    00099   else if((arrayIndex >= (dimension/2)) && (arrayIndex < dimension))
    00100   {
    00101     fieldPosition = - step/2 - (arrayIndex-dimension/2) * step;
    00102   }
    00103 }
    00104
    00105
    00106 void GT2005CenterCircleFinder::transformFieldToArray(double fieldPos, bool xDirection, int& arrayIndex)
    00107 {
    00108   int border;
    00109   int step;
    00110   int dimension;
    00111   if(xDirection)
    00112   {
    00113     border = areaOfInterestX;
    00114     step = houghStepX;
    00115     dimension = houghDimensionX;
    00116   }
    00117   else
    00118   {
    00119     border = areaOfInterestY;
    00120     step = houghStepY;
    00121     dimension = houghDimensionY;
    00122   }
    00123
    00124  /* 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
    00125   * positioned at x/y =  border/2.
    00126   * if the field-coordinate is a number between 0 and border/2, then the arrayIndex will be a number between 0 and dimension/2:
    00127   * 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
    00128   * 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
    00129   * substracted from (dimension/2-1)
    00130   *
    00131   * if the field-coordinate is a number between 0 and -border/2, then the arrayIndex will be number between dimension/2 and dimension
    00132   * same as above, the number (field-coord div step) will be calculated. if the number equals to dimension/2-1, then the corresponding
    00133   * 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
    00134   * added to (field-coord div step).
    00135   */
    00136   if((fieldPos >= 0) && (fieldPos < (border/2)))
    00137   {
    00138     arrayIndex = (dimension/2 - 1) - (int(fieldPos) / step);
    00139   }
    00140   else if((fieldPos < 0) && (fieldPos > -(border/2)))
    00141   {
    00142     fieldPos *= -1;
    00143     arrayIndex = (int(fieldPos) / step) + (dimension/2);
    00144   }
    00145
    00146 }
    00147
    00148
    00149 void GT2005CenterCircleFinder::determineCirclePoint(const CameraMatrix& cameraMatrix, const Image& image, const RobotPose& robotPose,                                                               const CameraMatrix& prevCameraMatrix)
    00150  {
    00151    //Hough Transformation,
    00152    //(x - xc)^2 + (y - yc)^2 = r^2
    00153    int xc = 0;
    00154    int xc1 = 0; //needed, because the sqrt of a value has got two results
    00155    int yc = 0;
    00156
    00157    //variables to determine the max entry
    00158    int maxEntry = 0; //max value that occur (it is possible that more than one cell has got his number of entries)
    00159    int absMaxEntry = 0; //the absolute maximum value (only one hough space cell has got this number of entries)
    00160    bool existAbsMaxEntry = false; //does an absolute max value exist?
    00161    Vector2<double> absMaxFieldCoord(0,0);
    00162    multipleCandidate maxEntryFieldCoord[maxNumberOfCandidatesMaxEntry];
    00163    int NumberOfEntriesMaxEntry = 0;
    00164
    00165
    00166    //fill HoughSpace(2-dimensional)
    00167    for(int i = 0; i < numberOfCandidates; i++)
    00168    {
    00169        for(int y = 0; y < houghDimensionY; y++)
    00170        {
    00171          transformArrayToField(y,false,yc);
    00172          double help = double(sqr(radius) - sqr(candidatePoints[i].pointOnField.y - yc)); // help = (x - xc)^2
    00173          if(help > 0)
    00174          {
    00175             help = sqrt(help); // help = x - xc
    00176             xc = int(candidatePoints[i].pointOnField.x - help);
    00177             xc1 = int(help + candidatePoints[i].pointOnField.x);
    00178
    00179             int arrayIndex;
    00180             if((xc < (areaOfInterestX/2)) && (xc > (-areaOfInterestX/2)))
    00181             {
    00182               transformFieldToArray(xc,true,arrayIndex),
    00183               houghSpace[arrayIndex][y] += 1;
    00184               flagY[y] = true;
    00185               //determine maxEntry and absMaxEntry
    00186               if(houghSpace[arrayIndex][y] > maxEntry)
    00187               {
    00188                 NumberOfEntriesMaxEntry = 1; //skip 0, becausevone new entry will be inserted in this loop
    00189                 if(houghSpace[arrayIndex][y] > absMaxEntry)
    00190                 {
    00191                   maxEntry = absMaxEntry;
    00192                   maxEntryFieldCoord[0].pointOnField = absMaxFieldCoord;
    00193                   maxEntryFieldCoord[0].count = 0;
    00194                   maxEntryFieldCoord[0].avgDistance = 0;
    00195                   absMaxEntry = houghSpace[arrayIndex][y];
    00196                   existAbsMaxEntry = true;
    00197                   absMaxFieldCoord = Vector2<double>(xc,yc);
    00198                 }
    00199                 else if(houghSpace[arrayIndex][y] == absMaxEntry)
    00200                 {
    00201                   existAbsMaxEntry = false;
    00202                   maxEntry = absMaxEntry;
    00203                   maxEntryFieldCoord[0].pointOnField = absMaxFieldCoord;
    00204                   maxEntryFieldCoord[0].count = 0;
    00205                   maxEntryFieldCoord[0].avgDistance = 0;
    00206                 }
    00207                 else if(houghSpace[arrayIndex][y] < absMaxEntry)
    00208                 {
    00209                   maxEntry = houghSpace[arrayIndex][y];
    00210                   maxEntryFieldCoord[0].pointOnField = Vector2<double>(xc, yc);
    00211                   maxEntryFieldCoord[0].count = 0;
    00212                   maxEntryFieldCoord[0].avgDistance = 0;
    00213                 }
    00214               }//if(houghSpace[arrayIndex][y] > maxEntry)
    00215               else if(houghSpace[arrayIndex][y] == maxEntry)
    00216               {
    00217                 if(NumberOfEntriesMaxEntry < maxNumberOfCandidatesMaxEntry)
    00218                 {
    00219                   maxEntryFieldCoord[NumberOfEntriesMaxEntry].pointOnField = Vector2<double>(xc, yc);
    00220                   maxEntryFieldCoord[NumberOfEntriesMaxEntry].count = 0;
    00221                   maxEntryFieldCoord[NumberOfEntriesMaxEntry].avgDistance = 0;
    00222                   NumberOfEntriesMaxEntry++;
    00223                 }
    00224                 else
    00225                 {
    00226 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00227                   OUTPUT(idText,text,"erhöhe maxNumberOfCandidatesMaxEntry");
    00228 #endif
    00229                 }
    00230               }//else if(houghSpace[arrayIndex][y] == maxEntry)
    00231             }//if((xc < (areaOfInterestX/2)) && (xc > (-areaOfInterestX/2)))
    00232
    00233             if((xc1 < (areaOfInterestX/2)) && (xc1 > (-areaOfInterestX/2)))
    00234             {
    00235               transformFieldToArray(xc1,true,arrayIndex),
    00236               houghSpace[arrayIndex][y] += 1;
    00237               flagY[y] = true;
    00238               //determine maxEntry and absMaxEntry
    00239               if(houghSpace[arrayIndex][y] > maxEntry)
    00240               {
    00241                 NumberOfEntriesMaxEntry = 1;
    00242                 if(houghSpace[arrayIndex][y] > absMaxEntry)
    00243                 {
    00244                   maxEntry = absMaxEntry;
    00245                   maxEntryFieldCoord[0].pointOnField = absMaxFieldCoord;
    00246                   maxEntryFieldCoord[0].count = 0;
    00247                   maxEntryFieldCoord[0].avgDistance = 0;
    00248                   absMaxEntry = houghSpace[arrayIndex][y];
    00249                   existAbsMaxEntry = true;
    00250                   absMaxFieldCoord = Vector2<double>(xc1,yc);
    00251                 }
    00252                 else if(houghSpace[arrayIndex][y] == absMaxEntry)
    00253                 {
    00254                   existAbsMaxEntry = false;
    00255                   maxEntry = absMaxEntry;
    00256                   maxEntryFieldCoord[0].pointOnField = absMaxFieldCoord;
    00257                   maxEntryFieldCoord[0].count = 0;
    00258                   maxEntryFieldCoord[0].avgDistance = 0;
    00259                 }
    00260                 else if(houghSpace[arrayIndex][y] < absMaxEntry)
    00261                 {
    00262                   maxEntry = houghSpace[arrayIndex][y];
    00263                   maxEntryFieldCoord[0].pointOnField = Vector2<double>(xc1, yc);
    00264                   maxEntryFieldCoord[0].count = 0;
    00265                   maxEntryFieldCoord[0].avgDistance = 0;
    00266                 }
    00267               }//if(houghSpace[arrayIndex][y] > maxEntry)
    00268               else if(houghSpace[arrayIndex][y] == maxEntry)
    00269               {
    00270                 if(NumberOfEntriesMaxEntry < maxNumberOfCandidatesMaxEntry)
    00271                 {
    00272                   maxEntryFieldCoord[NumberOfEntriesMaxEntry].pointOnField = Vector2<double>(xc1, yc);
    00273                   maxEntryFieldCoord[NumberOfEntriesMaxEntry].count = 0;
    00274                   NumberOfEntriesMaxEntry++;
    00275                 }
    00276                 else
    00277                 {
    00278 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00279                   OUTPUT(idText,text,"erhöhe maxNumberOfCandidatesMaxEntry");
    00280 #endif
    00281                 }
    00282               }//else if(houghSpace[arrayIndex][y] == maxEntry)
    00283             }//if((xc1 < (areaOfInterestX/2)) && (xc1 > (-areaOfInterestX/2)))
    00284          }//if(help1 > 0)
    00285
    00286        }//for(int y = 0; y < houghDimension; y++)
    00287
    00288    }//for(int i = 0; i < numberOfCandidates; i++)
    00289 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00290    OUTPUT(idText,text,"maxEntry, abs: " << maxEntry << ", " << absMaxEntry << ", " << existAbsMaxEntry);
    00291 #endif
    00292
    00293    //evaluation
    00294    if(maxEntry >= mEntry)
    00295    {
    00296      if(existAbsMaxEntry)
    00297      {
    00298        checkCircleMidpoint(absMaxFieldCoord, absMaxEntry, cameraMatrix, image, robotPose, prevCameraMatrix);                         }
    00299      else
    00300      {
    00301        checkCircleMidpoint2(maxEntry, NumberOfEntriesMaxEntry, maxEntryFieldCoord, cameraMatrix, image, robotPose, prevCameraMatrix);
    00302      }//else
    00303     }//if((existMaxEntry)&&(absMaxEntry >= aMaxEntry) && (absMaxEntry < max))
    00304
    00305     if(!circleFound && timeTillLastSeen < 60)
    00306     {
    00307       Drawings::Color c;
    00308       if (timeTillLastSeen<2)
    00309       {
    00310         c = Drawings::yellow;
    00311       }
    00312       else if (timeTillLastSeen<10)
    00313       {
    00314         c = Drawings::yellowOrange;
    00315       }
    00316       else if (timeTillLastSeen<30)
    00317       {
    00318         c = Drawings::orange;
    00319       }
    00320       else
    00321       {
    00322         c = Drawings::red;
    00323       }
    00324       CIRCLE(circlePoints_Field, midPointLastSeenField.x, midPointLastSeenField.y, 180, 3,Drawings::ps_solid, c)
    00325     }
    00326  }
    00327
    00328
    00329
    00330  bool GT2005CenterCircleFinder::getCircle(Vector2<double>& mid)
    00331  {
    00332    if(circleFound)
    00333    {
    00334      mid = midPoint;
    00335      return(true);
    00336    }
    00337    else
    00338      return(false);
    00339  }
    00340
    00341
    00342
    00343  void GT2005CenterCircleFinder::checkCircleMidpoint(Vector2<double> candidate, int absMaxEntry, const CameraMatrix& cameraMatrix,                                                                  const Image& image, const RobotPose& robotPose, const CameraMatrix& prevCameraMatrix)
    00344  {
    00345    bool circleMidPoint = false;
    00346    Vector2<int> candidateInImage;
    00347    Vector2<int> candidateOnField(int(candidate.x), int(candidate.y));
    00348    Geometry::calculatePointInImage(candidateOnField,cameraMatrix,image.cameraInfo,candidateInImage);
    00349    if(absMaxEntry >= enoughPros)
    00350      circleMidPoint = true;
    00351    else
    00352    {
    00353      int count = 0;
    00354      double distance = 0;
    00355      double maxQuantizationError = max(maxQuantizationErrorX,maxQuantizationErrorY);
    00356      for(int i = 0; i < numberOfCandidates; i++)
    00357      {
    00358        distance = sqrt(sqr(candidatePoints[i].pointOnField.x - candidate.x) + sqr(candidatePoints[i].pointOnField.y - candidate.y));
    00359        if((distance > (radius - (maxQuantizationError*2))) && (distance < (radius + (maxQuantizationError*2))))
    00360        {
    00361          count++;
    00362        }
    00363      }
    00364 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00365    OUTPUT(idText,text,"midpointtest1: " << count);
    00366 #endif
    00367      if((count >= mEntry) && (count < enoughPros))
    00368      {
    00369        // scan in four direction and look for white
    00370        circleMidPoint = scanSurrounding(candidateInImage, candidate, cameraMatrix, image, robotPose, prevCameraMatrix);
    00371      }
    00372      else if(count >= enoughPros)
    00373        circleMidPoint = true;
    00374    }//else
    00375
    00376  /*  if(circleMidPoint == true)
    00377    {
    00378      midPoint = candidate;
    00379      circleFound = true;
    00380
    00381      //drawings of the mid-point
    00382      Vector2<double> fieldCoord = Geometry::relative2FieldCoord(robotPose, candidate.x, candidate.y);
    00383      CIRCLE(circlePoints_Field, fieldCoord.x, fieldCoord.y, 150, 30,Drawings::ps_dash, Drawings::black);
    00384      //CIRCLE(circlePoints_Field, candidate.x, candidate.y, 150, 30,Drawings::ps_dash, Drawings::black);
    00385      CIRCLE(circlePoints_image, candidateInImage.x, candidateInImage.y, 3, 1,Drawings::ps_dash, Drawings::black);
    00386    }
    00387    */
    00388
    00389    Vector2<double> fieldCoord = Geometry::relative2FieldCoord(robotPose, candidate.x, candidate.y);
    00390    if(circleMidPoint == true)
    00391    {
    00392      midPoint = fieldCoord;
    00393      circleFound = true;
    00394      timeTillLastSeen = 0;
    00395      CIRCLE(circlePoints_Field, fieldCoord.x, fieldCoord.y, 180, 3,Drawings::ps_solid, Drawings::pink);
    00396      midPointLastSeenField = fieldCoord;
    00397    }
    00398
    00399 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00400    OUTPUT(idText, text, "");
    00401    OUTPUT(idText, text, "");
    00402 #endif
    00403  }
    00404
    00405
    00406
    00407  void GT2005CenterCircleFinder::checkCircleMidpoint2(int maxEntry, int NumberOfEntriesMaxEntry,                                                                                                      multipleCandidate maxEntryFieldCoord[maxNumberOfCandidatesMaxEntry],                                                                           const CameraMatrix& cameraMatrix, const Image& image, const RobotPose& robotPose,                                                              const CameraMatrix& prevCameraMatrix)
    00408  {
    00409    bool circleMidPoint = false;
    00410    double distance = 0;
    00411    int absMax = 0;
    00412    bool existAbsMax = false;
    00413    int position = 0;
    00414    double minAvgDistance = 2000; //just for initialization
    00415    double maxQuantizationError = max(maxQuantizationErrorX,maxQuantizationErrorY);
    00416
    00417
    00418    if(maxEntry >= mEntry2)
    00419    {
    00420 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00421      OUTPUT(idText, text, "midpoint2: maxEntry > mEntry2");
    00422 #endif
    00423      //calculate the average distance between the candidate
    00424      for(int i = 0; i < NumberOfEntriesMaxEntry; i++)
    00425      {
    00426        for(int j = i + 1; j < NumberOfEntriesMaxEntry; j++)
    00427        {
    00428          distance = sqrt(sqr(maxEntryFieldCoord[i].pointOnField.x - maxEntryFieldCoord[j].pointOnField.x) +                                                              sqr(maxEntryFieldCoord[i].pointOnField.y - maxEntryFieldCoord[j].pointOnField.y));
    00429          maxEntryFieldCoord[i].avgDistance += distance;
    00430          maxEntryFieldCoord[j].avgDistance += distance;
    00431        }//for(int j = 0; j < NumberOfEntriesMaxEntry;j++)
    00432        maxEntryFieldCoord[i].avgDistance = maxEntryFieldCoord[i].avgDistance/(NumberOfEntriesMaxEntry - 1);
    00433        if(maxEntryFieldCoord[i].avgDistance < minAvgDistance)
    00434        {
    00435          minAvgDistance = maxEntryFieldCoord[i].avgDistance;
    00436          position = i;
    00437        }
    00438      }//for(int i = 0; i < NumberOfEntriesMaxEntry; i++)
    00439
    00440
    00441     /* ///////LÖSCHEN!!!
    00442      Vector2<int> candidateInImage;
    00443      Vector2<int> candidateOnField((int)maxEntryFieldCoord[position].pointOnField.x,                                                                                               (int)maxEntryFieldCoord[position].pointOnField.y);
    00444      Geometry::calculatePointInImage(candidateOnField ,cameraMatrix,image.cameraInfo,candidateInImage);
    00445      CIRCLE(circlePoints_image, candidateInImage.x, candidateInImage.y, 3, 1,Drawings::ps_dash, Drawings::blue);
    00446      ////LÖSCHEN!!!*/
    00447
    00448 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00449      OUTPUT(idText, text, "midpoint2: minAvgDistance = " << minAvgDistance);
    00450 #endif
    00451      if(minAvgDistance <= acceptedAvgDistance)
    00452      {
    00453 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00454        OUTPUT(idText, text, "midpoint2: Cluster");
    00455 #endif
    00456        circleMidPoint = true;
    00457      }
    00458
    00459    }//if(maxEntry > enoughPros)
    00460    if(circleMidPoint == false)
    00461    {
    00462      for(int j = 0; j < NumberOfEntriesMaxEntry; j++)
    00463      {
    00464        for(int i = 0; i < numberOfCandidates; i++)
    00465        {
    00466          distance = sqrt(sqr(candidatePoints[i].pointOnField.x - maxEntryFieldCoord[j].pointOnField.x) +                                                               sqr(candidatePoints[i].pointOnField.y - maxEntryFieldCoord[j].pointOnField.y));
    00467          if((distance > (radius - (maxQuantizationError*2))) && (distance < (radius + (maxQuantizationError*2))))
    00468          {
    00469            maxEntryFieldCoord[j].count++;
    00470          }
    00471        }//for(int i = 0; i < numberOfCandidates; i++)
    00472
    00473        if(maxEntryFieldCoord[j].count > absMax)
    00474        {
    00475          existAbsMax = true;
    00476          absMax = maxEntryFieldCoord[j].count;
    00477          position = j;
    00478        }
    00479        else if(maxEntryFieldCoord[j].count == absMax)
    00480        {
    00481          existAbsMax = false;
    00482        }
    00483      }//for(int j = 0; j < NumberOfEntriesMaxEntry; j++)
    00484
    00485      Vector2<int> candidateInImage;
    00486      Vector2<int> candidateOnField((int)maxEntryFieldCoord[position].pointOnField.x,                                                                                               (int)maxEntryFieldCoord[position].pointOnField.y);
    00487      Geometry::calculatePointInImage(candidateOnField ,cameraMatrix,image.cameraInfo,candidateInImage);
    00488 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00489      OUTPUT(idText, text, "midpoint2:    existAbsMax, absMax: " << existAbsMax << ", " << absMax);
    00490 #endif
    00491      if(existAbsMax)
    00492      {
    00493        if(maxEntryFieldCoord[position].count >= enoughPros)
    00494          circleMidPoint = true;
    00495        else if((maxEntryFieldCoord[position].count < enoughPros) && (maxEntryFieldCoord[position].count >= mEntry))
    00496        {
    00497          circleMidPoint = scanSurrounding(candidateInImage, maxEntryFieldCoord[position].pointOnField, cameraMatrix, image, robotPose,                                                   prevCameraMatrix);
    00498        }
    00499      }//if(existAbsMax)
    00500      else if(absMax >= mEntry3)
    00501      {
    00502        for(int i = 0; (i < NumberOfEntriesMaxEntry) && (circleMidPoint == false); i++)
    00503        {
    00504          if(maxEntryFieldCoord[i].count == absMax)
    00505          {
    00506            candidateOnField = Vector2<int>((int)maxEntryFieldCoord[i].pointOnField.x, (int)maxEntryFieldCoord[i].pointOnField.y);
    00507            Geometry::calculatePointInImage(candidateOnField ,cameraMatrix,image.cameraInfo,candidateInImage);
    00508            circleMidPoint = scanSurrounding(candidateInImage, maxEntryFieldCoord[i].pointOnField, cameraMatrix, image, robotPose,                                                          prevCameraMatrix);
    00509            if(circleMidPoint)
    00510            {
    00511              position = i;
    00512            }
    00513          }//if(maxEntryFieldCoord[i].count = absMax)
    00514        }//for(int i = 0; (i < NumberOfEntriesMaxEntry) && (circleMidPoint == false); i++)
    00515      }//else if(absMax >= mEntry3)
    00516    }// if(circleMidPoint == false)
    00517
    00518  /*   if(circleMidPoint == true)
    00519    {
    00520      midPoint = maxEntryFieldCoord[position].pointOnField;
    00521      circleFound = true;
    00522
    00523      //drawings of the mid-point
    00524      Vector2<double> fieldCoord = Geometry::relative2FieldCoord(robotPose, maxEntryFieldCoord[position].pointOnField.x,                                                                                                    maxEntryFieldCoord[position].pointOnField.y);
    00525      Vector2<int> candidateInImage;
    00526      Vector2<int> candidateOnField((int)maxEntryFieldCoord[position].pointOnField.x,                                                                                               (int)maxEntryFieldCoord[position].pointOnField.y);
    00527      Geometry::calculatePointInImage(candidateOnField ,cameraMatrix,image.cameraInfo,candidateInImage);
    00528      CIRCLE(circlePoints_Field, fieldCoord.x, fieldCoord.y, 150, 30,Drawings::ps_dash, Drawings::yellowOrange);
    00529      CIRCLE(circlePoints_image, candidateInImage.x, candidateInImage.y, 3, 1,Drawings::ps_dash, Drawings::yellowOrange);
    00530    }*/
    00531
    00532     Vector2<double> fieldCoord = Geometry::relative2FieldCoord(robotPose, maxEntryFieldCoord[position].pointOnField.x,                                                                                                    maxEntryFieldCoord[position].pointOnField.y);
    00533     Vector2<int> candidateInImage;
    00534     Vector2<int> candidateOnField((int)maxEntryFieldCoord[position].pointOnField.x,                                                                                               (int)maxEntryFieldCoord[position].pointOnField.y);
    00535     Geometry::calculatePointInImage(candidateOnField ,cameraMatrix,image.cameraInfo,candidateInImage);
    00536    if(circleMidPoint == true)
    00537    {
    00538      timeTillLastSeen = 0;
    00539      midPoint = fieldCoord;
    00540      circleFound = true;
    00541      CIRCLE(circlePoints_Field, fieldCoord.x, fieldCoord.y, 180, 3,Drawings::ps_solid, Drawings::pink);
    00542      midPointLastSeenField = fieldCoord;
    00543    }
    00544
    00545
    00546 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00547     OUTPUT(idText, text, "");
    00548     OUTPUT(idText, text, "");
    00549 #endif
    00550  }
    00551
    00552
    00553 bool GT2005CenterCircleFinder::scanSurrounding(Vector2<int> start, Vector2<double> startOnField, const CameraMatrix& cameraMatrix,                                                            const Image& image, const RobotPose& robotPose, const CameraMatrix& prevCameraMatrix)
    00554 {
    00555   int positiveInNumberOfDirection = 0;
    00556   Vector2<int> current;
    00557   Vector2<int> end;
    00558   Vector2<int> step;
    00559   int difference = 0;
    00560   bool currentWasSet = false;
    00561    bool positiv = false;
    00562   double distance = 5000;
    00563   double positivDistance = 0;
    00564   int numberOfPositivDistance = 0;
    00565   double distanceArray[numberOfScanLines];
    00566   int numberOfEntriesDistanceArray = 0;
    00567
    00568
    00569   //North
    00570 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00571   OUTPUT(idText, text,"");
    00572   OUTPUT(idText, text, "NORTH:");
    00573 #endif
    00574
    00575   if((start.x >= 0) && (start.x <= image.cameraInfo.resolutionWidth) && (start.y > 0))
    00576   {
    00577     if(start.y > image.cameraInfo.resolutionHeight)
    00578     {
    00579       if((start.y - scanLength) < image.cameraInfo.resolutionHeight)
    00580       {
    00581         current = Vector2<int>(start.x, image.cameraInfo.resolutionHeight);
    00582         currentWasSet = true;
    00583       }
    00584     }
    00585     else
    00586     {
    00587       current = start;
    00588       currentWasSet = true;
    00589     }//else
    00590     if(currentWasSet)
    00591     {
    00592       if((start.y - scanLength) < 0)
    00593         end = Vector2<int> (start.x,0);
    00594       else
    00595         end = Vector2<int> (start.x, start.y - scanLength);
    00596
    00597        step = Vector2<int>(0,-1);
    00598        scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix, distance, positiv);
    00599        if(positiv)
    00600        {
    00601          positivDistance = max(distance,positivDistance);
    00602          numberOfPositivDistance++;
    00603        }
    00604        else
    00605        {
    00606          distanceArray[numberOfEntriesDistanceArray] = distance;
    00607          numberOfEntriesDistanceArray++;
    00608        }
    00609     }
    00610   }//if((start.x >= 0) && (start.x <= image.cameraInfo.resolutionWidth) && (start.y > 0))
    00611
    00612
    00613
    00614     //South
    00615 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00616     OUTPUT(idText, text,"");
    00617     OUTPUT(idText, text, "SOUTH:");
    00618 #endif
    00619     currentWasSet = false;
    00620     positiv = false;
    00621     distance = 5000;
    00622
    00623     if((start.x >= 0) && (start.x <= image.cameraInfo.resolutionWidth) && (start.y < image.cameraInfo.resolutionHeight))
    00624     {
    00625       if(start.y < 0)
    00626       {
    00627         if((start.y + scanLength) > 0)
    00628         {
    00629           current = Vector2<int>(start.x, 0);
    00630           currentWasSet = true;
    00631         }
    00632       }
    00633       else
    00634       {
    00635         current = start;
    00636         currentWasSet = true;
    00637       }
    00638       if(currentWasSet)
    00639       {
    00640         if((start.y + scanLength) > image.cameraInfo.resolutionHeight)
    00641          end = Vector2<int> (start.x,image.cameraInfo.resolutionHeight);
    00642         else
    00643          end = Vector2<int> (start.x, start.y + scanLength);
    00644         step = Vector2<int> (0,1);
    00645         scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix, distance,positiv);
    00646         if(positiv)
    00647        {
    00648          positivDistance = max(distance,positivDistance);
    00649          numberOfPositivDistance++;
    00650        }
    00651        else
    00652        {
    00653          distanceArray[numberOfEntriesDistanceArray] = distance;
    00654          numberOfEntriesDistanceArray++;
    00655        }
    00656       }
    00657     }//if((start.x >= 0) && (start.x <= image.cameraInfo.resolutionWidth) && (start.y <= image.cameraInfo.resolutionHeight))
    00658
    00659   //West
    00660 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00661     OUTPUT(idText, text,"");
    00662     OUTPUT(idText, text, "WEST:");
    00663 #endif
    00664     currentWasSet = false;
    00665     positiv = false;
    00666     distance = 5000;
    00667
    00668     if((start.y  >= 0) && (start.y <= image.cameraInfo.resolutionHeight) && (start.x > 0))
    00669     {
    00670       if(start.x > image.cameraInfo.resolutionWidth)
    00671       {
    00672         if((start.x - scanLength) < image.cameraInfo.resolutionWidth)
    00673         {
    00674           current = Vector2<int>(image.cameraInfo.resolutionWidth, start.y);
    00675           currentWasSet = true;
    00676         }
    00677       }
    00678       else
    00679       {
    00680         current = start;
    00681         currentWasSet = true;
    00682       }
    00683       if(currentWasSet)
    00684       {
    00685         if((start.x - scanLength) < 0)
    00686          end = Vector2<int> (0, start.y);
    00687         else
    00688          end = Vector2<int> (start.x - scanLength, start.y);
    00689         step = Vector2<int> (-1,0);
    00690         scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix, distance,positiv);
    00691         if(positiv)
    00692        {
    00693          positivDistance = max(distance,positivDistance);
    00694          numberOfPositivDistance++;
    00695        }
    00696        else
    00697        {
    00698          distanceArray[numberOfEntriesDistanceArray] = distance;
    00699          numberOfEntriesDistanceArray++;
    00700        }
    00701       }
    00702     }//if((start.y  >= 0) && (start.y <= image.cameraInfo.resolutionHeight) && (start.x > 0)
    00703
    00704
    00705   //East
    00706 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00707     OUTPUT(idText, text,"");
    00708     OUTPUT(idText, text, "EAST:");
    00709 #endif
    00710     currentWasSet = false;
    00711     positiv = false;
    00712     distance = 5000;
    00713
    00714
    00715     if((start.y  >= 0) && (start.y <= image.cameraInfo.resolutionHeight) && (start.x < image.cameraInfo.resolutionWidth))
    00716     {
    00717       if(start.x < 0)
    00718       {
    00719         if((start.x + scanLength) > 0)
    00720         {
    00721           current = Vector2<int>(0, start.y);
    00722           currentWasSet = true;
    00723         }
    00724       }
    00725       else
    00726       {
    00727         current = start;
    00728         currentWasSet = true;
    00729       }
    00730       if(currentWasSet)
    00731       {
    00732         if((start.x + scanLength) > image.cameraInfo.resolutionWidth)
    00733           end = Vector2<int> (image.cameraInfo.resolutionWidth, start.y);
    00734         else
    00735           end = Vector2<int> (start.x + scanLength, start.y);
    00736        step = Vector2<int> (1,0);
    00737        scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix, distance, positiv);
    00738        if(positiv)
    00739        {
    00740          positivDistance = max(distance,positivDistance);
    00741          numberOfPositivDistance++;
    00742        }
    00743        else
    00744        {
    00745          distanceArray[numberOfEntriesDistanceArray] = distance;
    00746          numberOfEntriesDistanceArray++;
    00747        }
    00748       }
    00749     }//if((start.y  >= 0) && (start.y <= image.cameraInfo.resolutionHeight) && (start.x < image.cameraInfo.resolutionWidth))
    00750
    00751
    00752   //NORTHWEST
    00753 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00754     OUTPUT(idText, text,"");
    00755     OUTPUT(idText, text, "NORTHWEST:");
    00756 #endif
    00757     currentWasSet = false;
    00758     positiv = false;
    00759     distance = 5000;
    00760
    00761     if((start.x > 0) && (start.y > 0))
    00762     {
    00763     if((start.y > image.cameraInfo.resolutionHeight) && (start.x <= image.cameraInfo.resolutionWidth))
    00764       {
    00765         if(start.y - scanLength < image.cameraInfo.resolutionHeight)
    00766         {
    00767           difference = start.y - image.cameraInfo.resolutionHeight;
    00768           if((start.x - difference > 0) && (start.x - difference <= image.cameraInfo.resolutionWidth))
    00769           {
    00770             current = Vector2<int>(start.x - difference, image.cameraInfo.resolutionHeight);
    00771             currentWasSet = true;
    00772           }
    00773         }
    00774       }
    00775       else if((start.x > image.cameraInfo.resolutionWidth) && (start.y <= image.cameraInfo.resolutionHeight))
    00776       {
    00777         if(start.x - scanLength < image.cameraInfo.resolutionWidth)
    00778         {
    00779           difference = start.x - image.cameraInfo.resolutionWidth;
    00780           if((start.y - difference > 0) && (start.y - difference <= image.cameraInfo.resolutionHeight))
    00781           {
    00782             current = Vector2<int>(image.cameraInfo.resolutionWidth, start.y - difference);
    00783             currentWasSet = true;
    00784           }
    00785         }
    00786       }
    00787       else if((start.y > image.cameraInfo.resolutionHeight) && (start.x > image.cameraInfo.resolutionWidth))
    00788       {
    00789         if(start.y - image.cameraInfo.resolutionHeight == start.x - image.cameraInfo.resolutionWidth)
    00790         {
    00791           if((start.y - scanLength < image.cameraInfo.resolutionHeight) && (start.x - scanLength < image.cameraInfo.resolutionWidth))
    00792           {
    00793             current = Vector2<int>(image.cameraInfo.resolutionWidth, image.cameraInfo.resolutionHeight);
    00794             currentWasSet = true;
    00795           }
    00796         }
    00797         else if(start.y - image.cameraInfo.resolutionHeight < start.x - image.cameraInfo.resolutionWidth)
    00798         {
    00799           if(start.x - scanLength < image.cameraInfo.resolutionWidth)
    00800           {
    00801             difference = start.x - image.cameraInfo.resolutionWidth;
    00802             if((start.y - difference > 0) && (start.y - difference < image.cameraInfo.resolutionHeight))
    00803             {
    00804               current = Vector2<int>(image.cameraInfo.resolutionWidth, start.y - difference);
    00805               currentWasSet = true;
    00806             }
    00807           }
    00808         }
    00809         else if(start.y - image.cameraInfo.resolutionHeight > start.x - image.cameraInfo.resolutionWidth)
    00810         {
    00811           if(start.y - scanLength < image.cameraInfo.resolutionHeight)
    00812           {
    00813             difference = start.y - image.cameraInfo.resolutionHeight;
    00814             if((start.x - difference > 0) && (start.x - difference < image.cameraInfo.resolutionWidth))
    00815             {
    00816               current = Vector2<int>(start.x - difference, image.cameraInfo.resolutionHeight);
    00817               currentWasSet = true;
    00818             }
    00819           }
    00820         }
    00821       }
    00822       else if((start.y <= image.cameraInfo.resolutionHeight) && (start.x <= image.cameraInfo.resolutionWidth))
    00823       {
    00824         current = start;
    00825         currentWasSet = true;
    00826       }
    00827       if(currentWasSet)
    00828       {
    00829         if((start.y - scanLength < 0) && (start.x - scanLength >= 0))
    00830         {
    00831           difference = abs(start.y - scanLength);
    00832           end = Vector2<int>(start.x - scanLength + difference,0);
    00833         }
    00834         else if((start.y - scanLength < 0) && (start.x - scanLength < 0))
    00835         {
    00836           if(abs(start.x - scanLength) == abs(start.y - scanLength))
    00837           {
    00838             end = Vector2<int>(0,0);
    00839           }
    00840           else if(abs(start.x - scanLength) < abs(start.y - scanLength))
    00841           {
    00842             difference = abs(start.y - scanLength);
    00843             end = Vector2<int>(start.x - scanLength + difference, 0);
    00844           }
    00845           else if(abs(start.x - scanLength) > abs(start.y - scanLength))
    00846           {
    00847             difference = abs(start.x - scanLength);
    00848             end = Vector2<int>(0, start.y - scanLength + difference);
    00849           }
    00850         }
    00851         else if((start.y - scanLength >= 0) && (start.x - scanLength < 0))
    00852         {
    00853           difference = abs(start.x - scanLength);
    00854           end = Vector2<int>(0,start.y - scanLength + difference);
    00855         }
    00856         else
    00857         {
    00858           end = Vector2<int>(start.x - scanLength, start.y + scanLength);
    00859         }
    00860          step = Vector2<int>(-1,-1);
    00861          scan(current, startOnField, end, step, positiveInNumberOfDirection,cameraMatrix, image, robotPose, prevCameraMatrix,distance, positiv);
    00862          if(positiv)
    00863        {
    00864          positivDistance = max(distance,positivDistance);
    00865          numberOfPositivDistance++;
    00866        }
    00867        else
    00868        {
    00869          distanceArray[numberOfEntriesDistanceArray] = distance;
    00870          numberOfEntriesDistanceArray++;
    00871        }
    00872       }
    00873     }
    00874
    00875
    00876   //SOUTHWEST
    00877 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    00878     OUTPUT(idText, text,"");
    00879     OUTPUT(idText, text, "SOUTHWEST:");
    00880 #endif
    00881     currentWasSet = false;
    00882     positiv = false;
    00883     distance = 5000;
    00884
    00885     if((start.x > 0) && (start.y < image.cameraInfo.resolutionHeight))
    00886     {
    00887     if((start.y < 0) && (start.x <= image.cameraInfo.resolutionWidth))
    00888       {
    00889         if(start.y + scanLength > 0)
    00890         {
    00891           difference = abs(start.y);
    00892           if((start.x - difference > 0) && (start.x - difference <= image.cameraInfo.resolutionWidth))
    00893           {
    00894             current = Vector2<int>(start.x - difference,0);
    00895             currentWasSet = true;
    00896           }
    00897         }
    00898       }
    00899       else if((start.x > image.cameraInfo.resolutionWidth) && (start.y >= 0))
    00900       {
    00901         if(start.x - scanLength < image.cameraInfo.resolutionWidth)
    00902         {
    00903           difference = start.x - image.cameraInfo.resolutionWidth;
    00904           if((start.y + difference > 0) && (start.y + difference <= image.cameraInfo.resolutionHeight))
    00905           {
    00906             current = Vector2<int>(image.cameraInfo.resolutionWidth, start.y + difference);
    00907             currentWasSet = true;
    00908           }
    00909         }
    00910       }
    00911       else if((start.y < 0) && (start.x > image.cameraInfo.resolutionWidth))
    00912       {
    00913         if(abs(start.y) == start.x - image.cameraInfo.resolutionWidth)
    00914         {
    00915           if((start.y + scanLength > 0) && (start.x - scanLength < image.cameraInfo.resolutionWidth))
    00916           {
    00917             current = Vector2<int>(image.cameraInfo.resolutionWidth, image.cameraInfo.resolutionHeight);
    00918             currentWasSet = true;
    00919           }
    00920         }
    00921         else if(abs(start.y) < start.x - image.cameraInfo.resolutionWidth)
    00922         {
    00923           if(start.x - scanLength < image.cameraInfo.resolutionWidth)
    00924           {
    00925             difference = start.x - image.cameraInfo.resolutionWidth;
    00926             if((start.y + difference > 0) && (start.y + difference < image.cameraInfo.resolutionHeight))
    00927             {
    00928               current = Vector2<int>(image.cameraInfo.resolutionWidth, start.y + difference);
    00929               currentWasSet = true;
    00930             }
    00931           }
    00932         }
    00933         else if(abs(start.y) > start.x - image.cameraInfo.resolutionWidth)
    00934         {
    00935           if(start.y + scanLength > 0)
    00936           {
    00937             difference = abs(start.y);
    00938             if((start.x - difference > 0) && (start.x - difference < image.cameraInfo.resolutionWidth))
    00939             {
    00940               current = Vector2<int>(start.x - difference, 0);
    00941               currentWasSet = true;
    00942             }
    00943           }
    00944         }
    00945       }
    00946       else if((start.y >= 0) && (start.x <= image.cameraInfo.resolutionWidth))
    00947       {
    00948         current = start;
    00949         currentWasSet = true;
    00950       }
    00951       if(currentWasSet)
    00952       {
    00953         if((start.y + scanLength > image.cameraInfo.resolutionHeight) && (start.x - scanLength >= 0))
    00954         {
    00955           difference = image.cameraInfo.resolutionHeight - start.y;
    00956           end = Vector2<int>(start.x - difference,image.cameraInfo.resolutionHeight);
    00957         }
    00958         else if((start.y + scanLength > image.cameraInfo.resolutionHeight) && (start.x - scanLength < 0))
    00959         {
    00960           if(abs(start.x - scanLength) == start.y + scanLength - image.cameraInfo.resolutionHeight )
    00961           {
    00962             end = Vector2<int>(0,image.cameraInfo.resolutionHeight);
    00963           }
    00964           else if(abs(start.x - scanLength) <  start.y + scanLength - image.cameraInfo.resolutionHeight)
    00965           {
    00966             difference = image.cameraInfo.resolutionHeight - start.y;
    00967             end = Vector2<int>(start.x - difference, image.cameraInfo.resolutionHeight);
    00968           }
    00969           else if(abs(start.x - scanLength) >  start.y + scanLength - image.cameraInfo.resolutionHeight)
    00970           {
    00971             difference = start.x;
    00972             end = Vector2<int>(0, start.y + difference);
    00973           }
    00974         }
    00975         else if((start.y + scanLength <= image.cameraInfo.resolutionHeight) && (start.x - scanLength < 0))
    00976         {
    00977           difference = start.x;
    00978           end = Vector2<int>(0,start.y + difference);
    00979         }
    00980         else
    00981         {
    00982           end = Vector2<int>(start.x - scanLength, start.y + scanLength);
    00983         }
    00984          step = Vector2<int>(-1,1);
    00985          scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix,distance,positiv);
    00986          if(positiv)
    00987        {
    00988          positivDistance = max(distance,positivDistance);
    00989          numberOfPositivDistance++;
    00990        }
    00991        else
    00992        {
    00993          distanceArray[numberOfEntriesDistanceArray] = distance;
    00994          numberOfEntriesDistanceArray++;
    00995        }
    00996
    00997       }
    00998     }
    00999
    01000
    01001   //SOUTHEAST
    01002 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01003     OUTPUT(idText, text,"");
    01004     OUTPUT(idText, text, "SOUTHEAST:");
    01005 #endif
    01006     currentWasSet = false;
    01007     positiv = false;
    01008     distance = 5000;
    01009
    01010     if((start.x < image.cameraInfo.resolutionWidth) && (start.y < image.cameraInfo.resolutionHeight))
    01011     {
    01012     if((start.y < 0) && (start.x >= 0))
    01013       {
    01014         if(start.y + scanLength > 0)
    01015         {
    01016           difference = abs(start.y);
    01017           if((start.x + difference > 0) && (start.x + difference <= image.cameraInfo.resolutionWidth))
    01018           {
    01019             current = Vector2<int>(start.x + difference,0);
    01020             currentWasSet = true;
    01021           }
    01022         }
    01023       }
    01024       else if((start.x < 0) && (start.y >= 0))
    01025       {
    01026         if(start.x + scanLength > 0)
    01027         {
    01028           difference = abs(start.x);
    01029           if((start.y + difference > 0) && (start.y + difference <= image.cameraInfo.resolutionHeight))
    01030           {
    01031             current = Vector2<int>(0, start.y + difference);
    01032             currentWasSet = true;
    01033           }
    01034         }
    01035       }
    01036       else if((start.y < 0) && (start.x < 0))
    01037       {
    01038         if(abs(start.y) == abs(start.x))
    01039         {
    01040           if((start.y + scanLength > 0) && (start.x + scanLength > 0))
    01041           {
    01042             current = Vector2<int>(0,0);
    01043             currentWasSet = true;
    01044           }
    01045         }
    01046         else if(abs(start.y) > abs(start.x))
    01047         {
    01048           if(start.y + scanLength > 0)
    01049           {
    01050             difference = abs(start.y);
    01051             if((start.x + difference > 0) && (start.x + difference < image.cameraInfo.resolutionWidth))
    01052             {
    01053               current = Vector2<int>(start.x + difference, 0);
    01054               currentWasSet = true;
    01055             }
    01056           }
    01057         }
    01058         else if(abs(start.y) < abs(start.x))
    01059         {
    01060           if(start.x + scanLength > 0)
    01061           {
    01062             difference = abs(start.x);
    01063             if((start.y + difference > 0) && (start.y + difference < image.cameraInfo.resolutionHeight))
    01064             {
    01065               current = Vector2<int>(0, start.y + difference);
    01066               currentWasSet = true;
    01067             }
    01068           }
    01069         }
    01070       }
    01071       else if((start.y >= 0) && (start.x >= 0))
    01072       {
    01073         current = start;
    01074         currentWasSet = true;
    01075       }
    01076       if(currentWasSet)
    01077       {
    01078         if((start.y + scanLength > image.cameraInfo.resolutionHeight) && (start.x + scanLength <= image.cameraInfo.resolutionWidth))
    01079         {
    01080           difference = image.cameraInfo.resolutionHeight - start.y;
    01081           end = Vector2<int>(start.x + difference,image.cameraInfo.resolutionHeight);
    01082         }
    01083         else if((start.y + scanLength > image.cameraInfo.resolutionHeight) && (start.x + scanLength > image.cameraInfo.resolutionWidth))
    01084         {
    01085           if(start.x - image.cameraInfo.resolutionWidth == start.y - image.cameraInfo.resolutionHeight )
    01086           {
    01087             end = Vector2<int>(image.cameraInfo.resolutionWidth,image.cameraInfo.resolutionHeight);
    01088           }
    01089           else if(start.x - image.cameraInfo.resolutionWidth < start.y - image.cameraInfo.resolutionHeight)
    01090           {
    01091             difference = image.cameraInfo.resolutionHeight - start.y;
    01092             end = Vector2<int>(start.x + difference, image.cameraInfo.resolutionHeight);
    01093           }
    01094           else if(start.x - image.cameraInfo.resolutionWidth > start.y - image.cameraInfo.resolutionHeight)
    01095           {
    01096             difference = image.cameraInfo.resolutionWidth - start.x;
    01097             end = Vector2<int>(image.cameraInfo.resolutionWidth, start.y + difference);
    01098           }
    01099         }
    01100         else if((start.y + scanLength <= image.cameraInfo.resolutionHeight) && (start.x + scanLength > image.cameraInfo.resolutionWidth))
    01101         {
    01102           difference = image.cameraInfo.resolutionWidth - start.x;
    01103           end = Vector2<int>(image.cameraInfo.resolutionWidth,start.y + difference);
    01104         }
    01105         else
    01106         {
    01107           end = Vector2<int>(start.x + scanLength, start.y + scanLength);
    01108         }
    01109          step = Vector2<int>(1,1);
    01110          scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix,distance,positiv);
    01111          if(positiv)
    01112        {
    01113          positivDistance = max(distance,positivDistance);
    01114          numberOfPositivDistance++;
    01115        }
    01116        else
    01117        {
    01118          distanceArray[numberOfEntriesDistanceArray] = distance;
    01119          numberOfEntriesDistanceArray++;
    01120        }
    01121       }
    01122     }
    01123
    01124
    01125   //NORTHEAST
    01126 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01127     OUTPUT(idText, text,"");
    01128     OUTPUT(idText, text, "NORTHEAST:");
    01129 #endif
    01130     currentWasSet = false;
    01131     positiv = false;
    01132     distance = 5000;
    01133
    01134     if((start.x < image.cameraInfo.resolutionWidth) && (start.y > 0))
    01135     {
    01136       if((start.y > image.cameraInfo.resolutionHeight) && (start.x >= 0))
    01137       {
    01138         if(start.y - scanLength < image.cameraInfo.resolutionHeight)
    01139         {
    01140           difference = start.y - image.cameraInfo.resolutionHeight;
    01141           if((start.x + difference > 0) && (start.x + difference <= image.cameraInfo.resolutionWidth))
    01142           {
    01143             current = Vector2<int>(start.x + difference, image.cameraInfo.resolutionHeight);
    01144             currentWasSet = true;
    01145           }
    01146         }
    01147       }
    01148       else if((start.x < 0) && (start.y <= image.cameraInfo.resolutionHeight))
    01149       {
    01150         if(start.x + scanLength > 0)
    01151         {
    01152           difference = abs(start.x);
    01153           if((start.y - difference > 0) && (start.y - difference <= image.cameraInfo.resolutionHeight))
    01154           {
    01155             current = Vector2<int>(0, start.y - difference);
    01156             currentWasSet = true;
    01157           }
    01158         }
    01159       }
    01160       else if((start.y > image.cameraInfo.resolutionHeight) && (start.x < 0))
    01161       {
    01162         if(start.y - image.cameraInfo.resolutionHeight == abs(start.x))
    01163         {
    01164           if((start.y - scanLength < image.cameraInfo.resolutionHeight) && (start.x + scanLength > 0))
    01165           {
    01166             current = Vector2<int>(0,image.cameraInfo.resolutionHeight);
    01167             currentWasSet = true;
    01168           }
    01169         }
    01170         else if(start.y - image.cameraInfo.resolutionHeight < abs(start.x))
    01171         {
    01172           if(start.x + scanLength > 0)
    01173           {
    01174             difference = abs(start.x);
    01175             if((start.y - difference > 0) && (start.y - difference < image.cameraInfo.resolutionHeight))
    01176             {
    01177               current = Vector2<int>(0, start.y - difference);
    01178               currentWasSet = true;
    01179             }
    01180           }
    01181         }
    01182         else if(start.y - image.cameraInfo.resolutionHeight > abs(start.x))
    01183         {
    01184           if(start.y - scanLength < image.cameraInfo.resolutionHeight)
    01185           {
    01186             difference = start.y - image.cameraInfo.resolutionHeight;
    01187             if((start.x + difference > 0) && (start.x + difference < image.cameraInfo.resolutionWidth))
    01188             {
    01189               current = Vector2<int>(start.x + difference,image.cameraInfo.resolutionHeight);
    01190               currentWasSet = true;
    01191             }
    01192           }
    01193         }
    01194       }
    01195       else if((start.y <= image.cameraInfo.resolutionHeight) && (start.x >= 0))
    01196       {
    01197         current = start;
    01198         currentWasSet = true;
    01199       }
    01200       if(currentWasSet)
    01201       {
    01202         if((start.y - scanLength < 0) && (start.x + scanLength <= image.cameraInfo.resolutionWidth))
    01203         {
    01204           difference = start.y;
    01205           end = Vector2<int>(start.x + difference,0);
    01206         }
    01207         else if((start.y - scanLength < 0) && (start.x + scanLength > image.cameraInfo.resolutionWidth))
    01208         {
    01209           if(start.x - image.cameraInfo.resolutionWidth == abs(start.y))
    01210           {
    01211             end = Vector2<int>(image.cameraInfo.resolutionWidth,0);
    01212           }
    01213           else if(start.x - image.cameraInfo.resolutionWidth < abs(start.y))
    01214           {
    01215             difference = abs(start.y);
    01216             end = Vector2<int>(start.x + difference, 0);
    01217           }
    01218           else if(start.x - image.cameraInfo.resolutionWidth > abs(start.y))
    01219           {
    01220             difference = start.x - image.cameraInfo.resolutionWidth;
    01221             end = Vector2<int>(image.cameraInfo.resolutionWidth, start.y - difference);
    01222           }
    01223         }
    01224         else if((start.y - scanLength >= 0) && (start.x + scanLength > image.cameraInfo.resolutionWidth))
    01225         {
    01226           difference = image.cameraInfo.resolutionWidth - start.x;
    01227           end = Vector2<int>(image.cameraInfo.resolutionWidth,start.y - difference);
    01228         }
    01229         else
    01230         {
    01231           end = Vector2<int>(start.x + scanLength, start.y - scanLength);
    01232         }
    01233          step = Vector2<int>(1,-1);
    01234          scan(current, startOnField, end, step, positiveInNumberOfDirection, cameraMatrix, image, robotPose, prevCameraMatrix,distance,positiv);
    01235          if(positiv)
    01236        {
    01237          positivDistance = max(distance,positivDistance);
    01238          numberOfPositivDistance++;
    01239        }
    01240        else
    01241        {
    01242          distanceArray[numberOfEntriesDistanceArray] = distance;
    01243          numberOfEntriesDistanceArray++;
    01244        }
    01245       }
    01246     }
    01247 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01248    OUTPUT(idText, text, "found positives = " << positiveInNumberOfDirection);
    01249 #endif
    01250    if(positiveInNumberOfDirection >= minNumberOfPositives)
    01251      return(true);
    01252   /* else if(positiveInNumberOfDirection >= minNumberOfPositivesForDistanceCheck)
    01253    {
    01254 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01255      OUTPUT(idText, text, "positive Distance = " << positivDistance);
    01256 #endif
    01257      for(int i = 0; i < numberOfEntriesDistanceArray; i++)
    01258      {
    01259        if(distanceArray[i] <= positivDistance)
    01260          positiveInNumberOfDirection++;
    01261
    01262      }
    01263 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01264      OUTPUT(idText, text, "found positives2 = " << positiveInNumberOfDirection);
    01265 #endif
    01266      if(positiveInNumberOfDirection >= minNumberOfPositives)
    01267        return(true);
    01268      else
    01269        return(false);
    01270    }*/
    01271    else
    01272      return(false);
    01273 }
    01274
    01275  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)
    01276  {
    01277    unsigned char y,u,v;
    01278    Vector2<int> current2 = current;
    01279    bool useCurrent2 = false;
    01280    bool onlyWhite = false;
    01281    int countOnlyWhite = 0;
    01282    colorClass currentColorClass = yellow;
    01283    //drawings of the scan lines
    01284 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01285    LINE(circlePoints_image, current.x, current.y, end.x, end.y,1,1,Drawings::yellow);
    01286 #endif
    01287    //scanning
    01288    //check if startpixel is white
    01289    y = colorCorrector.correct(current.x, current.y, 0, image.image[current.y][0][current.x]);
    01290    u = colorCorrector.correct(current.x, current.y, 1, image.image[current.y][1][current.x]);
    01291    v = colorCorrector.correct(current.x, current.y, 2, image.image[current.y][2][current.x]);
    01292    currentColorClass = COLOR_CLASS(y, u, v, colorTable);
    01293
    01294    while((currentColorClass == white) && (current != end))
    01295    {
    01296       current += step;
    01297       y = colorCorrector.correct(current.x, current.y, 0, image.image[current.y][0][current.x]);
    01298       u = colorCorrector.correct(current.x, current.y, 1, image.image[current.y][1][current.x]);
    01299       v = colorCorrector.correct(current.x, current.y, 2, image.image[current.y][2][current.x]);
    01300       currentColorClass = COLOR_CLASS(y, u, v, colorTable);
    01301
    01302       if((current == end) && (currentColorClass == white))
    01303         onlyWhite = true;
    01304
    01305    }
    01306    if(onlyWhite)
    01307    {
    01308      if(countOnlyWhite <= onlyWhiteAllowed)
    01309      {
    01310        positiveInNumberOfDirection++;
    01311 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01312        CIRCLE(circlePoints_image, current.x, current.y, 3, 1,Drawings::ps_dash, Drawings::red);
    01313        OUTPUT(idText, text, "positiv: ");
    01314        OUTPUT(idText, text, "countOnlyWhite: " << countOnlyWhite);
    01315 #endif
    01316        countOnlyWhite++;
    01317        positiv = true;
    01318      }
    01319    }
    01320    else if((currentColorClass != white) && (current != end))//check the distance between current and start
    01321    {
    01322      Vector2<int> currentOnField;
    01323      if(Geometry::calculatePointOnField(current.x,current.y,cameraMatrix,prevCameraMatrix,image.cameraInfo,currentOnField))
    01324        {
    01325         double distance = sqrt(sqr(startOnField.x - currentOnField.x) + sqr(startOnField.y - currentOnField.y));
    01326         if(distance < maximalDistanceAllowedBetweenMidAndBorder)
    01327         {
    01328           bool whiteFound = false;
    01329           bool greenFound = false;
    01330           int whiteCount = 0;
    01331           while((current != end) && !(whiteFound))
    01332           {
    01333             y = colorCorrector.correct(current.x, current.y, 0, image.image[current.y][0][current.x]);
    01334             u = colorCorrector.correct(current.x, current.y, 1, image.image[current.y][1][current.x]);
    01335             v = colorCorrector.correct(current.x, current.y, 2, image.image[current.y][2][current.x]);
    01336             currentColorClass = COLOR_CLASS(y, u, v, colorTable);
    01337             if(currentColorClass == white)
    01338             {
    01339               if(greenFound)
    01340                whiteCount++;
    01341             }
    01342             else if(currentColorClass == green)
    01343             {
    01344               if(!greenFound)
    01345               {
    01346                 greenFound = true;
    01347 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01348                 OUTPUT(idText, text, "greenFound1 ");
    01349 #endif
    01350               }
    01351               else
    01352               {
    01353                 whiteCount = 0;
    01354               }
    01355             }
    01356             else
    01357             {
    01358               whiteCount = 0;
    01359             }
    01360             if(whiteCount == neededWhiteCount)
    01361               whiteFound = true;
    01362             current += step;
    01363           }//while((current != end) && (whiteCount != neededWhiteCount)
    01364           if(whiteFound)
    01365           {
    01366 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01367             OUTPUT(idText, text, "whiteFound ");
    01368 #endif
    01369             //check distance
    01370             if(Geometry::calculatePointOnField(current.x,current.y,cameraMatrix,prevCameraMatrix,image.cameraInfo,currentOnField))
    01371             {
    01372               double distance = sqrt(sqr(startOnField.x - currentOnField.x) + sqr(startOnField.y - currentOnField.y));
    01373 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01374               OUTPUT(idText, text, "distance: "  << distance);
    01375 #endif
    01376               if(distance < maximalDistanceAllowedBetweenMidAndBorder)
    01377               {
    01378                 if(current != end)
    01379                 {
    01380                   useCurrent2 = true;
    01381                   current2 = current;
    01382                   current2 += step;
    01383                   y = colorCorrector.correct(current.x, current.y, 0, image.image[current.y][0][current.x]);
    01384                   u = colorCorrector.correct(current.x, current.y, 1, image.image[current.y][1][current.x]);
    01385                   v = colorCorrector.correct(current.x, current.y, 2, image.image[current.y][2][current.x]);
    01386                   currentColorClass = COLOR_CLASS(y, u, v, colorTable);
    01387                   while((current2 != end) && (currentColorClass != green))
    01388                   {
    01389                     current2 += step;
    01390                     y = colorCorrector.correct(current.x, current.y, 0, image.image[current.y][0][current.x]);
    01391                     u = colorCorrector.correct(current.x, current.y, 1, image.image[current.y][1][current.x]);
    01392                     v = colorCorrector.correct(current.x, current.y, 2, image.image[current.y][2][current.x]);
    01393                     currentColorClass = COLOR_CLASS(y, u, v, colorTable);
    01394                   }
    01395                   if(currentColorClass == green)
    01396                   {
    01397 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01398                     OUTPUT(idText, text, "greenFound2 ");
    01399 #endif
    01400                     positiveInNumberOfDirection++;
    01401                     positiv = true;
    01402 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01403                     LINE(circlePoints_image, current.x, current.y, current2.x, current2.y, 1, 20, Drawings::pink);
    01404 #endif
    01405                   }
    01406                 }
    01407               }
    01408             }
    01409           }//if(whiteFound)
    01410         }//if(distance < maximalDistanceAllowedBetweenMidAndBorder)
    01411       }// if(Geometry::calculatePointOnField(current.x,current.y,cameraMatrix,prevCameraMatrix,image.cameraInfo,currentOnField))
    01412    }//else if((currentColorClass != white) && (current != end))//check the distance between current and start
    01413    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)))
    01414   {
    01415     Vector2<int> currentOnField;
    01416     if(useCurrent2)
    01417     {
    01418       if(Geometry::calculatePointOnField(current2.x,current2.y,cameraMatrix,prevCameraMatrix,image.cameraInfo,currentOnField))
    01419       {
    01420         endDistance = sqrt(sqr(startOnField.x - currentOnField.x) + sqr(startOnField.y - currentOnField.y));
    01421 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01422         OUTPUT(idText, text, "border-distance: " << endDistance);
    01423         CIRCLE(circlePoints_image, current2.x, current2.y, 3, 1,Drawings::ps_dash, Drawings::pink);
    01424 #endif
    01425       }
    01426     }
    01427     else
    01428     {
    01429       if(Geometry::calculatePointOnField(current.x,current.y,cameraMatrix,prevCameraMatrix,image.cameraInfo,currentOnField))
    01430       {
    01431         endDistance = sqrt(sqr(startOnField.x - currentOnField.x) + sqr(startOnField.y - currentOnField.y));
    01432 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01433         OUTPUT(idText, text, "border-distance: " << endDistance);
    01434         CIRCLE(circlePoints_image, current.x, current.y, 3, 1,Drawings::ps_dash, Drawings::pink);
    01435 #endif
    01436       }
    01437     }
    01438   }
    01439   else if(!positiv)
    01440   {
    01441     endDistance = 5000;
    01442   }
    01443   else if(positiv)
    01444   {
    01445     Vector2<int> currentOnField;
    01446     if(Geometry::calculatePointOnField(current.x,current.y,cameraMatrix,prevCameraMatrix,image.cameraInfo,currentOnField))
    01447     {
    01448       endDistance = sqrt(sqr(startOnField.x - currentOnField.x) + sqr(startOnField.y - currentOnField.y));
    01449 #ifdef DEBUG_CENTER_CIRCLE_WITH_LOGS
    01450       OUTPUT(idText, text, "border-distance-positive: " << endDistance);
    01451 #endif
    01452     }
    01453   }
    01454 }
    01455
    01456
    01457
    01458
    01459
    01460
    01461
    01462