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

Modules/ImageProcessor/GT2004ImageProcessor/GT2004BallSpecialist.cpp

Go to the documentation of this file.
00001 /**
00002 * @file GT2004BallSpecialist.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 "GT2004BallSpecialist.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 "GT2004ImageProcessorTools.h"
00020 #include "Platform/GTAssert.h"
00021 #include "Tools/Math/Common.h"
00022 
00023 GT2004BallSpecialist::GT2004BallSpecialist
00024 (
00025  const ColorCorrector& colorCorrector
00026  )
00027  :
00028 colorCorrector(colorCorrector)
00029 {
00030 }
00031 
00032 void GT2004BallSpecialist::searchBall
00033 (
00034  const Image& image, 
00035  const ColorTable& colorTable,
00036  const CameraMatrix& cameraMatrix,
00037  const CameraMatrix& prevCameraMatrix,
00038  int x, int y,
00039  BallPercept& ballPercept
00040  )
00041 {
00042   BallPointList ballPoints;
00043   Vector2<int> center;
00044   double radius;
00045   int countOrange = 0;
00046   int countAmbiguous = 0; 
00047   int maxOrangePerLine = 0;
00048   int countPixel = 0;
00049   CameraInfo bwCameraInfo = image.cameraInfo;
00050   bwCameraInfo.resolutionHeight*=2;
00051   bwCameraInfo.resolutionWidth*=2;
00052   bwCameraInfo.opticalCenter.x*=2;
00053   bwCameraInfo.opticalCenter.y*=2;
00054   bwCameraInfo.focalLength*=2;
00055   bwCameraInfo.focalLengthInv/=2;
00056   
00057   scanForBallPoints(image, bwCameraInfo, colorTable, x, y, ballPoints, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
00058   
00059   //ignore ball if not enough points are orange
00060   //OUTPUT (idText, text, "GT2004BallSpecialist: " << countOrange * 100 / countPixel << "% orange");
00061   int i;
00062   int numberOfSoftEdgePoints = 0;
00063   int numberOfPointsInYellow = 0;
00064   
00065   for(i = 0; i < ballPoints.number; i++)
00066   {
00067     if (ballPoints[i].yellowIsClose && !ballPoints[i].atBorder) numberOfPointsInYellow++;
00068     if (!ballPoints[i].hardEdge && !ballPoints[i].atBorder) numberOfSoftEdgePoints++;
00069   }
00070   /*  OUTPUT (idText, text, "GT2004BallSpecialist: " << endl << "----------------------------" << endl
00071   << "Ball Points: " << ballPoints.number << endl
00072   << "Yellow Points: " << (numberOfPointsInYellow*100)/ballPoints.number << "%" << endl
00073   << "SoftEdge Points: " << (numberOfSoftEdgePoints*100)/ballPoints.number << "%" << endl << endl
00074   << "Orange Pixel: " << (countOrange*100)/countPixel << "%" << endl
00075   << "Ambiguous Pixel: " << (countAmbiguous*100)/countPixel << "%" << endl
00076   );*/
00077   
00078   if ((countOrange > countAmbiguous) && 
00079     (countOrange * 6 > countPixel) &&                          // >  16.66% orange
00080 //    (numberOfSoftEdgePoints * 5 <= ballPoints.number * 2) &&   // <= 40% with soft edge
00081     (numberOfPointsInYellow * 4 <= ballPoints.number * 3)      // <= 75% with Yellow close
00082     )
00083   {
00084     //try only points near green with hard edge first
00085     BallPointList testPoints;
00086     for(i = 0; i < ballPoints.number; i++)
00087     {
00088       if (ballPoints[i].greenIsClose && ballPoints[i].hardEdge) 
00089         testPoints.add(ballPoints[i]);
00090     }
00091     if (
00092       testPoints.number *2 >= ballPoints.number &&
00093       createBallPerceptLevenbergMarquardt(testPoints, center, radius) &&
00094       checkIfPointsAreInsideBall(ballPoints, center, radius))
00095     {
00096       addBallPercept(image, bwCameraInfo, colorTable, cameraMatrix, prevCameraMatrix, center, radius, ballPercept);
00097     }
00098     else
00099     {
00100       //now all points if not at border with hard edge
00101       testPoints.number = 0;
00102       for(i = 0; i < ballPoints.number; i++)
00103       {
00104         if (!ballPoints[i].atBorder && ballPoints[i].hardEdge) 
00105           testPoints.add(ballPoints[i]);
00106       }
00107       if (
00108         createBallPerceptLevenbergMarquardt(testPoints, center, radius) &&
00109         checkIfPointsAreInsideBall(ballPoints, center, radius))
00110       {
00111         addBallPercept(image, bwCameraInfo, colorTable, cameraMatrix, prevCameraMatrix, center, radius, ballPercept);
00112       }
00113       else
00114       {
00115         //now all points if not at border
00116         testPoints.number = 0;
00117         for(i = 0; i < ballPoints.number; i++)
00118         {
00119           if (!ballPoints[i].atBorder) 
00120             testPoints.add(ballPoints[i]);
00121         }
00122         if (
00123           createBallPerceptLevenbergMarquardt(testPoints, center, radius) &&
00124           checkIfPointsAreInsideBall(ballPoints, center, radius))
00125         {
00126           addBallPercept(image, bwCameraInfo, colorTable, cameraMatrix, prevCameraMatrix, center, radius, ballPercept);
00127         }
00128         else
00129         {
00130           //take all points if nothing else works
00131           if (createBallPerceptLevenbergMarquardt(ballPoints, center, radius))
00132           {
00133             addBallPercept(image, bwCameraInfo, colorTable, cameraMatrix, prevCameraMatrix, center, radius, ballPercept);
00134           }
00135         }
00136       }
00137     }
00138   }
00139   
00140   DEBUG_DRAWING_FINISHED(imageProcessor_ball1);
00141   DEBUG_DRAWING_FINISHED(imageProcessor_ball2);
00142 }
00143 
00144 void GT2004BallSpecialist::BallPointList::add(const BallPoint& ballPoint)
00145 {
00146   ASSERT(number < maxNumberOfPoints);
00147   ballPoints[number++] = ballPoint;
00148   DOT(imageProcessor_ball2, ballPoint.x / 2, ballPoint.y / 2,
00149     (ballPoint.hardEdge) ? Drawings::blue : Drawings::orange, 
00150     (ballPoint.atBorder) ? Drawings::black :
00151   (ballPoint.greenIsClose) ? Drawings::green :
00152   (ballPoint.yellowIsClose) ? Drawings::yellow :
00153   Drawings::white
00154     );
00155 }
00156 
00157 void GT2004BallSpecialist::scanForBallPoints
00158 (
00159  const Image& image,
00160  const CameraInfo& bwCameraInfo,
00161  const ColorTable& colorTable,
00162  int x, int y,
00163  BallPointList& ballPoints,
00164  int& countAmbiguous, 
00165  int& countOrange,
00166  int& maxOrangePerLine,
00167  int& countPixel
00168  )
00169 {
00170   // search for ball variables
00171   BallPoint north;
00172   BallPoint east;
00173   BallPoint south;
00174   BallPoint west;
00175   
00176   BallPoint start;
00177   Vector2<int>step;
00178   BallPoint destination;
00179   
00180   //  int xStep;
00181   //  int yStep;
00182   
00183   start.x = x * 2; start.y = y * 2;
00184   BallPoint start2;
00185   
00186   DOT(imageProcessor_ball2, x, y, Drawings::black, Drawings::white);
00187   
00188   //find north ///////////////////////////////////////////
00189   step.x = 0; step.y = -1;
00190   findEndOfBall(image, bwCameraInfo, colorTable, start, step, north, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
00191   if(north.atBorder)
00192   {
00193     start2 = north - step;
00194     //find east
00195     step.x = 1; step.y = 0;
00196     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
00197     {
00198       ballPoints.add(destination);
00199     }
00200     //find west
00201     step.x = -1; step.y = 0;
00202     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
00203     {
00204       ballPoints.add(destination);
00205     }
00206   }
00207   else
00208   {
00209     ballPoints.add(north);
00210   }
00211   
00212   //find east ///////////////////////////////////////////
00213   step.x = 1; step.y = 0;
00214   findEndOfBall(image, bwCameraInfo, colorTable, start, step, east, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
00215   if(east.atBorder)
00216   {
00217     start2 = east - step;
00218     //find north
00219     step.x = 0; step.y = -1;
00220     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
00221     {
00222       ballPoints.add(destination);
00223     }
00224     //find south
00225     step.x = 0; step.y = 1;
00226     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
00227     {
00228       ballPoints.add(destination);
00229     }
00230   }
00231   else
00232   {
00233     ballPoints.add(east);
00234   }
00235   
00236   //find south ///////////////////////////////////////////
00237   step.x = 0; step.y = 1;
00238   findEndOfBall(image, bwCameraInfo, colorTable, start, step, south, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
00239   if(south.atBorder)
00240   {
00241     start2 = south - step;
00242     //find east
00243     step.x = 1; step.y = 0;
00244     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
00245     {
00246       ballPoints.add(destination);
00247     }
00248     //find west
00249     step.x = -1; step.y = 0;
00250     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
00251     {
00252       ballPoints.add(destination);
00253     }
00254   }
00255   else
00256   {
00257     ballPoints.add(south);
00258   }
00259   
00260   //find west ///////////////////////////////////////////
00261   step.x = -1; step.y = 0;
00262   findEndOfBall(image, bwCameraInfo, colorTable, start, step, west, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
00263   if(west.atBorder)
00264   {
00265     start2 = west - step;
00266     //find north
00267     step.x = 0; step.y = -1;
00268     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
00269     {
00270       ballPoints.add(destination);
00271     }
00272     //find south
00273     step.x = 0; step.y = 1;
00274     if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
00275     {
00276       ballPoints.add(destination);
00277     }
00278   }
00279   else
00280   {
00281     ballPoints.add(west);
00282   }
00283   
00284   //
00285   if( (south.y - north.y) > (east.x - west.x) )
00286   {
00287     if ((north.y + south.y) / 2 != start.y)
00288     {
00289       start.y = (north.y + south.y) / 2;
00290       //find east
00291       step.x = 1; step.y = 0;
00292       findEndOfBall(image, bwCameraInfo, colorTable, start, step, east, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
00293       if(!east.atBorder)
00294       {
00295         ballPoints.add(east);
00296       }
00297       //find west
00298       step.x = -1; step.y = 0;
00299       findEndOfBall(image, bwCameraInfo, colorTable, start, step, west, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
00300       if (!west.atBorder)
00301       {
00302         ballPoints.add(west);
00303       }
00304       //////////
00305       start.x = (west.x + east.x) / 2;
00306     }
00307   }
00308   else
00309   {
00310     if ((west.x + east.x) / 2 != start.x)
00311     {
00312       start.x = (west.x + east.x) / 2;
00313       //find north
00314       step.x = 0; step.y = -1;
00315       findEndOfBall(image, bwCameraInfo, colorTable, start, step, north, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
00316       if(!north.atBorder)
00317       {
00318         ballPoints.add(north);
00319       }
00320       //find south
00321       step.x = 0; step.y = 1;
00322       findEndOfBall(image, bwCameraInfo, colorTable, start, step, south, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
00323       if(!south.atBorder)
00324       {
00325         ballPoints.add(south);
00326       }
00327       //////////
00328       start.y = (north.y + south.y) / 2;
00329     }
00330   }
00331   //
00332   
00333   // find in diagonal
00334   
00335   //  for(xStep = -1; xStep <= 1; xStep += 2)
00336   for (step.x = -1; step.x <= 1; step.x += 2)
00337   {
00338     //for(yStep = -1; yStep <= 1; yStep += 2)
00339     for (step.y = -1; step.y <= 1; step.y += 2)
00340     {
00341       //step.x = xStep; step.y = yStep;
00342       findEndOfBall(image, bwCameraInfo, colorTable, start, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
00343       if (!destination.atBorder)
00344       {
00345         ballPoints.add(destination);
00346       }
00347     } //for(int yStep ...
00348   } //for(int xStep ...
00349 }
00350 
00351 bool GT2004BallSpecialist::findEndOfBall
00352 (
00353  const Image& image,
00354  const CameraInfo& bwCameraInfo,
00355  const ColorTable& colorTable,
00356  const BallPoint& start,
00357  const Vector2<int>& step,
00358  BallPoint& destination,
00359  int& countAmbiguous,
00360  int& countOrange,
00361  int& maxOrangePerLine,
00362  int& countPixel
00363  )
00364 {
00365 /*
00366 stopColors are colors indicating the end of the ball:
00367 green, yellow, skyblue
00368 more than 3 pixels of a stopColor indicate the end of the ball
00369   */
00370   int stopColorCounter = 0;
00371   int currentOrange = 0;
00372   int currentAmbiguous = 0;
00373   int len = 0;
00374   int stopLen = 0;
00375 //  bool isAmbiguous = false;
00376   
00377   colorClass currentColorClass;
00378   
00379   Vector2<int> firstStopColor = start;
00380   unsigned char lastGoodColorOrangeSim = 0;
00381   unsigned char currentOrangeSim = 0;
00382   unsigned char prevOrangeSim = 0;
00383   Vector2<int> lastPoint = start;
00384   destination = start;
00385   destination.greenIsClose = false;
00386   destination.yellowIsClose = false;
00387   destination.atBorder = false;
00388   destination.hardEdge = true;
00389   
00390   bool isOrange = false;
00391   
00392   bool goOn = true;
00393   while(goOn)
00394   {
00395     lastPoint = destination;
00396     destination += step;
00397     /////////////
00398     if(destination.x < 0 || destination.x >= bwCameraInfo.resolutionWidth ||
00399       destination.y < 0 || destination.y >= bwCameraInfo.resolutionHeight-1) // avoid artefact at bottom of hires image
00400     {
00401       destination.atBorder = true;
00402       countPixel += len;
00403       goOn = false;
00404     }
00405     else
00406     {
00407       currentColorClass = CORRECTED_COLOR_CLASS(
00408         destination.x / 2,destination.y / 2,
00409         image.getHighResY(destination.x,destination.y),
00410         image.image[destination.y / 2][1][destination.x / 2],
00411         image.image[destination.y / 2][2][destination.x / 2]);
00412       
00413       // counting all orange pixels on the scanned horz./vert./diag. lines
00414       len++;
00415       if ( currentColorClass == orange )
00416       {
00417         currentOrange++;
00418       }
00419 
00420     /*
00421       isAmbiguous = (currentColorClass == pink ||
00422         currentColorClass == yellow ||
00423         currentColorClass == red
00424         );
00425       
00426       if (isAmbiguous)
00427     */
00428     if (currentColorClass == pink || currentColorClass == yellow || currentColorClass == red)
00429       {
00430         currentAmbiguous++;
00431       }
00432       
00433       LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::green);
00434       
00435       prevOrangeSim = currentOrangeSim;
00436       currentOrangeSim = getSimilarityToOrange(
00437         colorCorrector.correct(destination.x / 2,destination.y / 2, 0, image.getHighResY(destination.x,destination.y)),
00438         colorCorrector.correct(destination.x / 2,destination.y / 2, 1, image.image[destination.y / 2][1][destination.x / 2]),
00439         colorCorrector.correct(destination.x / 2,destination.y / 2, 2, image.image[destination.y / 2][2][destination.x / 2])
00440         );
00441       isOrange = (currentOrangeSim > 30) || (currentColorClass == orange);
00442       
00443       if(currentColorClass == green || 
00444         currentColorClass == yellow ||
00445         currentColorClass == skyblue ||
00446         currentColorClass == red ||
00447         currentColorClass == blue ||
00448         //        currentColorClass == pink || //added
00449         //removed because there is pink in the ball and I have removed pink from orange similarity
00450         !isOrange
00451         )
00452       {
00453         if (stopColorCounter == 0)
00454         {
00455           firstStopColor = destination;
00456           lastGoodColorOrangeSim = prevOrangeSim;
00457           stopLen = len;
00458         }
00459         if (currentColorClass == green) destination.greenIsClose = true;
00460         if (currentColorClass == yellow) destination.yellowIsClose = true;
00461         stopColorCounter++;
00462         
00463         if (isOrange)
00464         {
00465           LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::orange);
00466         }
00467         else
00468         {
00469           LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::red);
00470         }
00471         
00472         if(stopColorCounter > 8) 
00473         {
00474           destination = firstStopColor;
00475           countPixel += stopLen;
00476           goOn = false;
00477         }
00478       }
00479       else
00480       {
00481         destination.greenIsClose = false;
00482         destination.yellowIsClose = false;
00483         stopColorCounter = 0;
00484       }
00485       
00486     } // else destination in range
00487   } //  while(goOn)
00488   
00489   destination -= step;
00490   
00491   // compute sum of all orange pixels and max-pixels-per-line
00492   countOrange += currentOrange;
00493   countAmbiguous += currentAmbiguous;
00494   maxOrangePerLine = max ( maxOrangePerLine, currentOrange);
00495   if (destination.greenIsClose) destination.yellowIsClose = false;
00496   
00497   /*  if (!destination.atBorder)
00498   {
00499   OUTPUT(idText, text, "Point(" << destination.x << "/" << destination.y << "): " << lastGoodColorOrangeSim << " -> " << currentOrangeSim);
00500   }*/
00501   if ( 2 * currentOrangeSim < lastGoodColorOrangeSim ) destination.hardEdge = true;
00502   else destination.hardEdge = false;
00503   
00504   if (!destination.atBorder)
00505   {
00506     if (destination.greenIsClose)
00507     {
00508       LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::blue);
00509     }
00510     else
00511       if (destination.yellowIsClose)
00512       {
00513         LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::skyblue);
00514       }
00515       else
00516       {
00517         LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::pink);
00518       }
00519   }
00520   return true;
00521 }
00522 
00523 
00524 
00525 bool GT2004BallSpecialist::createBallPerceptLevenbergMarquardt
00526 (
00527  const BallPointList& ballPoints,
00528  Vector2<int>& center,
00529  double& radius
00530  )
00531 {
00532   if (ballPoints.number < 3)
00533     return false;
00534   
00535   double Mx = 0.0;
00536   double My = 0.0;
00537   double Mxx = 0.0;
00538   double Myy = 0.0;
00539   double Mxy = 0.0;
00540   double Mz = 0.0;
00541   double Mxz = 0.0;
00542   double Myz = 0.0;
00543   
00544   for (int i = 0; i < ballPoints.number; ++i)
00545   {
00546     double x = ballPoints[i].x;
00547     double y = ballPoints[i].y;
00548     double xx = x*x;
00549     double yy = y*y;
00550     double z = xx + yy;
00551     
00552     Mx += x;
00553     My += y;
00554     Mxx += xx;
00555     Myy += yy;
00556     Mxy += x*y;
00557     Mz += z;
00558     Mxz += x*z;
00559     Myz += y*z;
00560   }
00561   
00562   try
00563   {
00564     Matrix_nxn<double, 3> M;
00565     double Matrix[9] = 
00566     {
00567       Mxx, Mxy, Mx,
00568         Mxy, Myy, My,
00569         Mx, My, ballPoints.number
00570     };
00571     M = Matrix;
00572     
00573     Vector_n<double, 3> v;
00574     
00575     v[0] = -Mxz;
00576     v[1] = -Myz;
00577     v[2] = -Mz;
00578     
00579     Vector_n<double, 3> BCD;
00580     BCD = M.solve(v);
00581     
00582     center.x = (int)(-BCD[0] / 2.0);
00583     center.y = (int)(-BCD[1] / 2.0);
00584     
00585     double tmpWurzel = BCD[0]*BCD[0]/4.0 + BCD[1]*BCD[1]/4.0 - BCD[2];
00586     
00587     if (tmpWurzel < 0.0)
00588       return false;
00589     
00590     radius = sqrt(tmpWurzel);
00591   }
00592   catch (MVException)
00593   {
00594     return false;
00595   }
00596   catch (...)
00597   {
00598     OUTPUT(idText, text, "Unknown exception in GT2004BallSpecialist::createBallPerceptsFromXPoints");
00599     return false;
00600   }
00601   
00602   return true;
00603 }
00604 
00605 bool GT2004BallSpecialist::checkIfPointsAreInsideBall(const BallPointList& ballPoints, Vector2<int>& center, double radius)
00606 {
00607   for(int i = 0; i < ballPoints.number; i++)
00608   {
00609     if (Geometry::distance(center, ballPoints[i]) > radius * 1.1)
00610     {
00611       return false;
00612     }
00613   } 
00614   return true;
00615 }
00616 
00617 void GT2004BallSpecialist::addBallPercept
00618 (
00619  const Image& image,
00620  const CameraInfo& bwCameraInfo,
00621  const ColorTable& colorTable,
00622  const CameraMatrix& cameraMatrix,
00623  const CameraMatrix& prevCameraMatrix,
00624  const Vector2<int>& center,
00625  double radius,
00626  BallPercept& ballPercept
00627  )
00628 {
00629   // test if ball is below horizon
00630   double factor = bwCameraInfo.focalLengthInv;
00631   Vector3<double> 
00632     vectorToCenter(1, (bwCameraInfo.opticalCenter.x - center.x) * factor, (bwCameraInfo.opticalCenter.y - center.y) * factor);
00633   Vector3<double> 
00634     vectorToCenterWorld = cameraMatrix.rotation * vectorToCenter;
00635   
00636   //Is the point above the horizon ? - return
00637   //  if(vectorToCenterWorld.z <= -5 * factor) //for perfect horizon
00638   if(vectorToCenterWorld.z < 1 * factor)
00639   {
00640     if (radius <= 12.0)
00641     {//small ghost balls can easily appear on an orange fringe of a pink/yellow landmark
00642       // this because the scan follows along the fringe; this new scan starts from the 
00643       // hypothetical center of the "ghost" ball, which in such case lies inside the landmark
00644       // and hence produces a high number of "non orange" points; a true ball however,
00645       // should have enough orange inside
00646       Vector2<int> current(center);
00647       int orangeCount = 0;
00648       int totalCount = 0;
00649       double scanAngle = 0;
00650       colorClass currentColor;
00651       currentColor = CORRECTED_COLOR_CLASS(
00652         current.x/2, current.y/2,
00653         image.getHighResY(current.x,current.y),
00654         image.image[current.y/2][1][current.x/2],
00655         image.image[current.y/2][2][current.x/2]
00656       );
00657       if(currentColor == orange)
00658         orangeCount++;
00659       totalCount++;
00660       for (int sector=0; sector<8; sector++)
00661       {
00662         BresenhamLineScan scan(center, scanAngle, bwCameraInfo);
00663         scan.init();
00664         current = center;
00665         int steps; 
00666         if ((sector%2)!=0)
00667           steps = (int)(radius/sqrt(2.0));
00668         else
00669           steps = (int)(radius);
00670         for (int i=0; i<steps && i<scan.numberOfPixels; i++)
00671         {
00672           scan.getNext(current);
00673           currentColor = CORRECTED_COLOR_CLASS(
00674             current.x/2, current.y/2,
00675             image.getHighResY(current.x,current.y),
00676             image.image[current.y/2][1][current.x/2],
00677             image.image[current.y/2][2][current.x/2]
00678           );
00679           if(currentColor == orange)
00680             orangeCount++;
00681           totalCount++;
00682           DOT(imageProcessor_ball2, current.x/2, current.y/2, 
00683               (currentColor!=gray) ? Drawings::gray : Drawings::black, 
00684               ColorClasses::colorClassToDrawingsColor(currentColor));
00685         }
00686         scanAngle += pi_4;
00687       }
00688       if (orangeCount*3 < totalCount) //not enough orange to be ball
00689         return;
00690     }
00691     Vector2<double>angles;
00692     Geometry::calculateAnglesForPoint(center, cameraMatrix, prevCameraMatrix, bwCameraInfo, angles);
00693     ballPercept.add(
00694       bwCameraInfo,
00695       center,
00696       radius,
00697       angles, 
00698       Geometry::pixelSizeToAngleSize(radius, bwCameraInfo), 
00699       cameraMatrix.translation, 
00700       cameraMatrix.isValid);
00701   }
00702 }
00703 
00704 /*
00705 * $Log: GT2004BallSpecialist.cpp,v $
00706 * Revision 1.15  2004/09/09 11:37:39  wachter
00707 * - Fixed some more doxygen-errors
00708 *
00709 * Revision 1.14  2004/07/09 14:12:55  nistico
00710 * Some more safety checks
00711 *
00712 * Revision 1.13  2004/07/09 14:04:38  mkunz
00713 * comments corrected
00714 *
00715 * Revision 1.12  2004/06/29 08:52:41  juengel
00716 * Removed the "<= 40% with hard edge" check for ball detection.
00717 *
00718 * Revision 1.11  2004/06/28 08:55:05  schmitt
00719 * simplified some calculations
00720 *
00721 * Revision 1.10  2004/06/16 10:56:48  altmeyer
00722 * fixed error "mehrdeutiger Funktionsaufruf: srqt": Replaced "2" with "2.0"
00723 *
00724 * Revision 1.9  2004/06/14 14:14:54  mkunz
00725 * only syntax changes
00726 *
00727 * Revision 1.8  2004/06/13 11:28:16  nistico
00728 * Post processing scan on small balls, scanning radially always starting
00729 * from the calculated canter, looking for percentage of orange.
00730 * Ghost balls on pink/yellow landmarks problem should be finally fixed now.
00731 *
00732 * Revision 1.7  2004/06/05 07:58:21  roefer
00733 * Compensation for motion distortions of images
00734 *
00735 * Revision 1.6  2004/05/27 19:38:56  loetzsch
00736 * compilation fixed
00737 *
00738 * Revision 1.5  2004/05/27 14:35:06  mkunz
00739 * HardEdgePoints <> SoftEdgePoints
00740 * workaround for artefacts on buttom of hires image
00741 * pink ist no stopcolor (pink is no longer similar to orange)
00742 *
00743 * Revision 1.4  2004/05/26 20:25:48  mkunz
00744 * some comments added
00745 *
00746 * Revision 1.3  2004/05/25 20:39:07  mkunz
00747 * befor taking all points not at border, take only all points on a hard edge wich are not at border
00748 *
00749 * Revision 1.2  2004/05/25 17:45:59  mkunz
00750 * take only points with hard edges at first try
00751 *
00752 * Revision 1.1.1.1  2004/05/22 17:19:41  cvsadm
00753 * created new repository GT2004_WM
00754 *
00755 * Revision 1.3  2004/05/14 16:37:42  nistico
00756 * Ghost balls on landmark improvements:
00757 * -introduced ambiguous (pink, red, yellow) color check in ball recognition
00758 * -some parameter tuning
00759 *
00760 * Revision 1.2  2004/05/07 15:16:24  nistico
00761 * All geometry calculations are making use of intrinsic functions.
00762 * I updated most of the images processors to make correct use of this.
00763 * Please test if there are any problems, because i'm going to remove the
00764 * old code soon.
00765 *
00766 * Revision 1.1  2004/05/04 13:40:19  tim
00767 * added GT2004ImageProcessor
00768 *
00769 */

Generated on Thu Sep 23 19:57:28 2004 for GT2004 by doxygen 1.3.6