00001
00002
00003
00004
00005
00006
00007
00008
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
00060
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
00071
00072
00073
00074
00075
00076
00077
00078 if ((countOrange > countAmbiguous) &&
00079 (countOrange * 6 > countPixel) &&
00080
00081 (numberOfPointsInYellow * 4 <= ballPoints.number * 3)
00082 )
00083 {
00084
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
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
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
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
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
00181
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
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
00334
00335
00336 for (step.x = -1; step.x <= 1; step.x += 2)
00337 {
00338
00339 for (step.y = -1; step.y <= 1; step.y += 2)
00340 {
00341
00342 findEndOfBall(image, bwCameraInfo, colorTable, start, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
00343 if (!destination.atBorder)
00344 {
00345 ballPoints.add(destination);
00346 }
00347 }
00348 }
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
00367
00368
00369
00370 int stopColorCounter = 0;
00371 int currentOrange = 0;
00372 int currentAmbiguous = 0;
00373 int len = 0;
00374 int stopLen = 0;
00375
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)
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
00414 len++;
00415 if ( currentColorClass == orange )
00416 {
00417 currentOrange++;
00418 }
00419
00420
00421
00422
00423
00424
00425
00426
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
00449
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 }
00487 }
00488
00489 destination -= step;
00490
00491
00492 countOrange += currentOrange;
00493 countAmbiguous += currentAmbiguous;
00494 maxOrangePerLine = max ( maxOrangePerLine, currentOrange);
00495 if (destination.greenIsClose) destination.yellowIsClose = false;
00496
00497
00498
00499
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
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
00637
00638 if(vectorToCenterWorld.z < 1 * factor)
00639 {
00640 if (radius <= 12.0)
00641 {
00642
00643
00644
00645
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)
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
00706
00707
00708
00709
00710
00711
00712
00713
00714
00715
00716
00717
00718
00719
00720
00721
00722
00723
00724
00725
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740
00741
00742
00743
00744
00745
00746
00747
00748
00749
00750
00751
00752
00753
00754
00755
00756
00757
00758
00759
00760
00761
00762
00763
00764
00765
00766
00767
00768
00769