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

# Modules/ImageProcessor/GT2005ImageProcessor/GT2005PlayerSpecialist.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005FlagSpecialist.cpp
    00003 *
    00004 * This file contains a class for Image Processing.
    00005 * @author <A href=mailto:marc.dassler@web.de>Marc Dassler</A>
    00006 */
    00007
    00008 #include "Tools/FieldDimensions.h"
    00009 #include "Tools/Player.h"
    00010 #include "Tools/Math/Common.h"
    00011 #include "Modules/ImageProcessor/ImageProcessorTools/ColorCorrector.h"
    00012 #include "GT2005PlayerSpecialist.h"
    00013 #include "GT2005ImageProcessorTools.h"
    00014
    00015
    00016 GT2005PlayerSpecialist::GT2005PlayerSpecialist(
    00017   const ColorCorrector& colorCorrector,
    00018   const ColorTable& colorTable,
    00019   const Image& image,
    00020   const ImageInfo& imageInfo,
    00021   const LinesPercept& linesPercept,
    00022   const CameraMatrix& cameraMatrix,
    00023   PlayersPercept& playersPercept):
    00024   colorCorrector(colorCorrector),
    00025   colorTable(colorTable),
    00026   image(image),
    00027   imageInfo(imageInfo),
    00028   linesPercept(linesPercept),
    00029   cameraMatrix(cameraMatrix),
    00030   playersPercept(playersPercept)
    00031 {
    00032     maxDepth = 0;
    00033 }
    00034
    00035
    00036 void GT2005PlayerSpecialist::execute()
    00037 {
    00038       clusterRobotLinesPercepts();
    00039       for (int c=0;c<clusteredRedRobots;c++)
    00040       {
    00041         if (averageRedRobotSignaturePoints[c]>1)
    00042         {
    00043           LINE(imageProcessor_robot_detection,
    00044             averageRedRobotSignatureStart[c].x, averageRedRobotSignature[c].y,
    00045             averageRedRobotSignatureStop[c].x, averageRedRobotSignature[c].y,
    00046             1, Drawings::ps_solid, Drawings::red );
    00047
    00048           LINE(imageProcessor_robot_detection,
    00049             averageRedRobotSignatureStart[c].x, averageRedRobotSignatureStart[c].y,
    00050             averageRedRobotSignatureStop[c].x, averageRedRobotSignatureStop[c].y,
    00051             1, Drawings::ps_solid, Drawings::yellow );
    00052
    00053           NLINE("imageProcessor:robot detection",
    00054             averageRedRobotSignatureStart[c].x, averageRedRobotSignature[c].y,
    00055             averageRedRobotSignatureStop[c].x, averageRedRobotSignature[c].y,
    00056             1, Drawings::ps_solid, Drawings::red );
    00057
    00058           NLINE("imageProcessor:robot detection",
    00059             averageRedRobotSignatureStart[c].x, averageRedRobotSignatureStart[c].y,
    00060             averageRedRobotSignatureStop[c].x, averageRedRobotSignatureStop[c].y,
    00061             1, Drawings::ps_solid, Drawings::yellow );
    00062
    00063           detectRedRobots(averageRedRobotSignatureStart[c],averageRedRobotSignatureStop[c],averageRedRobotSignature[c]);
    00064         }
    00065       }
    00066       for (int c=0;c<clusteredBlueRobots;c++)
    00067       {
    00068         if (averageBlueRobotSignaturePoints[c]>1)
    00069         {
    00070           LINE(imageProcessor_robot_detection,
    00071             averageBlueRobotSignatureStart[c].x, averageBlueRobotSignature[c].y,
    00072             averageBlueRobotSignatureStop[c].x, averageBlueRobotSignature[c].y,
    00073             1, Drawings::ps_solid, Drawings::blue );
    00074
    00075           LINE(imageProcessor_robot_detection,
    00076             averageBlueRobotSignatureStart[c].x, averageBlueRobotSignatureStart[c].y,
    00077             averageBlueRobotSignatureStop[c].x, averageBlueRobotSignatureStop[c].y,
    00078             1, Drawings::ps_solid, Drawings::yellow );
    00079
    00080           NLINE("imageProcessor:robot detection",
    00081             averageBlueRobotSignatureStart[c].x, averageBlueRobotSignature[c].y,
    00082             averageBlueRobotSignatureStop[c].x, averageBlueRobotSignature[c].y,
    00083             1, Drawings::ps_solid, Drawings::blue );
    00084
    00085           NLINE("imageProcessor:robot detection",
    00086             averageBlueRobotSignatureStart[c].x, averageBlueRobotSignatureStart[c].y,
    00087             averageBlueRobotSignatureStop[c].x, averageBlueRobotSignatureStop[c].y,
    00088             1, Drawings::ps_solid, Drawings::yellow );
    00089
    00090
    00091           detectBlueRobots(averageBlueRobotSignatureStart[c],averageBlueRobotSignatureStop[c],averageBlueRobotSignature[c]);
    00092         }
    00093       }
    00094
    00095
    00096 }
    00097
    00098 void GT2005PlayerSpecialist::detectRedRobots(
    00099      Vector2<int> averageRobotSignatureStart,
    00100      Vector2<int> averageRobotSignatureStop,
    00101      Vector2<int> averageRobotSignature)
    00102 {
    00103   // walk the averageRobotSignature and search for white - robotColor - white
    00104   // or search left and right side of the averageRobotSignature
    00105
    00106   Vector2<int> firstWhite(1000,0),firstRed(1000,0), lastRed(-1000,0), lastWhite(-1000,0),runner;
    00107   if (averageRobotSignatureStart != averageRobotSignatureStop)
    00108   {
    00109     BresenhamLineScan avgRobotSigBresenham (averageRobotSignatureStart, averageRobotSignatureStop);
    00110     runner = averageRobotSignatureStart;
    00111     while(runner != averageRobotSignatureStop)
    00112     {
    00113        if ( runner.x >= imageInfo.maxImageCoordinates.x
    00114         || runner.x < 0
    00115         || runner.y >= imageInfo.maxImageCoordinates.y
    00116         || runner.y < 0)
    00117       {
    00118         OUTPUT(idText, text, "detectRedRobots: Bresenham left image");
    00119         break;
    00120       }
    00121       colorClass inspect = CORRECTED_COLOR_CLASS(runner.x,runner.y,
    00122           image.image[runner.y][0][runner.x],
    00123           image.image[runner.y][1][runner.x],
    00124           image.image[runner.y][2][runner.x],
    00125           colorTable, colorCorrector);
    00126
    00127
    00128       // TODO: one pixel is not enough to decide this
    00129       if (inspect == white && firstWhite.x == 1000)
    00130             firstWhite = runner;
    00131       if (inspect == red && firstRed.x == 1000)
    00132             firstRed = runner;
    00133       if (inspect == red && lastRed.x < runner.x)
    00134             lastRed = runner;
    00135       if (inspect == white && runner.x > firstWhite.x)
    00136             lastWhite = runner;
    00137
    00138       avgRobotSigBresenham.getNext(runner);
    00139     }
    00140   }        // fi (averageRobotSignatureStart != averageRobotSignatureStop)
    00141   else
    00142   {
    00143     firstWhite = averageRobotSignatureStart;
    00144   }
    00145
    00146   Vector2<unsigned char> cfirstWhite,cfirstRed, clastRed, clastWhite;
    00147
    00148   cfirstWhite.x = (char)firstWhite.x;
    00149   cfirstWhite.y = (char)firstWhite.y;
    00150
    00151   clastWhite.y = (char)lastWhite.y;
    00152   clastWhite.x = (char)lastWhite.x;
    00153
    00154   cfirstRed.y = (char)firstRed.y;
    00155   cfirstRed.x = (char)firstRed.x;
    00156
    00157   clastRed.y = (char)lastRed.y;
    00158   clastRed.x = (char)lastRed.x;
    00159
    00160
    00161   // TODO: only the red playercolor were found
    00162
    00163   //now take the midpoint of the white parts and start search green below
    00164   double proportion = (averageRobotSignatureStart-averageRobotSignatureStop).abs();
    00165
    00166
    00167   Vector2<double> dir;
    00168   dir.x = averageRobotSignatureStop.x-averageRobotSignatureStart.x;
    00169   dir.y = averageRobotSignatureStop.y-averageRobotSignatureStart.y;
    00170   Geometry::Line averageLine = Geometry::Line(averageRobotSignatureStart, dir);
    00171   // if average line angle bigger than 15 degree, we probably observe the roboter at the side
    00172   if (sin(dir.y/proportion) > pi/12)
    00173     proportion *= 11.0/17.0; // robot height to robot length observed at side
    00174   else
    00175     proportion *= 1.0; // robot height to robot width observed at front
    00176   int depth = (int) proportion;
    00177   Vector2<unsigned char> point,lower(0,0);
    00178   double distance;
    00179   if (firstWhite.x != 1000)
    00180   {
    00181     point = findRedFootPointYAxis(cfirstWhite,Vector2<char>(0,1),depth);
    00182     if (point.x != 255 && point.y > lower.y)  lower = point;
    00183     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    00184     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    00185     point = findRedFootPointYAxis(cfirstWhite,Vector2<char>(1,1),depth);
    00186     if (point.x != 255 && point.y > lower.y)  lower = point;
    00187     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    00188     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    00189     point = findRedFootPointYAxis(cfirstWhite,Vector2<char>(-1,1),depth);
    00190     if (point.x != 255 && point.y > lower.y)  lower = point;
    00191     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    00192     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    00193   }
    00194   if (lastWhite.x != -1000)
    00195   {
    00196     point = findRedFootPointYAxis(clastWhite,Vector2<char>(0,1),depth);
    00197     if (point.x != 255 && point.y > lower.y)  lower = point;
    00198     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    00199     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    00200     point = findRedFootPointYAxis(clastWhite,Vector2<char>(1,1),depth);
    00201     if (point.x != 255 && point.y > lower.y)  lower = point;
    00202     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    00203     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    00204     point = findRedFootPointYAxis(clastWhite,Vector2<char>(-1,1),depth);
    00205     if (point.x != 255 && point.y > lower.y)  lower = point;
    00206     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    00207     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    00208   }
    00209   if (lower.x!=0)
    00210   {
    00211     Vector2<double> dlower;dlower.x = lower.x;dlower.y = lower.y;
    00212     distance = fabs(Geometry::getDistanceToLine(averageLine,dlower));
    00213   }
    00214   else
    00215     distance = 0;
    00216   if (firstRed.x != 1000 && proportion-distance>0)
    00217   {
    00218     point = findRedFootPointYAxis(cfirstRed,Vector2<char>(0,1),depth);
    00219     if (point.x != 255 && point.y > lower.y)  lower = point;
    00220     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    00221     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    00222     point = findRedFootPointYAxis(cfirstRed,Vector2<char>(1,1),depth);
    00223     if (point.x != 255 && point.y > lower.y)  lower = point;
    00224     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    00225     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    00226     point = findRedFootPointYAxis(cfirstRed,Vector2<char>(-1,1),depth);
    00227     if (point.x != 255 && point.y > lower.y)  lower = point;
    00228     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    00229     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    00230   }
    00231   if (lower.x!=0)
    00232   {
    00233     Vector2<double> dlower;dlower.x = lower.x;dlower.y = lower.y;
    00234     distance = fabs(Geometry::getDistanceToLine(averageLine,dlower));
    00235   }
    00236   else
    00237     distance = 0;
    00238   if (lastRed.x != -1000 && proportion-distance>0)
    00239   {
    00240     point = findRedFootPointYAxis(clastRed,Vector2<char>(0,1),depth);
    00241     if (point.x != 255 && point.y > lower.y)  lower = point;
    00242     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    00243     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    00244     point = findRedFootPointYAxis(clastRed,Vector2<char>(1,1),depth);
    00245     if (point.x != 255 && point.y > lower.y)  lower = point;
    00246     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    00247     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    00248     point = findRedFootPointYAxis(clastRed,Vector2<char>(-1,1),depth);
    00249     if (point.x != 255 && point.y > lower.y)  lower = point;
    00250     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::red);
    00251     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::red);
    00252   }
    00253
    00254   // create percept
    00255   Vector2<double> pointOnField;
    00256   SinglePlayerPercept percept;
    00257   Vector2<int> robotCenter((averageRobotSignatureStop.x-averageRobotSignatureStart.x)/2+averageRobotSignatureStart.x,lower.y);
    00258
    00259   // now take the middle of the averageRobotXLine and the y of the lowest point
    00260   if (Geometry::calculatePointOnField(robotCenter.x, robotCenter.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, percept.offsetOnField))
    00261   {
    00262     CIRCLE(imageProcessor_robot_detection, lower.x, lower.y,10,1, Drawings::ps_solid, Drawings::yellow);
    00263     CIRCLE(imageProcessor_robot_detection, robotCenter.x, robotCenter.y,10,1, Drawings::ps_solid, Drawings::red);
    00264     NCIRCLE("imageProcessor:robot detection", lower.x, lower.y,10,1, Drawings::ps_solid, Drawings::yellow);
    00265     NCIRCLE("imageProcessor:robot detection", robotCenter.x, robotCenter.y,10,1, Drawings::ps_solid, Drawings::red);
    00266     percept.directionOnField = 0;           // why = 0?
    00267     percept.offsetOnField = percept.offsetOnField;         // ??
    00268     percept.positionInImage = robotCenter;
    00269     percept.validity = 1;
    00270
    00271   Vector2<double> foot;
    00272   if (Geometry::calculatePointOnField(lower.x, lower.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, foot)){
    00273     percept.orientation = atan2(percept.offsetOnField.x - foot.x, percept.offsetOnField.y - foot.y);
    00274   }else{
    00275     percept.orientation = 0;
    00276   }
    00277
    00278     playersPercept.addRedPlayer(percept);
    00279   }
    00280 }
    00281
    00282 void GT2005PlayerSpecialist::detectBlueRobots(
    00283      Vector2<int> averageRobotSignatureStart,
    00284      Vector2<int> averageRobotSignatureStop,
    00285      Vector2<int> averageRobotSignature)
    00286 {
    00287   // walk the averageRobotSignature and search for white - robotColor - white
    00288   // or search left and right side of the averageRobotSignature
    00289
    00290   Vector2<int> firstWhite(1000,0),firstBlue(1000,0), lastBlue(-1000,0), lastWhite(-1000,0),runner;
    00291   if (averageRobotSignatureStart != averageRobotSignatureStop)
    00292   {
    00293     BresenhamLineScan avgRobotSigBresenham (averageRobotSignatureStart, averageRobotSignatureStop);
    00294     runner = averageRobotSignatureStart;
    00295     while(runner != averageRobotSignatureStop)
    00296     {
    00297       if ( runner.x >= imageInfo.maxImageCoordinates.x
    00298         || runner.x < 0
    00299         || runner.y >= imageInfo.maxImageCoordinates.y
    00300         || runner.y < 0)
    00301       {
    00302         OUTPUT(idText, text, "detectBlueRobots: Bresenham left image start "  << averageRobotSignatureStart.x << ":" << averageRobotSignatureStart.y);
    00303         OUTPUT(idText, text, "stop "  << averageRobotSignatureStop.x << ":" << averageRobotSignatureStop.y);
    00304         break;
    00305       }
    00306
    00307       colorClass inspect = CORRECTED_COLOR_CLASS(runner.x,runner.y,
    00308           image.image[runner.y][0][runner.x],
    00309           image.image[runner.y][1][runner.x],
    00310           image.image[runner.y][2][runner.x],
    00311           colorTable, colorCorrector);
    00312
    00313       // TODO: one pixel is not enough to decide this
    00314       if (inspect == white && firstWhite.x == 1000)
    00315             firstWhite = runner;
    00316       if (inspect == blue && firstBlue.x == 1000)
    00317             firstBlue = runner;
    00318       if (inspect == blue && lastBlue.x < runner.x)
    00319             lastBlue = runner;
    00320       if (inspect == white && runner.x > firstWhite.x)
    00321             lastWhite = runner;
    00322
    00323       avgRobotSigBresenham.getNext(runner);
    00324     }
    00325   }
    00326   else
    00327   {
    00328     firstWhite = averageRobotSignatureStart;
    00329   }
    00330
    00331   Vector2<unsigned char> cfirstWhite,cfirstBlue, clastBlue, clastWhite;
    00332
    00333   cfirstWhite.x = (char)firstWhite.x;
    00334   cfirstWhite.y = (char)firstWhite.y;
    00335
    00336   clastWhite.y = (char)lastWhite.y;
    00337   clastWhite.x = (char)lastWhite.x;
    00338
    00339   cfirstBlue.y = (char)firstBlue.y;
    00340   cfirstBlue.x = (char)firstBlue.x;
    00341
    00342   clastBlue.y = (char)lastBlue.y;
    00343   clastBlue.x = (char)lastBlue.x;
    00344
    00345
    00346   // TODO: only the blue playercolor were found
    00347
    00348   //now take the midpoint of the white parts and start search green below
    00349   double proportion = (averageRobotSignatureStart-averageRobotSignatureStop).abs();
    00350
    00351
    00352   Vector2<double> dir;
    00353   dir.x = averageRobotSignatureStop.x-averageRobotSignatureStart.x;
    00354   dir.y = averageRobotSignatureStop.y-averageRobotSignatureStart.y;
    00355   Geometry::Line averageLine = Geometry::Line(averageRobotSignatureStart, dir);
    00356   // if average line angle bigger than 15 degree, we probably observe the roboter at the side
    00357   if (sin(dir.y/proportion) > pi/12)
    00358     proportion *= 11.0/17.0; // robot height to robot length observed at side
    00359   else
    00360     proportion *= 1.0; // robot height to robot width observed at front
    00361   int depth = (int) proportion;
    00362   Vector2<unsigned char> point,lower(0,0);
    00363   double distance;
    00364   if (firstWhite.x != 1000)
    00365   {
    00366     point = findBlueFootPointYAxis(cfirstWhite,Vector2<char>(0,1),depth);
    00367     if (point.x != 255 && point.y > lower.y)  lower = point;
    00368     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    00369     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    00370     point = findBlueFootPointYAxis(cfirstWhite,Vector2<char>(1,1),depth);
    00371     if (point.x != 255 && point.y > lower.y)  lower = point;
    00372     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    00373     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    00374     point = findBlueFootPointYAxis(cfirstWhite,Vector2<char>(-1,1),depth);
    00375     if (point.x != 255 && point.y > lower.y)  lower = point;
    00376     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    00377     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    00378   }
    00379   if (lastWhite.x != -1000)
    00380   {
    00381     point = findBlueFootPointYAxis(clastWhite,Vector2<char>(0,1),depth);
    00382     if (point.x != 255 && point.y > lower.y)  lower = point;
    00383     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    00384     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    00385     point = findBlueFootPointYAxis(clastWhite,Vector2<char>(1,1),depth);
    00386     if (point.x != 255 && point.y > lower.y)  lower = point;
    00387     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    00388     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    00389     point = findBlueFootPointYAxis(clastWhite,Vector2<char>(-1,1),depth);
    00390     if (point.x != 255 && point.y > lower.y)  lower = point;
    00391     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    00392     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    00393   }
    00394   if (lower.x!=0)
    00395   {
    00396     Vector2<double> dlower;dlower.x = lower.x;dlower.y = lower.y;
    00397     distance = fabs(Geometry::getDistanceToLine(averageLine,dlower));
    00398   }
    00399   else
    00400     distance = 0;
    00401   if (firstBlue.x != 1000 && proportion-distance>0)
    00402   {
    00403     point = findBlueFootPointYAxis(cfirstBlue,Vector2<char>(0,1),depth);
    00404     if (point.x != 255 && point.y > lower.y)  lower = point;
    00405     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    00406     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    00407     point = findBlueFootPointYAxis(cfirstBlue,Vector2<char>(1,1),depth);
    00408     if (point.x != 255 && point.y > lower.y)  lower = point;
    00409     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    00410     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    00411     point = findBlueFootPointYAxis(cfirstBlue,Vector2<char>(-1,1),depth);
    00412     if (point.x != 255 && point.y > lower.y)  lower = point;
    00413     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    00414     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    00415   }
    00416   if (lower.x!=0)
    00417   {
    00418     Vector2<double> dlower;dlower.x = lower.x;dlower.y = lower.y;
    00419     distance = fabs(Geometry::getDistanceToLine(averageLine,dlower));
    00420   }
    00421   else
    00422     distance = 0;
    00423   if (lastBlue.x != -1000 && proportion-distance>0)
    00424   {
    00425     point = findBlueFootPointYAxis(clastBlue,Vector2<char>(0,1),depth);
    00426     if (point.x != 255 && point.y > lower.y)  lower = point;
    00427     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    00428     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    00429     point = findBlueFootPointYAxis(clastBlue,Vector2<char>(1,1),depth);
    00430     if (point.x != 255 && point.y > lower.y)  lower = point;
    00431     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    00432     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    00433     point = findBlueFootPointYAxis(clastBlue,Vector2<char>(-1,1),depth);
    00434     if (point.x != 255 && point.y > lower.y)  lower = point;
    00435     DOT(imageProcessor_robot_detection, point.x, point.y, Drawings::white, Drawings::blue);
    00436     NDOT("imageProcessor:robot detection", point.x, point.y, Drawings::white, Drawings::blue);
    00437   }
    00438
    00439   // create percept
    00440   Vector2<double> pointOnField;
    00441   SinglePlayerPercept percept;
    00442   Vector2<int> robotCenter((averageRobotSignatureStop.x-averageRobotSignatureStart.x)/2+averageRobotSignatureStart.x,lower.y);
    00443   // now take the middle of the averageRobotXLine and the y of the lowest point
    00444   if (Geometry::calculatePointOnField(robotCenter.x, robotCenter.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, percept.offsetOnField))
    00445   {
    00446     CIRCLE(imageProcessor_robot_detection, lower.x, lower.y,10,1, Drawings::ps_solid, Drawings::yellow);
    00447     CIRCLE(imageProcessor_robot_detection, robotCenter.x, robotCenter.y,10,1, Drawings::ps_solid, Drawings::blue);
    00448     NCIRCLE("imageProcessor:robot detection", lower.x, lower.y,10,1, Drawings::ps_solid, Drawings::yellow);
    00449     NCIRCLE("imageProcessor:robot detection", robotCenter.x, robotCenter.y,10,1, Drawings::ps_solid, Drawings::blue);
    00450     percept.directionOnField = 0;
    00451     percept.offsetOnField = percept.offsetOnField;
    00452     percept.positionInImage = robotCenter;
    00453     percept.validity = 1;
    00454
    00455   Vector2<double> foot;
    00456   if (Geometry::calculatePointOnField(lower.x, lower.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, foot)){
    00457     percept.orientation = atan2(percept.offsetOnField.x - foot.x, percept.offsetOnField.y - foot.y);
    00458   }else{
    00459     percept.orientation = 0;
    00460   }
    00461
    00462     playersPercept.addBluePlayer(percept);
    00463   }
    00464 }
    00465
    00466 void GT2005PlayerSpecialist::clusterRobotLinesPercepts()
    00467 {
    00468   // ---------------- Red robots
    00469   clusteredRedRobots = 0;
    00470   if (linesPercept.points[LinesPercept::redRobot].numberOfPoints > 1)
    00471   {
    00472     int lastStart = 0,currentClusteredPoints = 0;
    00473     averageRedRobotSignature[0] = linesPercept.points[LinesPercept::redRobot].pointsInImage[0];
    00474     averageRedRobotSignatureStart[0] = linesPercept.points[LinesPercept::redRobot].pointsInImage[0];
    00475     averageRedRobotSignaturePoints[0] = 0;
    00476     for(int c = 1;c <= linesPercept.points[LinesPercept::redRobot].numberOfPoints;c++)
    00477     {
    00478       averageRedRobotSignatureStart[clusteredRedRobots] = linesPercept.points[LinesPercept::redRobot].pointsInImage[lastStart];
    00479       int diff = linesPercept.points[LinesPercept::redRobot].pointsInImage[c].x -
    00480                 linesPercept.points[LinesPercept::redRobot].pointsInImage[c-1].x;
    00481
    00482       if (c == linesPercept.points[LinesPercept::redRobot].numberOfPoints
    00483         || (diff >= 20 && !checkForWhiteLineBetweenRed(linesPercept.points[LinesPercept::redRobot].pointsInImage[c-1],
    00484         linesPercept.points[LinesPercept::redRobot].pointsInImage[c])))
    00485       {
    00486         averageRedRobotSignatureStop[clusteredRedRobots] = linesPercept.points[LinesPercept::redRobot].pointsInImage[c-1];
    00487         lastStart = c;
    00488         averageRedRobotSignature[clusteredRedRobots] /= currentClusteredPoints;
    00489         averageRedRobotSignaturePoints[clusteredRedRobots] = currentClusteredPoints;
    00490         currentClusteredPoints = 0;
    00491         clusteredRedRobots++;
    00492         averageRedRobotSignaturePoints[clusteredRedRobots] = 0;
    00493         averageRedRobotSignature[clusteredRedRobots] = Vector2<int>(0,0);
    00494       }
    00495       if (clusteredRedRobots>=4)
    00496         break;
    00497       averageRedRobotSignature[clusteredRedRobots] += linesPercept.points[LinesPercept::redRobot].pointsInImage[c];
    00498       currentClusteredPoints++;
    00499     } // for(int c = 1;c <= linesPercept.points[LinesPercept::redRobot].numberOfPoints;c++)
    00500   }   // if (linesPercept.points[LinesPercept::redRobot].numberOfPoints > 1)
    00501   else
    00502   {
    00503     if (linesPercept.points[LinesPercept::redRobot].numberOfPoints==1)
    00504     {
    00505       //only one line
    00506       clusteredRedRobots = 1;
    00507       averageRedRobotSignature[0] = linesPercept.points[LinesPercept::redRobot].pointsInImage[0];
    00508       averageRedRobotSignatureStart[0] = averageRedRobotSignature[0];
    00509       averageRedRobotSignatureStop[0] = averageRedRobotSignature[0];
    00510       averageRedRobotSignaturePoints[0] = linesPercept.points[LinesPercept::redRobot].numberOfPoints;
    00511     }
    00512     else
    00513       clusteredRedRobots = 0;
    00514
    00515   }
    00516   // ---------------- Blue robots
    00517   clusteredBlueRobots = 0;
    00518   if (linesPercept.points[LinesPercept::blueRobot].numberOfPoints > 1)
    00519   {
    00520     int lastStart = 0,currentClusteredPoints = 0;
    00521     averageBlueRobotSignature[0] = linesPercept.points[LinesPercept::blueRobot].pointsInImage[0];
    00522     averageBlueRobotSignatureStart[0] = linesPercept.points[LinesPercept::blueRobot].pointsInImage[0];
    00523     averageBlueRobotSignaturePoints[0] = 0;
    00524     for(int c = 1;c <= linesPercept.points[LinesPercept::blueRobot].numberOfPoints;c++)
    00525     {
    00526       averageBlueRobotSignatureStart[clusteredBlueRobots] = linesPercept.points[LinesPercept::blueRobot].pointsInImage[lastStart];
    00527       int diff = linesPercept.points[LinesPercept::blueRobot].pointsInImage[c].x -
    00528                 linesPercept.points[LinesPercept::blueRobot].pointsInImage[c-1].x;
    00529       if (c == linesPercept.points[LinesPercept::blueRobot].numberOfPoints
    00530         || (diff >= 20 && !checkForWhiteLineBetweenBlue(linesPercept.points[LinesPercept::blueRobot].pointsInImage[c-1],
    00531         linesPercept.points[LinesPercept::blueRobot].pointsInImage[c])))
    00532       {
    00533         averageBlueRobotSignatureStop[clusteredBlueRobots] = linesPercept.points[LinesPercept::blueRobot].pointsInImage[c-1];
    00534         lastStart = c;
    00535         averageBlueRobotSignature[clusteredBlueRobots] /= currentClusteredPoints;
    00536         averageBlueRobotSignaturePoints[clusteredBlueRobots] = currentClusteredPoints;
    00537         currentClusteredPoints = 0;
    00538         clusteredBlueRobots++;
    00539         averageBlueRobotSignaturePoints[clusteredBlueRobots] = 0;
    00540         averageBlueRobotSignature[clusteredBlueRobots] = Vector2<int>(0,0);
    00541       }
    00542       if (clusteredBlueRobots>=4)
    00543         break;
    00544       averageBlueRobotSignature[clusteredBlueRobots] += linesPercept.points[LinesPercept::blueRobot].pointsInImage[c];
    00545       currentClusteredPoints++;
    00546     }
    00547   }
    00548   else
    00549   {
    00550     if (linesPercept.points[LinesPercept::blueRobot].numberOfPoints == 1)
    00551     {
    00552     //only one blue line
    00553       clusteredBlueRobots = 1;
    00554       averageBlueRobotSignature[0] = linesPercept.points[LinesPercept::blueRobot].pointsInImage[0];
    00555       averageBlueRobotSignatureStart[0] = averageBlueRobotSignature[0];
    00556       averageBlueRobotSignatureStop[0] = averageBlueRobotSignature[0];
    00557       averageBlueRobotSignaturePoints[0] = linesPercept.points[LinesPercept::blueRobot].numberOfPoints;
    00558     }
    00559     else
    00560       clusteredBlueRobots = 0;
    00561   }
    00562 }
    00563
    00564 bool GT2005PlayerSpecialist::checkForWhiteLineBetweenBlue(Vector2<int> start, Vector2<int> stop)
    00565 {
    00566   BresenhamLineScan whiteLine (start, stop);
    00567   Vector2<int> begin = start;
    00568
    00569   int wrongColor = 0;
    00570   int overallWhite = 0;
    00571   int overallWrongColor = 0;
    00572   int overallNoColor = 0;
    00573   int overallAccepted = 0;
    00574
    00575   colorClass inspect;
    00576   while(start != stop)
    00577   {
    00578     if ( start.x >= imageInfo.maxImageCoordinates.x
    00579       || start.x < 0
    00580       || start.y >= imageInfo.maxImageCoordinates.y
    00581       || start.y < 0)
    00582     {
    00583       OUTPUT(idText, text, "Blue Bresenham left image");
    00584       break;
    00585     }
    00586     inspect = CORRECTED_COLOR_CLASS(start.x,start.y,
    00587         image.image[start.y][0][start.x],
    00588         image.image[start.y][1][start.x],
    00589         image.image[start.y][2][start.x],
    00590         colorTable, colorCorrector);
    00591     switch(inspect)
    00592     {
    00593       case blue:
    00594       case skyblue:
    00595         overallAccepted++;
    00596         wrongColor = 0;
    00597         break;
    00598       case white:
    00599         wrongColor = 0;
    00600         overallWhite++;
    00601         break;
    00602       case noColor:
    00603         overallNoColor++;
    00604         wrongColor = 0;
    00605         break;
    00606       default:
    00607         wrongColor++;
    00608         overallWrongColor++;
    00609         break;
    00610     }
    00611     whiteLine.getNext(start);
    00612   }
    00613
    00614   if (overallAccepted > overallWhite
    00615     || overallNoColor > (overallAccepted + overallWhite)
    00616     || overallWrongColor > (overallAccepted + overallWhite))
    00617   {
    00618     //LINE(imageProcessor_robot_detection, begin.x, begin.y,
    00619     //   stop.x,stop.y,1,Drawings::white,Drawings::black);
    00620     return false;
    00621   }
    00622   else
    00623   {
    00624     //LINE(imageProcessor_robot_detection, begin.x, begin.y,
    00625     //   stop.x,stop.y,1,Drawings::white,Drawings::blue);
    00626     return true;
    00627   }
    00628
    00629
    00630 }
    00631
    00632 bool GT2005PlayerSpecialist::checkForWhiteLineBetweenRed(Vector2<int> start, Vector2<int> stop)
    00633 {
    00634   BresenhamLineScan whiteLine (start, stop);
    00635   Vector2<int> begin = start;
    00636   int wrongColor = 0;
    00637   int overallWhite = 0;
    00638   int overallWrongColor = 0;
    00639   int overallNoColor = 0;
    00640   int overallAccepted = 0;
    00641
    00642   colorClass inspect;
    00643   while(start!=stop)
    00644   {
    00645     if ( start.x >= imageInfo.maxImageCoordinates.x
    00646       || start.x < 0
    00647       || start.y >= imageInfo.maxImageCoordinates.y
    00648       || start.y < 0)
    00649     {
    00650       OUTPUT(idText, text, "Red Bresenham left image");
    00651       break;
    00652     }
    00653     inspect = CORRECTED_COLOR_CLASS(start.x,start.y,
    00654         image.image[start.y][0][start.x],
    00655         image.image[start.y][1][start.x],
    00656         image.image[start.y][2][start.x],
    00657         colorTable, colorCorrector);
    00658     switch(inspect)
    00659     {
    00660       case red:
    00661       case pink:
    00662         overallAccepted++;
    00663         wrongColor = 0;
    00664         break;
    00665       case white:
    00666         wrongColor = 0;
    00667         overallWhite++;
    00668         break;
    00669       case noColor:
    00670         overallNoColor++;
    00671         wrongColor = 0;
    00672         break;
    00673       default:
    00674         wrongColor++;
    00675         overallWrongColor++;
    00676         break;
    00677     }
    00678     whiteLine.getNext(start);
    00679   }
    00680
    00681   if (overallAccepted > overallWhite
    00682     || overallNoColor > (overallAccepted + overallWhite)
    00683     || overallWrongColor > (overallAccepted + overallWhite))
    00684   {
    00685   //  LINE(imageProcessor_robot_detection, begin.x, begin.y,
    00686   //     stop.x,stop.y,1,Drawings::white,Drawings::black);
    00687     return false;
    00688   }
    00689   else
    00690   {
    00691   //  LINE(imageProcessor_robot_detection, begin.x, begin.y,
    00692   //     stop.x,stop.y,1,Drawings::white,Drawings::red);
    00693     return true;
    00694   }
    00695
    00696 }
    00697
    00698 Vector2<unsigned char> GT2005PlayerSpecialist::findRedFootPointYAxis(
    00699   Vector2<unsigned char> start,
    00700   Vector2<char> direction,
    00701   short depth,
    00702   unsigned char recursionLevel)
    00703 {
    00704   if (recursionLevel == 3)
    00705   {
    00706     OUTPUT(idText, text, "Maximum Red Recursion Level reached #" << recursionLevel);
    00707     return start;
    00708   }
    00709   Vector2<unsigned char> runner = start;
    00710   short steps = 0;
    00711   while(true)
    00712   {
    00713     if ( runner.x >= imageInfo.maxImageCoordinates.x
    00714       || runner.x <= 0
    00715       || runner.y >= imageInfo.maxImageCoordinates.y
    00716       || runner.y <= 0)
    00717         return Vector2<unsigned char>(255,0);
    00718     colorClass inspect = CORRECTED_COLOR_CLASS(runner.x,runner.y,
    00719       image.image[runner.y][0][runner.x],
    00720       image.image[runner.y][1][runner.x],
    00721       image.image[runner.y][2][runner.x],
    00722       colorTable, colorCorrector);
    00723
    00724     if (inspect == green)
    00725           break;
    00726     runner.x += direction.x;
    00727     runner.y += direction.y;
    00728     steps++;
    00729     if (--depth > 5)
    00730     {
    00731       runner.x += direction.x;
    00732       runner.y += direction.y;
    00733       --depth;
    00734       steps++;
    00735     }
    00736   }
    00737
    00738   LINE(imageProcessor_robot_detection, start.x, start.y,
    00739        runner.x,runner.y,1,Drawings::white,
    00740        depth>15 ? Drawings::yellow : Drawings::blue);
    00741   NLINE("imageProcessor:robot detection", start.x, start.y,
    00742        runner.x,runner.y,1,Drawings::white,
    00743        depth>15 ? Drawings::yellow : Drawings::blue);
    00744   maxDepth = max(maxDepth,(int)recursionLevel);
    00745   if (steps==0)
    00746   {
    00747     DOT(imageProcessor_robot_detection, runner.x,runner.y,Drawings::white, Drawings::green);
    00748     return runner;
    00749     NDOT("imageProcessor:robot detection", runner.x,runner.y,Drawings::white, Drawings::green);
    00750     return runner;
    00751   }
    00752   Vector2<unsigned char> down, downLeft, downRight;
    00753   if (!(direction.x == 0 && direction.y == 1))
    00754     down = findRedFootPointYAxis(runner,Vector2<char>(0,1),depth,recursionLevel+1);
    00755   if (!(direction.x == -1 && direction.y == 1))
    00756     downLeft = findRedFootPointYAxis(runner,Vector2<char>(-1,1),depth,recursionLevel+1);
    00757   if (!(direction.x == 1 && direction.y == 1))
    00758     downRight = findRedFootPointYAxis(runner,Vector2<char>(1,1),depth,recursionLevel+1);
    00759
    00760
    00761
    00762   if (down.x != 255 && down.y > downLeft.y)
    00763   {
    00764     if (downRight.x != 255 && downRight.y > down.y)
    00765       return downRight;
    00766     else
    00767       return down;
    00768   }
    00769   else
    00770   {
    00771     if (downLeft.x != 255 && downLeft.y > downRight.y)
    00772       return downLeft;
    00773     else
    00774       return downRight;
    00775   }
    00776 }
    00777
    00778 Vector2<unsigned char> GT2005PlayerSpecialist::findBlueFootPointYAxis(
    00779   Vector2<unsigned char> start,
    00780   Vector2<char> direction,
    00781   short depth,
    00782   unsigned char recursionLevel)
    00783 {
    00784   if (recursionLevel == 3)
    00785   {
    00786     OUTPUT(idText, text, "Maximum Blue Recursion Level reached #" << recursionLevel);
    00787     return start;
    00788   }
    00789   Vector2<unsigned char> runner = start;
    00790   short steps = 0;
    00791   while(true)
    00792   {
    00793     if ( runner.x >= imageInfo.maxImageCoordinates.x
    00794       || runner.x <= 0
    00795       || runner.y >= imageInfo.maxImageCoordinates.y
    00796       || runner.y <= 0)
    00797         return Vector2<unsigned char>(255,0);
    00798     colorClass inspect = CORRECTED_COLOR_CLASS(runner.x,runner.y,
    00799       image.image[runner.y][0][runner.x],
    00800       image.image[runner.y][1][runner.x],
    00801       image.image[runner.y][2][runner.x],
    00802       colorTable, colorCorrector);
    00803
    00804     if (inspect == green)
    00805           break;
    00806     runner.x += direction.x;
    00807     runner.y += direction.y;
    00808     steps++;
    00809     if (--depth > 5)
    00810     {
    00811       runner.x += direction.x;
    00812       runner.y += direction.y;
    00813       --depth;
    00814       steps++;
    00815     }
    00816   }
    00817
    00818   LINE(imageProcessor_robot_detection, start.x, start.y,
    00819        runner.x,runner.y,1,Drawings::white,
    00820        depth>15 ? Drawings::yellow : Drawings::blue);
    00821   NLINE("imageProcessor:robot detection", start.x, start.y,
    00822        runner.x,runner.y,1,Drawings::white,
    00823        depth>15 ? Drawings::yellow : Drawings::blue);
    00824   if (steps==0)
    00825   {
    00826     DOT(imageProcessor_robot_detection, runner.x,runner.y,Drawings::white, Drawings::green);
    00827     NDOT("imageProcessor:robot detection", runner.x,runner.y,Drawings::white, Drawings::green);
    00828     return runner;
    00829   }
    00830   Vector2<unsigned char> down, downLeft, downRight;
    00831   if (!(direction.x == 0 && direction.y == 1))
    00832     down = findBlueFootPointYAxis(runner,Vector2<char>(0,1),depth,recursionLevel+1);
    00833   if (!(direction.x == -1 && direction.y == 1))
    00834     downLeft = findBlueFootPointYAxis(runner,Vector2<char>(-1,1),depth,recursionLevel+1);
    00835   if (!(direction.x == 1 && direction.y == 1))
    00836     downRight = findBlueFootPointYAxis(runner,Vector2<char>(1,1),depth,recursionLevel+1);
    00837
    00838
    00839   maxDepth = max(maxDepth,(int)recursionLevel);
    00840   if (down.x != 255 && down.y > downLeft.y)
    00841   {
    00842     if (downRight.x != 255 && downRight.y > down.y)
    00843       return downRight;
    00844     else
    00845       return down;
    00846   }
    00847   else
    00848   {
    00849     if (downLeft.x != 255 && downLeft.y > downRight.y)
    00850       return downLeft;
    00851     else
    00852       return downRight;
    00853   }
    00854 }
    00855 colorClass GT2005PlayerSpecialist::inspectColorArea(const Vector2<unsigned char>& pos,const Vector2<char>& dir)
    00856 {
    00857   // inspects a 2x2 color square and return the popularst color
    00858   if ( pos.x >= imageInfo.maxImageCoordinates.x
    00859       || pos.x <= 0
    00860       || pos.y >= imageInfo.maxImageCoordinates.y
    00861       || pos.y <= 0)
    00862       return CORRECTED_COLOR_CLASS(pos.x,pos.y,
    00863         image.image[pos.y][0][pos.x],
    00864         image.image[pos.y][1][pos.x],
    00865         image.image[pos.y][2][pos.x],
    00866         colorTable, colorCorrector);
    00867
    00868   colorClass inspect;
    00869   int cGreen = 0, cOkay = 0;
    00870   if (dir.x == 0 || dir.y == 0)
    00871   {
    00872     // orthogonal to axis
    00873     inspect = CORRECTED_COLOR_CLASS(pos.x,pos.y,
    00874       image.image[pos.y][0][pos.x],
    00875       image.image[pos.y][1][pos.x],
    00876       image.image[pos.y][2][pos.x],
    00877       colorTable, colorCorrector);
    00878     if (inspect == green) cGreen++;
    00879     if (inspect == white || inspect == red || inspect == noColor) cOkay++;
    00880
    00881     Vector2<int> insPos;
    00882     insPos.x = pos.x + dir.x;
    00883     insPos.y = pos.y + dir.y;
    00884     inspect = CORRECTED_COLOR_CLASS(insPos.x,insPos.y,
    00885       image.image[insPos.y][0][insPos.x],
    00886       image.image[insPos.y][1][insPos.x],
    00887       image.image[insPos.y][2][insPos.x],
    00888       colorTable, colorCorrector);
    00889     if (inspect == green) cGreen++;
    00890     if (inspect == white || inspect == red || inspect == noColor) cOkay++;
    00891
    00892     insPos.x = pos.x+dir.x;insPos.y = pos.y+dir.y; insPos.x += dir.y; insPos.y += dir.x;
    00893     inspect = CORRECTED_COLOR_CLASS(insPos.x,insPos.y,
    00894       image.image[insPos.y][0][insPos.x],
    00895       image.image[insPos.y][1][insPos.x],
    00896       image.image[insPos.y][2][insPos.x],
    00897       colorTable, colorCorrector);
    00898     if (inspect == green) cGreen++;
    00899     if (inspect == white || inspect == red || inspect == noColor) cOkay++;
    00900
    00901     insPos.x = pos.x+dir.x;insPos.y = pos.y+dir.y; insPos.x -= dir.y; insPos.y -=dir.x;
    00902     inspect = CORRECTED_COLOR_CLASS(insPos.x,insPos.y,
    00903       image.image[insPos.y][0][insPos.x],
    00904       image.image[insPos.y][1][insPos.x],
    00905       image.image[insPos.y][2][insPos.x],
    00906       colorTable, colorCorrector);
    00907     if (inspect == green) cGreen++;
    00908     if (inspect == white || inspect == red || inspect == noColor) cOkay++;
    00909   }
    00910   else
    00911   {
    00912     // diagonal to axis
    00913     inspect = CORRECTED_COLOR_CLASS(pos.x,pos.y,
    00914       image.image[pos.y][0][pos.x],
    00915       image.image[pos.y][1][pos.x],
    00916       image.image[pos.y][2][pos.x],
    00917       colorTable, colorCorrector);
    00918     if (inspect == green) cGreen++;
    00919     if (inspect == white || inspect == red || inspect == noColor) cOkay++;
    00920
    00921     Vector2<int> insPos;
    00922     insPos.x = pos.x + dir.x;
    00923     insPos.y = pos.y + dir.y;
    00924     inspect = CORRECTED_COLOR_CLASS(insPos.x,insPos.y,
    00925       image.image[insPos.y][0][insPos.x],
    00926       image.image[insPos.y][1][insPos.x],
    00927       image.image[insPos.y][2][insPos.x],
    00928       colorTable, colorCorrector);
    00929     if (inspect == green) cGreen++;
    00930     if (inspect == white || inspect == red || inspect == noColor) cOkay++;
    00931
    00932     insPos.x = pos.x + dir.x;insPos.y = pos.y;
    00933     inspect = CORRECTED_COLOR_CLASS(insPos.x,insPos.y,
    00934       image.image[insPos.y][0][insPos.x],
    00935       image.image[insPos.y][1][insPos.x],
    00936       image.image[insPos.y][2][insPos.x],
    00937       colorTable, colorCorrector);
    00938     if (inspect == green) cGreen++;
    00939     if (inspect == white || inspect == red || inspect == noColor) cOkay++;
    00940
    00941     insPos.y = pos.y+dir.y;insPos.x = pos.x;
    00942     inspect = CORRECTED_COLOR_CLASS(insPos.x,insPos.y,
    00943       image.image[insPos.y][0][insPos.x],
    00944       image.image[insPos.y][1][insPos.x],
    00945       image.image[insPos.y][2][insPos.x],
    00946       colorTable, colorCorrector);
    00947     if (inspect == green) cGreen++;
    00948     if (inspect == white || inspect == red || inspect == noColor) cOkay++;
    00949   }
    00950   if (cGreen >= cOkay)
    00951     return green;
    00952   else
    00953     return white;
    00954 }
    00955
    00956