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

# Modules/ImageProcessor/GT2005ImageProcessor/GT2005BallSpecialist.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005BallSpecialist.cpp
    00003 * This file contains a class for Image Processing.
    00004 * @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Juengel</A>
    00005 * @author Max Risler
    00006 * @author <A href=mailto:brunn@sim.tu-darmstadt.de>Ronnie Brunn</A>
    00007 * @author <A href=mailto:mkunz@sim.tu-darmstadt.de>Michael Kunz</A>
    00008 * @author <a href="mailto:walter.nistico@uni-dortmund.de">Walter Nistico</a>
    00009 */
    00010
    00011 #include "GT2005BallSpecialist.h"
    00012
    00013 #include "Tools/FieldDimensions.h"
    00014 #include "Tools/Math/MVTools.h"
    00015 #include "Tools/Math/Matrix_nxn.h"
    00016 #include "Tools/Math/Vector_n.h"
    00017 #include "Tools/Debugging/DebugDrawings.h"
    00018 #include "Tools/Debugging/Debugging.h"
    00019 #include "GT2005ImageProcessorTools.h"
    00020 #include "Platform/GTAssert.h"
    00021 #include "Tools/Math/Common.h"
    00022
    00023
    00024 GT2005BallSpecialist::GT2005BallSpecialist
    00025 (
    00026  const ColorCorrector& colorCorrector
    00027  )
    00028  :
    00029 colorCorrector(colorCorrector)
    00030 {
    00031
    00032 }
    00033
    00034 void GT2005BallSpecialist::searchBall
    00035 (
    00036  const Image& image,
    00037  const ColorTable& colorTable,
    00038  const CameraMatrix& cameraMatrix,
    00039  const CameraMatrix& prevCameraMatrix,
    00040  int x, int y,
    00041  BallPercept& ballPercept
    00042  )
    00043 {
    00044   BallPointList ballPoints;
    00045   Vector2<int> center;
    00046   double radius;
    00047   int countOrange = 0;
    00048   int countAmbiguous = 0;
    00049   int maxOrangePerLine = 0;
    00050   int countPixel = 0;
    00051   CameraInfo bwCameraInfo = image.cameraInfo;
    00052   bwCameraInfo.resolutionHeight*=2;
    00053   bwCameraInfo.resolutionWidth*=2;
    00054   bwCameraInfo.opticalCenter.x*=2;
    00055   bwCameraInfo.opticalCenter.y*=2;
    00056   bwCameraInfo.focalLength*=2;
    00057   bwCameraInfo.focalLengthInv/=2;
    00058
    00059   scanForBallPoints(image, bwCameraInfo, colorTable, x, y, ballPoints, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
    00060
    00061   //ignore ball if not enough points are orange
    00062   //OUTPUT (idText, text, "GT2005BallSpecialist: " << countOrange * 100 / countPixel << "% orange");
    00063   int i;
    00064   int numberOfSoftEdgePoints = 0;
    00065   int numberOfPointsInYellow = 0;
    00066
    00067   for(i = 0; i < ballPoints.number; i++)
    00068   {
    00069     if (ballPoints[i].yellowIsClose && !ballPoints[i].atBorder) numberOfPointsInYellow++;
    00070     if (!ballPoints[i].hardEdge && !ballPoints[i].atBorder) numberOfSoftEdgePoints++;
    00071   }
    00072   /*  OUTPUT (idText, text, "GT2005BallSpecialist: " << endl << "----------------------------" << endl
    00073   << "Ball Points: " << ballPoints.number << endl
    00074   << "Yellow Points: " << (numberOfPointsInYellow*100)/ballPoints.number << "%" << endl
    00075   << "SoftEdge Points: " << (numberOfSoftEdgePoints*100)/ballPoints.number << "%" << endl << endl
    00076   << "Orange Pixel: " << (countOrange*100)/countPixel << "%" << endl
    00077   << "Ambiguous Pixel: " << (countAmbiguous*100)/countPixel << "%" << endl
    00078   );*/
    00079
    00080   if ((countOrange > countAmbiguous) &&
    00081     (countOrange * 6 > countPixel) &&                          // >  16.66% orange
    00082 //    (numberOfSoftEdgePoints * 5 <= ballPoints.number * 2) &&   // <= 40% with soft edge
    00083     (numberOfPointsInYellow * 4 <= ballPoints.number * 3)      // <= 75% with Yellow close
    00084     )
    00085   {
    00086
    00087   considerBallPoints(ballPoints);
    00088
    00089
    00090     //try only points near green with hard edge first
    00091     BallPointList testPoints;
    00092     for(i = 0; i < ballPoints.number; i++)
    00093     {
    00094       if (ballPoints[i].greenIsClose && ballPoints[i].hardEdge)
    00095         testPoints.add(ballPoints[i]);
    00096     }
    00097     if (
    00098       testPoints.number *2 >= ballPoints.number &&
    00099       createBallPerceptLevenbergMarquardt(testPoints, center, radius) &&
    00100       checkIfPointsAreInsideBall(ballPoints, center, radius))
    00101     {
    00102       addBallPercept(image, bwCameraInfo, colorTable, cameraMatrix, prevCameraMatrix, center, radius, ballPercept);
    00103     }
    00104     else
    00105     {
    00106       //now all points if not at border with hard edge
    00107       testPoints.number = 0;
    00108       for(i = 0; i < ballPoints.number; i++)
    00109       {
    00110         if (!ballPoints[i].atBorder && ballPoints[i].hardEdge)
    00111           testPoints.add(ballPoints[i]);
    00112       }
    00113       if (
    00114         createBallPerceptLevenbergMarquardt(testPoints, center, radius) &&
    00115         checkIfPointsAreInsideBall(ballPoints, center, radius))
    00116       {
    00117         addBallPercept(image, bwCameraInfo, colorTable, cameraMatrix, prevCameraMatrix, center, radius, ballPercept);
    00118       }
    00119       else
    00120       {
    00121         //now all points if not at border
    00122         testPoints.number = 0;
    00123         for(i = 0; i < ballPoints.number; i++)
    00124         {
    00125           if (!ballPoints[i].atBorder)
    00126             testPoints.add(ballPoints[i]);
    00127         }
    00128         if (
    00129           createBallPerceptLevenbergMarquardt(testPoints, center, radius) &&
    00130           checkIfPointsAreInsideBall(ballPoints, center, radius))
    00131         {
    00132           addBallPercept(image, bwCameraInfo, colorTable, cameraMatrix, prevCameraMatrix, center, radius, ballPercept);
    00133         }
    00134         else
    00135         {
    00136           //take all points if nothing else works
    00137           if (createBallPerceptLevenbergMarquardt(ballPoints, center, radius))
    00138           {
    00139       calculateDeviationOfBallPoints(ballPoints, center, radius);
    00140             addBallPercept(image, bwCameraInfo, colorTable, cameraMatrix, prevCameraMatrix, center, radius, ballPercept);
    00141           }
    00142         }
    00143       }
    00144     }
    00145   }
    00146
    00147   DEBUG_DRAWING_FINISHED(imageProcessor_ball1);
    00148   DEBUG_DRAWING_FINISHED(imageProcessor_ball2);
    00149 }
    00150
    00151
    00152 void GT2005BallSpecialist::BallPointList::add(const BallPoint& ballPoint)
    00153 {
    00154   ASSERT(number < maxNumberOfPoints);
    00155   ballPoints[number++] = ballPoint;
    00156   DOT(imageProcessor_ball2, ballPoint.x / 2, ballPoint.y / 2,
    00157     (ballPoint.hardEdge) ? Drawings::blue : Drawings::orange,
    00158     (ballPoint.atBorder) ? Drawings::black :
    00159   (ballPoint.greenIsClose) ? Drawings::green :
    00160   (ballPoint.yellowIsClose) ? Drawings::yellow :
    00161   Drawings::white
    00162     );
    00163 }
    00164
    00165 void GT2005BallSpecialist::scanForBallPoints
    00166 (
    00167  const Image& image,
    00168  const CameraInfo& bwCameraInfo,
    00169  const ColorTable& colorTable,
    00170  int x, int y,
    00171  BallPointList& ballPoints,
    00172  int& countAmbiguous,
    00173  int& countOrange,
    00174  int& maxOrangePerLine,
    00175  int& countPixel
    00176  )
    00177 {
    00178   // search for ball variables
    00179   BallPoint north;
    00180   BallPoint east;
    00181   BallPoint south;
    00182   BallPoint west;
    00183
    00184   BallPoint start;
    00185   Vector2<int>step;
    00186   BallPoint destination;
    00187
    00188   //  int xStep;
    00189   //  int yStep;
    00190
    00191   start.x = x * 2; start.y = y * 2;
    00192   BallPoint start2;
    00193
    00194   DOT(imageProcessor_ball2, x, y, Drawings::black, Drawings::white);
    00195
    00196   //find north ///////////////////////////////////////////
    00197   step.x = 0; step.y = -1;
    00198   findEndOfBall(image, bwCameraInfo, colorTable, start, step, north, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
    00199   if(north.atBorder)
    00200   {
    00201     start2 = north - step;
    00202     //find east
    00203     step.x = 1; step.y = 0;
    00204     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    00205     {
    00206       ballPoints.add(destination);
    00207     }
    00208     //find west
    00209     step.x = -1; step.y = 0;
    00210     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    00211     {
    00212       ballPoints.add(destination);
    00213     }
    00214   }
    00215   else
    00216   {
    00217     ballPoints.add(north);
    00218   }
    00219
    00220   //find east ///////////////////////////////////////////
    00221   step.x = 1; step.y = 0;
    00222   findEndOfBall(image, bwCameraInfo, colorTable, start, step, east, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
    00223   if(east.atBorder)
    00224   {
    00225     start2 = east - step;
    00226     //find north
    00227     step.x = 0; step.y = -1;
    00228     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    00229     {
    00230       ballPoints.add(destination);
    00231     }
    00232     //find south
    00233     step.x = 0; step.y = 1;
    00234     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    00235     {
    00236       ballPoints.add(destination);
    00237     }
    00238   }
    00239   else
    00240   {
    00241     ballPoints.add(east);
    00242   }
    00243
    00244   //find south ///////////////////////////////////////////
    00245   step.x = 0; step.y = 1;
    00246   findEndOfBall(image, bwCameraInfo, colorTable, start, step, south, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
    00247   if(south.atBorder)
    00248   {
    00249     start2 = south - step;
    00250     //find east
    00251     step.x = 1; step.y = 0;
    00252     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    00253     {
    00254       ballPoints.add(destination);
    00255     }
    00256     //find west
    00257     step.x = -1; step.y = 0;
    00258     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    00259     {
    00260       ballPoints.add(destination);
    00261     }
    00262   }
    00263   else
    00264   {
    00265     ballPoints.add(south);
    00266   }
    00267
    00268   //find west ///////////////////////////////////////////
    00269   step.x = -1; step.y = 0;
    00270   findEndOfBall(image, bwCameraInfo, colorTable, start, step, west, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
    00271   if(west.atBorder)
    00272   {
    00273     start2 = west - step;
    00274     //find north
    00275     step.x = 0; step.y = -1;
    00276     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    00277     {
    00278       ballPoints.add(destination);
    00279     }
    00280     //find south
    00281     step.x = 0; step.y = 1;
    00282     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    00283     {
    00284       ballPoints.add(destination);
    00285     }
    00286   }
    00287   else
    00288   {
    00289     ballPoints.add(west);
    00290   }
    00291
    00292   //
    00293   if( (south.y - north.y) > (east.x - west.x) )
    00294   {
    00295     if ((north.y + south.y) / 2 != start.y)
    00296     {
    00297       start.y = (north.y + south.y) / 2;
    00298       //find east
    00299       step.x = 1; step.y = 0;
    00300       findEndOfBall(image, bwCameraInfo, colorTable, start, step, east, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
    00301       if(!east.atBorder)
    00302       {
    00303         ballPoints.add(east);
    00304       }
    00305       //find west
    00306       step.x = -1; step.y = 0;
    00307       findEndOfBall(image, bwCameraInfo, colorTable, start, step, west, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
    00308       if (!west.atBorder)
    00309       {
    00310         ballPoints.add(west);
    00311       }
    00312       //////////
    00313       start.x = (west.x + east.x) / 2;
    00314     }
    00315   }
    00316   else
    00317   {
    00318     if ((west.x + east.x) / 2 != start.x)
    00319     {
    00320       start.x = (west.x + east.x) / 2;
    00321       //find north
    00322       step.x = 0; step.y = -1;
    00323       findEndOfBall(image, bwCameraInfo, colorTable, start, step, north, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
    00324       if(!north.atBorder)
    00325       {
    00326         ballPoints.add(north);
    00327       }
    00328       //find south
    00329       step.x = 0; step.y = 1;
    00330       findEndOfBall(image, bwCameraInfo, colorTable, start, step, south, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
    00331       if(!south.atBorder)
    00332       {
    00333         ballPoints.add(south);
    00334       }
    00335       //////////
    00336       start.y = (north.y + south.y) / 2;
    00337     }
    00338   }
    00339   //
    00340
    00341   // find in diagonal
    00342
    00343   //  for(xStep = -1; xStep <= 1; xStep += 2)
    00344   for (step.x = -1; step.x <= 1; step.x += 2)
    00345   {
    00346     //for(yStep = -1; yStep <= 1; yStep += 2)
    00347     for (step.y = -1; step.y <= 1; step.y += 2)
    00348     {
    00349       //step.x = xStep; step.y = yStep;
    00350       findEndOfBall(image, bwCameraInfo, colorTable, start, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
    00351       if (!destination.atBorder)
    00352       {
    00353         ballPoints.add(destination);
    00354       }
    00355     } //for(int yStep ...
    00356   } //for(int xStep ...
    00357 }
    00358
    00359 bool GT2005BallSpecialist::findEndOfBall
    00360 (
    00361  const Image& image,
    00362  const CameraInfo& bwCameraInfo,
    00363  const ColorTable& colorTable,
    00364  const BallPoint& start,
    00365  const Vector2<int>& step,
    00366  BallPoint& destination,
    00367  int& countAmbiguous,
    00368  int& countOrange,
    00369  int& maxOrangePerLine,
    00370  int& countPixel
    00371  )
    00372 {
    00373 /*
    00374 stopColors are colors indicating the end of the ball:
    00375 green, yellow, skyblue
    00376 more than 3 pixels of a stopColor indicate the end of the ball
    00377   */
    00378   int stopColorCounter = 0;
    00379   int currentOrange = 0;
    00380   int currentAmbiguous = 0;
    00381   int len = 0;
    00382   int stopLen = 0;
    00383 //  bool isAmbiguous = false;
    00384
    00385   colorClass currentColorClass;
    00386
    00387   Vector2<int> firstStopColor = start;
    00388   unsigned char currentOrangeSim = 0;
    00389   Vector2<int> lastPoint = start;
    00390   destination = start;
    00391   destination.greenIsClose = false;
    00392   destination.yellowIsClose = false;
    00393   destination.atBorder = false;
    00394   destination.hardEdge = true;
    00395
    00396   bool isOrange = false;
    00397
    00398   bool goOn = true;
    00399   while(goOn)
    00400   {
    00401     lastPoint = destination;
    00402     destination += step;
    00403     /////////////
    00404     if(destination.x < 0 || destination.x >= bwCameraInfo.resolutionWidth ||
    00405       destination.y < 0 || destination.y >= bwCameraInfo.resolutionHeight-1) // avoid artefact at bottom of hires image
    00406     {
    00407       if (stopColorCounter == 0)
    00408       {
    00409         countPixel += len;
    00410         destination.atBorder = true;
    00411       }
    00412       else
    00413       {
    00414         destination = firstStopColor;
    00415         countPixel += stopLen;
    00416         destination.atBorder = false;
    00417       }
    00418       goOn = false;
    00419     }
    00420     else
    00421     {
    00422       currentColorClass = CORRECTED_COLOR_CLASS(
    00423         destination.x / 2,destination.y / 2,
    00424         image.getHighResY(destination.x,destination.y),
    00425         image.image[destination.y / 2][1][destination.x / 2],
    00426         image.image[destination.y / 2][2][destination.x / 2],
    00427         colorTable, colorCorrector);
    00428
    00429       // counting all orange pixels on the scanned horz./vert./diag. lines
    00430       len++;
    00431       if ( currentColorClass == orange )
    00432       {
    00433         currentOrange++;
    00434       }
    00435
    00436     /*
    00437       isAmbiguous = (currentColorClass == pink ||
    00438         currentColorClass == yellow ||
    00439         currentColorClass == red
    00440         );
    00441
    00442       if (isAmbiguous)
    00443     */
    00444     if (currentColorClass == pink || currentColorClass == yellow || currentColorClass == red)
    00445       {
    00446         currentAmbiguous++;
    00447       }
    00448
    00449       LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::green);
    00450
    00451       if (currentColorClass == orange)
    00452         isOrange = true;
    00453       else
    00454       {
    00455         currentOrangeSim = getSimilarityToOrange(
    00456           colorCorrector.correct(destination.x / 2,destination.y / 2, 0, image.getHighResY(destination.x,destination.y)),
    00457           colorCorrector.correct(destination.x / 2,destination.y / 2, 1, image.image[destination.y / 2][1][destination.x / 2]),
    00458           colorCorrector.correct(destination.x / 2,destination.y / 2, 2, image.image[destination.y / 2][2][destination.x / 2])
    00459           );
    00460         isOrange = currentOrangeSim > 30;
    00461       }
    00462
    00463       if(currentColorClass == green ||
    00464         currentColorClass == yellow ||
    00465         currentColorClass == skyblue ||
    00466         currentColorClass == red ||
    00467         currentColorClass == blue ||
    00468         //        currentColorClass == pink || //added
    00469         //removed because there is pink in the ball and I have removed pink from orange similarity
    00470         !isOrange
    00471         )
    00472       {
    00473         if (stopColorCounter == 0)
    00474         {
    00475           firstStopColor = destination;
    00476           stopLen = len;
    00477         }
    00478         if (currentColorClass == green) destination.greenIsClose = true;
    00479         if (currentColorClass == yellow) destination.yellowIsClose = true;
    00480         stopColorCounter++;
    00481
    00482         if (isOrange)
    00483         {
    00484           LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::orange);
    00485         }
    00486         else
    00487         {
    00488           LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::red);
    00489         }
    00490
    00491         if(stopColorCounter > 8)
    00492         {
    00493           destination = firstStopColor;
    00494           countPixel += stopLen;
    00495           goOn = false;
    00496         }
    00497       }
    00498       else
    00499       {
    00500         destination.greenIsClose = false;
    00501         destination.yellowIsClose = false;
    00502         stopColorCounter = 0;
    00503       }
    00504
    00505     } // else destination in range
    00506   } //  while(goOn)
    00507
    00508   destination -= step;
    00509
    00510   // compute sum of all orange pixels and max-pixels-per-line
    00511   countOrange += currentOrange;
    00512   countAmbiguous += currentAmbiguous;
    00513   maxOrangePerLine = max ( maxOrangePerLine, currentOrange);
    00514   if (destination.greenIsClose) destination.yellowIsClose = false;
    00515
    00516   /*  if (!destination.atBorder)
    00517   {
    00518   OUTPUT(idText, text, "Point(" << destination.x << "/" << destination.y << "): " << lastGoodColorOrangeSim << " -> " << currentOrangeSim);
    00519   }*/
    00520   if (!destination.atBorder)
    00521   {
    00522       unsigned char lastGoodColorOrangeSim = getSimilarityToOrange(
    00523       colorCorrector.correct(destination.x / 2,destination.y / 2, 0, image.getHighResY(destination.x,destination.y)),
    00524       colorCorrector.correct(destination.x / 2,destination.y / 2, 1, image.image[destination.y / 2][1][destination.x / 2]),
    00525       colorCorrector.correct(destination.x / 2,destination.y / 2, 2, image.image[destination.y / 2][2][destination.x / 2])
    00526       );
    00527     destination.hardEdge = (2 * currentOrangeSim < lastGoodColorOrangeSim);
    00528   }
    00529   else
    00530     destination.hardEdge = false;
    00531
    00532   if (!destination.atBorder)
    00533   {
    00534     if (destination.greenIsClose)
    00535     {
    00536       LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::blue);
    00537     }
    00538     else
    00539       if (destination.yellowIsClose)
    00540       {
    00541         LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::skyblue);
    00542       }
    00543       else
    00544       {
    00545         LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::pink);
    00546       }
    00547   }
    00548   return true;
    00549 }
    00550
    00551
    00552
    00553 bool GT2005BallSpecialist::createBallPerceptLevenbergMarquardt
    00554 (
    00555  const BallPointList& ballPoints,
    00556  Vector2<int>& center,
    00557  double& radius
    00558  )
    00559 {
    00560   if (ballPoints.number < 3)
    00561     return false;
    00562
    00563   double Mx = 0.0;
    00564   double My = 0.0;
    00565   double Mxx = 0.0;
    00566   double Myy = 0.0;
    00567   double Mxy = 0.0;
    00568   double Mz = 0.0;
    00569   double Mxz = 0.0;
    00570   double Myz = 0.0;
    00571
    00572   for (int i = 0; i < ballPoints.number; ++i)
    00573   {
    00574     double x = ballPoints[i].x;
    00575     double y = ballPoints[i].y;
    00576     double xx = x*x;
    00577     double yy = y*y;
    00578     double z = xx + yy;
    00579
    00580     Mx += x;
    00581     My += y;
    00582     Mxx += xx;
    00583     Myy += yy;
    00584     Mxy += x*y;
    00585     Mz += z;
    00586     Mxz += x*z;
    00587     Myz += y*z;
    00588   }
    00589
    00590   try
    00591   {
    00592     Matrix_nxn<double, 3> M;
    00593     double Matrix[9] =
    00594     {
    00595       Mxx, Mxy, Mx,
    00596         Mxy, Myy, My,
    00597         Mx, My, ballPoints.number
    00598     };
    00599     M = Matrix;
    00600
    00601     Vector_n<double, 3> v;
    00602
    00603     v[0] = -Mxz;
    00604     v[1] = -Myz;
    00605     v[2] = -Mz;
    00606
    00607     Vector_n<double, 3> BCD;
    00608     BCD = M.solve(v);
    00609
    00610     center.x = (int)(-BCD[0] / 2.0);
    00611     center.y = (int)(-BCD[1] / 2.0);
    00612
    00613     double tmpWurzel = BCD[0]*BCD[0]/4.0 + BCD[1]*BCD[1]/4.0 - BCD[2];
    00614
    00615     if (tmpWurzel < 0.0)
    00616       return false;
    00617
    00618     radius = sqrt(tmpWurzel);
    00619   }
    00620   catch (MVException)
    00621   {
    00622     return false;
    00623   }
    00624   catch (...)
    00625   {
    00626     OUTPUT(idText, text, "Unknown exception in GT2005BallSpecialist::createBallPerceptsFromXPoints");
    00627     return false;
    00628   }
    00629
    00630   return true;
    00631 }
    00632
    00633 bool GT2005BallSpecialist::checkIfPointsAreInsideBall(const BallPointList& ballPoints, Vector2<int>& center, double radius)
    00634 {
    00635   double d;
    00636   int numberOfNonBorderBallPoints = 0;
    00637   double dev = 0;
    00638   for(int i = 0; i < ballPoints.number; i++)
    00639   {
    00640     d = Geometry::distance(center, ballPoints[i]);
    00641     if (d > radius * 1.1)
    00642     {
    00643       return false;
    00644     }
    00645     if (!ballPoints[i].atBorder)
    00646     {
    00647       numberOfNonBorderBallPoints++;
    00648       dev += fabs(d-radius);
    00649     }
    00650   }
    00651   if (numberOfNonBorderBallPoints==0)
    00652   {
    00653     deviationOfBallPoints = 0;
    00654   }
    00655   else
    00656   {
    00657     deviationOfBallPoints = dev / numberOfNonBorderBallPoints;
    00658   }
    00659   return true;
    00660 }
    00661
    00662
    00663 void GT2005BallSpecialist::calculateDeviationOfBallPoints(const BallPointList& ballPoints, Vector2<int>& center, double radius)
    00664 {
    00665   double d;
    00666   int numberOfNonBorderBallPoints = 0;
    00667   double dev = 0;
    00668   for(int i = 0; i < ballPoints.number; i++)
    00669   {
    00670     d = Geometry::distance(center, ballPoints[i]);
    00671     if (!ballPoints[i].atBorder)
    00672   {
    00673     numberOfNonBorderBallPoints++;
    00674     dev += fabs(d-radius);
    00675   }
    00676   }
    00677   if (numberOfNonBorderBallPoints==0)
    00678   {
    00679     deviationOfBallPoints = 0;
    00680   }
    00681   else
    00682   {
    00683     deviationOfBallPoints = dev / numberOfNonBorderBallPoints;
    00684   }
    00685 }
    00686
    00687 void GT2005BallSpecialist::considerBallPoints(const BallPointList& ballPoints)
    00688 {
    00689   int numberOfNearGreenPoints = 0;
    00690   int numberOfNearYellowPoints = 0;
    00691   int numberOfHardEdgePoints = 0;
    00692   int numberOfNonBorderPoints = 0;
    00693
    00694   for (int i=0; i<ballPoints.number; i++)
    00695   {
    00696     if (!ballPoints[i].atBorder)
    00697     {
    00698       if (ballPoints[i].hardEdge)
    00699       {
    00700         numberOfHardEdgePoints++;
    00701       }
    00702       if (ballPoints[i].greenIsClose)
    00703       {
    00704         numberOfNearGreenPoints++;
    00705       }
    00706       if (ballPoints[i].yellowIsClose)
    00707       {
    00708         numberOfNearYellowPoints++;
    00709       }
    00710       numberOfNonBorderPoints++;
    00711     }
    00712   }
    00713   if (numberOfNonBorderPoints>0)
    00714   {
    00715     durabilityOfBallPoints=1.0;
    00716     if (!(numberOfHardEdgePoints>=3 && numberOfNearGreenPoints*2>numberOfNonBorderPoints))
    00717       durabilityOfBallPoints *= 0.8;
    00718     if (numberOfHardEdgePoints*3<numberOfNonBorderPoints*2)   // less than 2/3 hard edge
    00719         durabilityOfBallPoints *= 0.9;
    00720     if (numberOfHardEdgePoints*3<numberOfNonBorderPoints)     // less than 1/3 hard edge
    00721       durabilityOfBallPoints *= 0.9;
    00722     if (numberOfNearGreenPoints*4<numberOfNonBorderPoints)    // less than 1/4 near green
    00723       durabilityOfBallPoints *= 0.5;
    00724     if (numberOfNearGreenPoints==0)                           // no points near green
    00725       durabilityOfBallPoints *= 0.5;
    00726     if (numberOfNearYellowPoints*2>numberOfNonBorderPoints)   // more than 1/2 near yellow
    00727       durabilityOfBallPoints *= 0.9;
    00728     if (numberOfNonBorderPoints<4)                            // only 1-3 non border points
    00729       durabilityOfBallPoints *= 0.5;
    00730   }
    00731   else
    00732   {// there are only border ball points, so the ball is right in front of the camera, and the durability can stay 1.0
    00733     durabilityOfBallPoints=1.0;
    00734   }
    00735
    00736 }
    00737
    00738
    00739 double GT2005BallSpecialist::calculateReliability(
    00740   double percentOfOrange,
    00741   double radius,
    00742   Vector2<double> anglesToCenter,
    00743   const CameraInfo bwCameraInfo,
    00744   const CameraMatrix& cameraMatrix,
    00745   const CameraMatrix& prevCameraMatrix
    00746   )
    00747 {
    00748   double reliability = 1.0;
    00749   double distanceFromRadius = Geometry::getBallDistanceByAngleSize(int(2 * ballRadius), 2 * Geometry::pixelSizeToAngleSize(radius, bwCameraInfo));
    00750   double distanceFromAngle;
    00751   if (anglesToCenter.y < 0)
    00752   {
    00753     distanceFromAngle = (cameraMatrix.translation.z - ballRadius) / tan(-anglesToCenter.y); // tan() like used in the function in the ballPercept class. I think sin() should be correct, but at this small angles it should make no difference
    00754   }
    00755   else
    00756   {
    00757     distanceFromAngle = 0;
    00758   }
    00759   double distanceReliability = max(0.001, 1.0-fabs((distanceFromRadius-distanceFromAngle)/(distanceFromRadius+distanceFromAngle)));
    00760
    00761   if (deviationOfBallPoints/radius<0.1 && durabilityOfBallPoints>0.9 && percentOfOrange>0.9 && distanceReliability>0.9)
    00762   {
    00763     reliability = 1.0;
    00764   }
    00765   else
    00766   {
    00767     reliability = 1.0 * durabilityOfBallPoints * percentOfOrange * distanceReliability * (1-deviationOfBallPoints/radius);
    00768   }
    00769   /*
    00770   OUTPUT(idText,text,"Reliability calculation of ball percept: ");
    00771   OUTPUT(idText,text,"deviationOfBallPoints/radius: " << deviationOfBallPoints/radius);
    00772   OUTPUT(idText,text,"durabilityOfBallPoints: " << durabilityOfBallPoints);
    00773   OUTPUT(idText,text,"percentOfOrange: " << percentOfOrange);
    00774   OUTPUT(idText,text,"distanceReliability: " << distanceReliability);
    00775   OUTPUT(idText,text,"-->reliability: " << reliability);
    00776   */
    00777
    00778   return reliability;
    00779 }
    00780
    00781
    00782
    00783 void GT2005BallSpecialist::addBallPercept
    00784 (
    00785  const Image& image,
    00786  const CameraInfo& bwCameraInfo,
    00787  const ColorTable& colorTable,
    00788  const CameraMatrix& cameraMatrix,
    00789  const CameraMatrix& prevCameraMatrix,
    00790  const Vector2<int>& center,
    00791  double radius,
    00792  BallPercept& ballPercept
    00793  )
    00794 {
    00795   // test if ball is below horizon
    00796   double factor = bwCameraInfo.focalLengthInv;
    00797   Vector3<double>
    00798     vectorToCenter(1, (bwCameraInfo.opticalCenter.x - center.x) * factor, (bwCameraInfo.opticalCenter.y - center.y) * factor);
    00799   Vector3<double>
    00800     vectorToCenterWorld = cameraMatrix.rotation * vectorToCenter;
    00801
    00802   //Is the point above the horizon ? - return
    00803   //  if(vectorToCenterWorld.z <= -5 * factor) //for perfect horizon
    00804   if(vectorToCenterWorld.z < 1 * factor)
    00805   {
    00806   double percentOfOrange = 1.0; // big balls are already scanned enough. They are supposed to have nearly 100% orange inside
    00807     if (radius <= 12.0)
    00808     {//small ghost balls can easily appear on an orange fringe of a pink/yellow landmark
    00809       // this because the scan follows along the fringe; this new scan starts from the
    00810       // hypothetical center of the "ghost" ball, which in such case lies inside the landmark
    00811       // and hence produces a high number of "non orange" points; a true ball however,
    00812       // should have enough orange inside
    00813       Vector2<int> current(center), clippedCenter(center);
    00814       int orangeCount = 0;
    00815       int totalCount = 0;
    00816       double scanAngle = 0;
    00817       colorClass currentColor;
    00818       Geometry::clipPointInsideRectange(Vector2<int>(0,0), Vector2<int>(bwCameraInfo.resolutionWidth,bwCameraInfo.resolutionHeight), clippedCenter);
    00819       currentColor = CORRECTED_COLOR_CLASS(
    00820       clippedCenter.x/2, clippedCenter.y/2,
    00821       image.getHighResY(clippedCenter.x,clippedCenter.y),
    00822       image.image[clippedCenter.y/2][1][clippedCenter.x/2],
    00823       image.image[clippedCenter.y/2][2][clippedCenter.x/2],
    00824       colorTable, colorCorrector
    00825     );
    00826     if(currentColor == orange)
    00827       orangeCount++;
    00828     totalCount++;
    00829       for (int sector=0; sector<8; sector++)
    00830       {
    00831         BresenhamLineScan scan(clippedCenter, scanAngle, bwCameraInfo);
    00832         scan.init();
    00833         current = clippedCenter;
    00834         int steps;
    00835         if ((sector%2)!=0)
    00836           steps = (int)(radius/sqrt(2.0));
    00837         else
    00838           steps = (int)(radius);
    00839         for (int i=0; i<steps && i<scan.numberOfPixels; i++)
    00840         {
    00841           scan.getNext(current);
    00842           currentColor = CORRECTED_COLOR_CLASS(
    00843             current.x/2, current.y/2,
    00844             image.getHighResY(current.x,current.y),
    00845             image.image[current.y/2][1][current.x/2],
    00846             image.image[current.y/2][2][current.x/2],
    00847             colorTable, colorCorrector
    00848           );
    00849           if(currentColor == orange)
    00850             orangeCount++;
    00851           totalCount++;
    00852           DOT(imageProcessor_ball2, current.x/2, current.y/2,
    00853               (currentColor!=gray) ? Drawings::gray : Drawings::black,
    00854               ColorClasses::colorClassToDrawingsColor(currentColor));
    00855     }
    00856         scanAngle += pi_4;
    00857       }
    00858       if (orangeCount*3 < totalCount) //not enough orange to be ball
    00859         return;
    00860     percentOfOrange = (double)orangeCount/totalCount;
    00861     }
    00862
    00863   Vector2<double>angles;
    00864     Geometry::calculateAnglesForPoint(center, cameraMatrix, prevCameraMatrix, bwCameraInfo, angles);
    00865     double radiusAsAngle = Geometry::pixelSizeToAngleSize(radius, bwCameraInfo);
    00866     double reliability = calculateReliability(percentOfOrange,radius,angles,bwCameraInfo,cameraMatrix,prevCameraMatrix);
    00867
    00868     // ..
    00869     BallPercept newPercept;
    00870     newPercept.addHighRes(center,
    00871                           angles,
    00872                           radius,
    00873                           radiusAsAngle,
    00874                           cameraMatrix.translation,
    00875                           cameraMatrix.isValid,
    00876                           reliability);
    00877
    00878     MultipleBallPerceptElement newElement;
    00879     newElement.reliability = newPercept.reliability;
    00880     newElement.offsetOnField = newPercept.offsetOnField;
    00881
    00882     //set the distance error value
    00883     newElement.calculateDistanceValue(newElement.offsetOnField);
    00884
    00885     addMultiplePercept(newElement);
    00886     CIRCLE(imageProcessor_multipleBallPercepts, newPercept.centerInImage.x/2, newPercept.centerInImage.y/2, newPercept.radiusInImage/2, 2, Drawings::ps_dot, Drawings::orange);
    00887
    00888     //calculatePointOnField gives the position RELATIVE TO THE ROBOT...
    00889     //Vector2<int> centerOnField;
    00890     //if(Geometry::calculatePointOnField(newPercept.ballCenter.x, newPercept.ballCenter.y, cameraMatrix, prevCameraMatrix, bwCameraInfo,                                                                                                                                  centerOnField))
    00891     //{
    00892     //  CIRCLE(multipleBallPerceptsField, centerOnField.x, centerOnField.y, 30, 50,Drawings::ps_dash, Drawings::orange);
    00893     //}
    00894
    00895     ballPercept.addHighRes(center,
    00896                           angles,
    00897                           radius,
    00898                           radiusAsAngle,
    00899                           cameraMatrix.translation,
    00900                           cameraMatrix.isValid,
    00901                           reliability);
    00902   }
    00903 }
    00904
    00905
    00906
    00907 void GT2005BallSpecialist::resetMultiplePerceptsList(BallPercept& ballPercept)
    00908 {
    00909   ballPercept.multiplePercepts.numberOfElements = 0;
    00910   multiplePercepts.clear();
    00911 }
    00912
    00913
    00914 void GT2005BallSpecialist::addMultiplePercept(const MultipleBallPerceptElement& newPercept)
    00915 {
    00916   List<MultipleBallPerceptElement>::Pos iter = List<MultipleBallPerceptElement>::Pos();
    00917   if( multiplePercepts.getSize() == 0)
    00918   {
    00919     multiplePercepts.insert(newPercept, iter);
    00920   }
    00921   else
    00922   {
    00923     bool posFound = false;
    00924     iter = List<MultipleBallPerceptElement>::Pos(multiplePercepts.getFirst());
    00925     while(iter && !posFound)
    00926     {
    00927       const MultipleBallPerceptElement& t = multiplePercepts[iter];
    00928       if(newPercept.reliability < t.reliability)
    00929         ++iter;
    00930       else
    00931         posFound = true;
    00932     }
    00933     multiplePercepts.insert(newPercept,iter);
    00934   }
    00935 }
    00936
    00937 void GT2005BallSpecialist::forwardPercept(BallPercept& ballPercept)
    00938 {
    00939   List<MultipleBallPerceptElement>::Pos iter = List<MultipleBallPerceptElement>::Pos();
    00940   iter = List<MultipleBallPerceptElement>::Pos(multiplePercepts.getFirst());
    00941   int grenze = multiplePercepts.getSize() > 5 ? 5 : multiplePercepts.getSize();
    00942   for(int i = 0; i < grenze; i++)
    00943   {
    00944     ballPercept.multiplePercepts.add(multiplePercepts[iter]);
    00945     iter++;
    00946   }
    00947 }
    00948