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

# Modules/ImageProcessor/VLCImageProcessor/VLCRobotSpecialist.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file VLCRobotSpecialist.cpp
    00003 *
    00004 * This file contains a class for Image Processing.
    00005 *
    00006 * @author Tobias Wegner
    00007 */
    00008
    00009 #include "VLCRobotSpecialist.h"
    00010
    00011 #ifdef RS_DEBUG
    00012 #ifndef APERIOS1_3_2
    00013 #pragma message( "compiling VLCRobotSpecialist with debugging" )
    00014 #endif
    00015 #endif
    00016
    00017 #ifdef RS_DEBUG_CONSOLE
    00018 #ifndef APERIOS1_3_2
    00019 #pragma message( "compiling VLCRobotSpecialist with console outputs" )
    00020 #endif
    00021 #endif
    00022
    00023 //#define RS_UNIFORM_SCANLINES
    00024 #define RS_RADIAL_ON_IMAGE
    00025
    00026 //dirty hack, prevents from compiler errors
    00027 #define min(a, b)(((a) < (b))?(a):(b))
    00028 #define max(a, b)(((a) > (b))?(a):(b))
    00029
    00030 VLCRobotSpecialist::VLCRobotSpecialist
    00031 (
    00032  const ColorCorrector& colorCorrector_,
    00033  const Image& image_,
    00034  const ImageInfo& imageInfo_,
    00035  const ColorTable& colorTable_,
    00036  const CameraMatrix& cameraMatrix_,
    00037  const CameraMatrix& prevCameraMatrix_,
    00038  LinesPercept& linesPercept_,
    00039  PlayersPercept& playersPercept_,
    00040  const RobotPose& robotPose_,
    00041  const List<GreenChangeEvent>& greenChangeEvents_)
    00042  :
    00043 colorCorrector(colorCorrector_),
    00044 image(image_),
    00045 imageInfo(imageInfo_),
    00046 colorTable(colorTable_),
    00047 cameraMatrix(cameraMatrix_),
    00048 prevCameraMatrix(prevCameraMatrix_),
    00049 linesPercept(linesPercept_),
    00050 playersPercept(playersPercept_),
    00051 robotPose(robotPose_),
    00052 greenChangeEvents(greenChangeEvents_)
    00053 {
    00054 }
    00055
    00056 void VLCRobotSpecialist::reset ()
    00057 {
    00058   //reset candidates
    00059   topIndex = RS_NUM_CANDIDATES / 2 + 1;
    00060   bottomIndex = RS_NUM_CANDIDATES / 2;
    00061 }
    00062
    00063 void VLCRobotSpecialist::scan ()
    00064 {
    00065   //some types
    00066   Geometry::Line scanLine;
    00067   Geometry::Line clipLine;
    00068
    00069 #ifdef RS_RADIAL_ON_IMAGE
    00070   Vector2<int>  startPoint  = Vector2<int>(image.cameraInfo.resolutionWidth / 2, image.cameraInfo.resolutionHeight - 1);
    00071 #else
    00072   Vector2<int>  startPoint;
    00073
    00074   Geometry::calculatePointInImage (Vector2<int>(475, 0), cameraMatrix, CameraInfo::CameraInfo (), startPoint);
    00075
    00076   startPoint.y = startPoint.y;
    00077 #endif
    00078   Vector2<int>  endPoint;
    00079
    00080   //reset everything
    00081   reset ();
    00082
    00083   //calculation scan line
    00084   scanLine.base.x = startPoint.x;
    00085   scanLine.base.y = startPoint.y;
    00086
    00087   int i = -1;
    00088     double startAngle = -pi_2;
    00089     double stopAngle = pi_2;
    00090     double stepAngle = (stopAngle - startAngle) / RS_NUM_SCAN_LINES;
    00091
    00092   for (double angle = startAngle; angle <= stopAngle; angle += stepAngle)
    00093   {
    00094     CRASH_CHECK(i); i++;
    00095
    00096     //resetting start point
    00097     startPoint.x = (int) scanLine.base.x;
    00098     startPoint.y = (int) scanLine.base.y;
    00099
    00100     //calculation of radial scanline directions
    00101 #ifdef RS_RADIAL_ON_IMAGE
    00102     scanLine.direction.x = sin (angle);
    00103     scanLine.direction.y = cos (angle);
    00104 #else
    00105     Vector2<int>directionOnField;
    00106     Vector2<int>directionInImage;
    00107
    00108     directionOnField.x = (int) (cos (angle) * 1000000);
    00109     directionOnField.y = (int) (-sin (angle) * 1000000 + 475);
    00110
    00111     //projecting to field
    00112     Geometry::calculatePointInImage (directionOnField, cameraMatrix, CameraInfo::CameraInfo (), directionInImage);
    00113
    00114     scanLine.direction.x = directionInImage.x;
    00115     scanLine.direction.y = directionInImage.y;
    00116 #endif
    00117     scanLine.normalizeDirection ();
    00118
    00119     //intersecting with horizon
    00120     clipLine = imageInfo.horizon;
    00121
    00122     if ((scanLine.direction * clipLine.direction) == 1)
    00123       continue;
    00124
    00125     Geometry::getIntersectionOfLines(clipLine, scanLine, endPoint);
    00126
    00127     //clipping, test if intersection lies in image
    00128     if (((endPoint.x < 0) || (endPoint.x >= image.cameraInfo.resolutionWidth)) || ((endPoint.y < 0) || (endPoint.y >= image.cameraInfo.resolutionHeight)))
    00129     {
    00130       //intersection is outside of image, so calculate new new intersection
    00131       if (endPoint.x < 0)
    00132       {
    00133         //intersection was to left, so intersect with left boundary
    00134         clipLine.base = Vector2<double> (0, 0);
    00135         clipLine.direction = Vector2<double> (0, 1);
    00136
    00137         Geometry::getIntersectionOfLines(clipLine, scanLine, endPoint);
    00138       }
    00139       else
    00140         if (endPoint.x >= image.cameraInfo.resolutionWidth)
    00141         {
    00142           //intersection lies too right, so intersect with right boundary
    00143           clipLine.base = Vector2<double> (image.cameraInfo.resolutionWidth, 0);
    00144           clipLine.direction = Vector2<double> (0, 1);
    00145
    00146           Geometry::getIntersectionOfLines(clipLine, scanLine, endPoint);
    00147         };
    00148
    00149       if (endPoint.y < 0)
    00150       {
    00151         //intersection lies too high above, so intersect upper left boundary
    00152         clipLine.base = Vector2<double> (0, 0);
    00153         clipLine.direction = Vector2<double> (1, 0);
    00154
    00155         Geometry::getIntersectionOfLines(clipLine, scanLine, endPoint);
    00156       }
    00157
    00158       if (endPoint.y >= image.cameraInfo.resolutionHeight)
    00159         continue;                               //intersection is below image, so no scanning needed
    00160     };
    00161
    00162     if (((startPoint.x < 0) || (startPoint.x >= image.cameraInfo.resolutionWidth)) || ((startPoint.y < 0) || (startPoint.y >= image.cameraInfo.resolutionHeight)))
    00163     {
    00164       //intersection is outside of image, so calculate new new intersection
    00165       if (startPoint.x < 0)
    00166       {
    00167         //intersection was to left, so intersect with left boundary
    00168         clipLine.base = Vector2<double> (0, 0);
    00169         clipLine.direction = Vector2<double> (0, 1);
    00170
    00171         Geometry::getIntersectionOfLines(clipLine, scanLine, startPoint);
    00172       }
    00173       else
    00174         if (startPoint.x >= image.cameraInfo.resolutionWidth)
    00175         {
    00176           //intersection lies too right, so intersect with right boundary
    00177           clipLine.base = Vector2<double> (image.cameraInfo.resolutionWidth, 0);
    00178           clipLine.direction = Vector2<double> (0, 1);
    00179
    00180           Geometry::getIntersectionOfLines(clipLine, scanLine, startPoint);
    00181         };
    00182
    00183       if (startPoint.y >= image.cameraInfo.resolutionHeight)
    00184       {
    00185         //intersection lies too high above, so intersect upper left boundary
    00186         clipLine.base = Vector2<double> (0, image.cameraInfo.resolutionHeight - 1);
    00187         clipLine.direction = Vector2<double> (1, 0);
    00188
    00189         Geometry::getIntersectionOfLines(clipLine, scanLine, startPoint);
    00190       }
    00191
    00192       if (startPoint.y < 0)
    00193         continue;                               //intersection is below image, so no scanning needed
    00194     };
    00195
    00196 #ifndef RS_UNIFORM_SCANLINES
    00197     switch (i % 4)
    00198     {
    00199     case 0: { //full line
    00200           break;
    00201         };
    00202     case 1: { //half line
    00203         };
    00204     case 3: { //half line
    00205           //only half size scan line
    00206           startPoint.x = (startPoint.x + endPoint.x) / 2;
    00207           startPoint.y = (startPoint.y + endPoint.y) / 2;
    00208
    00209           break;
    00210         };
    00211     case 2: {
    00212           //only quarter size scan line
    00213           startPoint.x = (startPoint.x + 3 * endPoint.x) / 4;
    00214           startPoint.y = (startPoint.y + 3 * endPoint.y) / 4;
    00215
    00216           break;
    00217         };
    00218     };
    00219 #endif
    00220     scan (startPoint, endPoint);
    00221   };
    00222
    00223   clusterCandidates ();
    00224
    00225   return;
    00226 }
    00227
    00228 void VLCRobotSpecialist::scan (Vector2<int> startPoint)
    00229 {
    00230   unsigned char y, u, v;
    00231   colorClass    lastColor = noColor;
    00232
    00233   Vector2<int>  endPoint;
    00234   Geometry::Line  scanLine;
    00235
    00236   //calculating line perpendicular to horizon
    00237   scanLine.base.x = startPoint.x;
    00238   scanLine.base.y = startPoint.y;
    00239   scanLine.direction.x = - imageInfo.horizon.direction.y;
    00240   scanLine.direction.y = imageInfo.horizon.direction.x;
    00241
    00242   //calculating endpoint for bresenham
    00243   Geometry::getIntersectionOfLines (imageInfo.horizon, scanLine, endPoint);
    00244
    00245   // init Bresenham
    00246   BresenhamLineScan bresenhamScanLine = BresenhamLineScan (startPoint, endPoint);
    00247   Vector2<int>    actual    = startPoint;
    00248
    00249   Vector2<int> footPoint = startPoint;
    00250
    00251   for (int i = 0; i <= bresenhamScanLine.numberOfPixels; ++i)
    00252   {
    00253     CRASH_CHECK(i);
    00254
    00255     if (actual.x < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y < 0 || actual.y >= image.cameraInfo.resolutionHeight)
    00256       y = u = v = 0;
    00257     else
    00258     {
    00259       y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    00260       u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    00261       v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    00262     }
    00263       colorClass color = COLOR_CLASS(y, u, v);
    00264
    00265     if ((color == green) || (color == orange))
    00266     {
    00267       footPoint = actual;
    00268     };
    00269
    00270     if ((color == blue) || (color == red))
    00271     {
    00272       if ((color != lastColor) || (color == noColor))
    00273       {
    00274         //other robot found
    00275         RobotCandidate  candidate;
    00276
    00277         candidate.footPoint = footPoint;
    00278         candidate.candidateStart = actual;
    00279         candidate.color = color;
    00280
    00281         addCandidate (candidate);
    00282
    00283         lastColor = color;
    00284       }
    00285     }
    00286
    00287     bresenhamScanLine.getNext (actual);
    00288   }
    00289
    00290 #ifdef RS_DEBUG
    00291   LINE(imageProcessor_robot_detection,
    00292     startPoint.x, startPoint.y, endPoint.x, endPoint.y,
    00293     1, Drawings::ps_solid, Drawings::blue );
    00294 #endif
    00295
    00296   return;
    00297 }
    00298
    00299 void VLCRobotSpecialist::scan (Vector2<int> startPoint, Vector2<int> endPoint)
    00300 {
    00301   // init Bresenham
    00302   BresenhamLineScan scanLine  = BresenhamLineScan (startPoint, endPoint);
    00303   Vector2<int>    actual    = startPoint;
    00304
    00305   unsigned char y, u, v;
    00306
    00307   Vector2<int>  lastGreenPosition;
    00308   int       lastGreenCount    = -1;
    00309   bool      searchForGreen    = false;
    00310
    00311   // test whether or not startpoint has green color
    00312   if (actual.x < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y < 0 || actual.y >= image.cameraInfo.resolutionHeight)
    00313     y = u = v = 0;
    00314   else
    00315   {
    00316     y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    00317     u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    00318     v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    00319   }
    00320   colorClass color = COLOR_CLASS(y, u, v);
    00321
    00322   if (color != green)
    00323     return;                             //dont know, where green stopped, so better leave
    00324
    00325   double threshold = RS_NGP_TH_START;
    00326
    00327   for (int i = 0; i <= scanLine.numberOfPixels; ++i)
    00328   {
    00329     CRASH_CHECK(i);
    00330
    00331     // <copied from imp>
    00332     if (actual.x < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y < 0 || actual.y >= image.cameraInfo.resolutionHeight)
    00333       y = u = v = 0;
    00334     else
    00335     {
    00336       y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    00337       u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    00338       v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    00339     }
    00340       colorClass color = COLOR_CLASS(y, u, v);
    00341     //<copied end>
    00342
    00343 #ifndef RS_DEBUG_DRAW_RADIAL_SCANLINES_ONLY
    00344     //search for obstacles
    00345     if ((color == green) || (color == noColor) || (searchForGreen))
    00346     {
    00347       if (color == green)
    00348       {
    00349 #ifdef RS_FIXED_NO_GREEN_THRESHOLD
    00350         if (i - lastGreenCount < threshold)
    00351 #else
    00352         if (i - lastGreenCount < 2 + (1.0 - ((double) i / (double) scanLine.numberOfPixels)) * 7.5)
    00353 #endif
    00354           lastGreenCount = -1;
    00355
    00356         searchForGreen = false;
    00357       }
    00358     }
    00359     else
    00360     {
    00361       //check how many not green pixels are on the line
    00362       if (lastGreenCount < 0)
    00363       {
    00364         lastGreenCount = i;
    00365         lastGreenPosition = actual;
    00366       }
    00367
    00368 #ifdef RS_FIXED_NO_GREEN_THRESHOLD
    00369       if (i - lastGreenCount >= RS_NO_GREEN_PIXEL_THRESHOLD)
    00370 #else
    00371       if (i - lastGreenCount >= threshold)
    00372 #endif
    00373       {
    00374         //obstacle found
    00375         scan (lastGreenPosition);
    00376
    00377         searchForGreen = true;
    00378       }
    00379     }
    00380 #endif
    00381
    00382     scanLine.getNext (actual);
    00383
    00384     threshold -= (RS_NGP_TH_START - RS_NGP_TH_STOP) / scanLine.numberOfPixels;
    00385   };
    00386
    00387 #ifdef RS_DEBUG
    00388   LINE(imageProcessor_robot_detection,
    00389     startPoint.x, startPoint.y, endPoint.x, endPoint.y,
    00390     1, Drawings::ps_solid, Drawings::blue );
    00391 #endif
    00392
    00393   return;
    00394 }
    00395
    00396 void VLCRobotSpecialist::addCandidate (RobotCandidate candidate)
    00397 {
    00398   int index;
    00399
    00400   //a lineid > 0 means, that we are scanning left to right, so insert right to preserve order
    00401   if (candidate.lineID > 0)
    00402     index = topIndex;
    00403   else
    00404     index = bottomIndex;
    00405
    00406   //test index inside array boundary
    00407   if ((index > 0) && (index < RS_NUM_CANDIDATES))
    00408   {
    00409     //yes is, so save
    00410     candidates[index] = candidate;
    00411
    00412     //move borders
    00413     if (candidate.lineID > 0)
    00414             topIndex++;
    00415     else
    00416       bottomIndex--;
    00417   }
    00418   else
    00419   {
    00420     //no, outside, we had too many candidates
    00421     RECTANGLE (imageProcessor_robot_detection, candidate.footPoint.x - 2, candidate.footPoint.y - 2, candidate.footPoint.x + 2, candidate.footPoint.y + 2, 4, Drawings::ps_solid, Drawings::gray);
    00422   }
    00423
    00424   //do debug drawings
    00425 #ifdef  RS_DEBUG
    00426   {
    00427     if (candidate.color == blue)
    00428     {
    00429 #ifdef RS_DEBUG_SHOW_START_RUN
    00430       CIRCLE (imageProcessor_robot_detection, candidate.candidateStart.x, candidate.candidateStart.y, 1, 1, Drawings::ps_dot, Drawings::blue);
    00431 #endif
    00432
    00433 #ifdef RS_DEBUG_SHOW_RUNS
    00434       LINE (imageProcessor_robot_detection, candidate.candidateStart.x, candidate.candidateStart.y, candidate.candidateStop.x, candidate.candidateStop.y, 1, Drawings::ps_solid, Drawings::blue);
    00435 #endif
    00436     }
    00437     else
    00438     {
    00439 #ifdef RS_DEBUG_SHOW_START_RUN
    00440       CIRCLE (imageProcessor_robot_detection, candidate.candidateStart.x, candidate.candidateStart.y, 1, 1, Drawings::ps_dot, Drawings::red);
    00441 #endif
    00442
    00443 #ifdef RS_DEBUG_SHOW_RUNS
    00444       LINE (imageProcessor_robot_detection, candidate.candidateStart.x, candidate.candidateStart.y, candidate.candidateStop.x, candidate.candidateStop.y, 1, Drawings::ps_solid, Drawings::red);
    00445 #endif
    00446     }
    00447   }
    00448 #endif
    00449 }
    00450
    00451 bool VLCRobotSpecialist::findNearestFootPoint (Vector2<int> &footPoint)
    00452 {
    00453   enum Gradient
    00454   {
    00455     equal,
    00456     up,
    00457     down
    00458   };
    00459
    00460   Gradient  leftGradient = equal, rightGradient = equal;
    00461
    00462   GreenChangeEvent  lastLeft, lastRight;
    00463   int         delta;
    00464
    00465   int         orgLineID;
    00466
    00467   bool        nextLeftStepFound = false;
    00468   bool        nextRightStepFound  = false;
    00469
    00470   Vector2<int>    maxLeftPoint;
    00471   int         maxLeftDistance;
    00472
    00473   Vector2<int>    maxRightPoint;
    00474   int         maxRightDistance;
    00475
    00476   GreenChangeEvent tmpEvent;
    00477
    00478   int i = 0;
    00479
    00480   //first loop throug all feet, to find one fitting to candidates foot
    00481   while (footIterator)
    00482   {
    00483     CRASH_CHECK(i); i++;
    00484
    00485     GreenChangeEvent event = greenChangeEvents[footIterator];
    00486
    00487     //only look for start events
    00488     if (event.startEvent)
    00489       if (event.positionOnImage == footPoint)
    00490       {
    00491         //found start point for search
    00492         orgLineID = event.lineID;
    00493
    00494         //init iterators
    00495         maxLeftPoint = footPoint;
    00496         maxLeftDistance = event.distanceToHorizon;
    00497
    00498         maxRightPoint = footPoint;
    00499         maxRightDistance = event.distanceToHorizon;
    00500
    00501         List<GreenChangeEvent>::Pos leftIter = footIterator;
    00502         List<GreenChangeEvent>::Pos rightIter = footIterator;
    00503
    00504         lastLeft = event;
    00505         lastRight = event;
    00506
    00507         leftIter--;
    00508         rightIter++;
    00509
    00510         int j = 0;
    00511
    00512         //loop while both iterators are inside the list boundary
    00513         while (leftIter | rightIter)
    00514         {
    00515           CRASH_CHECK(j); j++;
    00516
    00517           //look left, so find next left foot
    00518           int k = 0;
    00519
    00520           while (leftIter)
    00521           {
    00522             CRASH_CHECK(k); k++;
    00523             event = greenChangeEvents[leftIter];
    00524
    00525             //check line id
    00526             if (lastLeft.lineID > ((GreenChangeEvent)greenChangeEvents[leftIter]).lineID + 1)
    00527             {
    00528               //new line id is 2 less than last ones, so we went too far and have to go back
    00529               leftIter++;
    00530
    00531               //do we have a start event?
    00532               if (!((GreenChangeEvent)greenChangeEvents[leftIter]).startEvent ||
    00533                 (((GreenChangeEvent)greenChangeEvents[leftIter]).lineID == lastLeft.lineID))
    00534               {
    00535                 //no, or we are on the last line, there is one line missing, draw orange
    00536 #ifdef RS_DEBUG_FEET
    00537                 LINE (imageProcessor_robot_detection, lastLeft.positionOnImage.x, lastLeft.positionOnImage.y, greenChangeEvents[leftIter].positionOnImage.x, greenChangeEvents[leftIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::orange);
    00538 #endif
    00539
    00540                 //end search
    00541                 leftIter = NULL;
    00542
    00543                 continue;
    00544               }
    00545               else
    00546               {
    00547                 //yes, start event so continue search
    00548                 nextLeftStepFound = true;
    00549               };
    00550             };
    00551
    00552             //are we too far from original line?
    00553             if (orgLineID - event.lineID > RS_FOOT_THRESHOLD)
    00554             {
    00555               //yes, search went to far away
    00556 #ifdef RS_DEBUG_FEET
    00557               LINE (imageProcessor_robot_detection, lastLeft.positionOnImage.x, lastLeft.positionOnImage.y, greenChangeEvents[leftIter].positionOnImage.x, greenChangeEvents[leftIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::white);
    00558 #endif
    00559               //so end search
    00560               leftIter = NULL;
    00561
    00562               continue;
    00563             };
    00564
    00565             if ((((GreenChangeEvent)greenChangeEvents[leftIter]).distanceToHorizon <= lastLeft.distanceToHorizon) &&
    00566               (((GreenChangeEvent)greenChangeEvents[leftIter]).lineID) < lastLeft.lineID)
    00567             {
    00568               //current event is closer to horizon than last one, and on a smaler scanline
    00569               if (!((GreenChangeEvent)greenChangeEvents[leftIter]).startEvent)
    00570               {
    00571                 //not a startevent, so go one step back
    00572                 leftIter++;
    00573
    00574                 tmpEvent = greenChangeEvents[leftIter];
    00575
    00576                 if (lastLeft.lineID == ((GreenChangeEvent)greenChangeEvents[leftIter]).lineID)
    00577                 {
    00578                   //back on old scanline, so draw pink debug drawing
    00579 #ifdef RS_DEBUG_FEET
    00580                   LINE (imageProcessor_robot_detection, lastLeft.positionOnImage.x, lastLeft.positionOnImage.y, greenChangeEvents[leftIter].positionOnImage.x, greenChangeEvents[leftIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::red);
    00581 #endif
    00582
    00583                   leftIter = NULL;  //switched back, error
    00584
    00585                   continue;
    00586                 }
    00587                 else
    00588                   if (!((GreenChangeEvent)greenChangeEvents[leftIter]).startEvent)
    00589                   {
    00590                     //start event expected, so draw black
    00591 #ifdef RS_DEBUG_FEET
    00592                     LINE (imageProcessor_robot_detection, lastLeft.positionOnImage.x, lastLeft.positionOnImage.y, greenChangeEvents[leftIter].positionOnImage.x, greenChangeEvents[leftIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::black);
    00593 #endif
    00594
    00595                     leftIter = NULL;  //switched back, error
    00596
    00597                     continue;
    00598                   }
    00599               }
    00600
    00601               nextLeftStepFound = true;
    00602             }
    00603
    00604             if (nextLeftStepFound)
    00605             {
    00606               //check gradient
    00607               delta = lastLeft.distanceToHorizon - ((GreenChangeEvent)greenChangeEvents[leftIter]).distanceToHorizon;
    00608
    00609               if (delta > RS_FOOT_GRADIENT)
    00610               { //up
    00611                 switch (leftGradient)
    00612                 {
    00613                 case up:
    00614                   {
    00615                     leftGradient = up;
    00616
    00617                     break;
    00618                   };
    00619                 case equal:
    00620                 case down:
    00621                   {
    00622                     footPoint = maxLeftPoint;
    00623
    00624 #ifdef RS_DEBUG_FEET
    00625                     CIRCLE (imageProcessor_robot_detection, footPoint.x, footPoint.y, 2, 1, Drawings::ps_solid, Drawings::pink);
    00626 #endif
    00627
    00628                     return true;
    00629                   };
    00630                 }
    00631               }
    00632               else
    00633               {
    00634                 if (delta < -RS_FOOT_GRADIENT)
    00635                 { //down
    00636                   leftGradient = down;
    00637                 }
    00638                 else
    00639                 {
    00640                   leftGradient = equal;
    00641                 }
    00642               }
    00643
    00644 #ifdef RS_DEBUG_FEET
    00645               LINE (imageProcessor_robot_detection, lastLeft.positionOnImage.x, lastLeft.positionOnImage.y, greenChangeEvents[leftIter].positionOnImage.x, greenChangeEvents[leftIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::pink);
    00646 #endif
    00647
    00648               lastLeft = greenChangeEvents[leftIter];
    00649
    00650               if (maxLeftDistance < lastLeft.distanceToHorizon)
    00651               {
    00652                 maxLeftDistance = lastLeft.distanceToHorizon;
    00653
    00654                 maxLeftPoint = lastLeft.positionOnImage;
    00655               };
    00656
    00657               nextLeftStepFound = false;
    00658
    00659               leftIter--;
    00660
    00661               break;
    00662             };
    00663
    00664             leftIter--;
    00665           };
    00666
    00667           //look right
    00668           //find next foot right
    00669           k = 0;
    00670           while (rightIter)
    00671           {
    00672             CRASH_CHECK(k); k++;
    00673             event = greenChangeEvents[rightIter];
    00674
    00675             if (lastRight.lineID + 1 < ((GreenChangeEvent)greenChangeEvents[rightIter]).lineID)
    00676             {
    00677               //new line id is 2 less than last ones, so go one back
    00678               rightIter--;
    00679
    00680               if (!((GreenChangeEvent)greenChangeEvents[rightIter]).startEvent ||
    00681                 (((GreenChangeEvent)greenChangeEvents[rightIter]).lineID == lastRight.lineID))
    00682               {
    00683                 //was one line too far, draw orange
    00684 #ifdef RS_DEBUG_FEET
    00685                 LINE (imageProcessor_robot_detection, lastRight.positionOnImage.x, lastRight.positionOnImage.y, greenChangeEvents[rightIter].positionOnImage.x, greenChangeEvents[rightIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::orange);
    00686 #endif
    00687
    00688                 rightIter = NULL;
    00689
    00690                 continue;
    00691               }
    00692               else
    00693                 nextRightStepFound = true;
    00694             };
    00695
    00696             if (event.lineID - orgLineID > RS_FOOT_THRESHOLD)
    00697             {
    00698               //search went to far away
    00699 #ifdef RS_DEBUG_FEET
    00700               LINE (imageProcessor_robot_detection, lastRight.positionOnImage.x, lastRight.positionOnImage.y, greenChangeEvents[rightIter].positionOnImage.x, greenChangeEvents[rightIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::white);
    00701 #endif
    00702
    00703               rightIter = NULL; //one line too far
    00704
    00705               continue;
    00706             };
    00707
    00708             if ((((GreenChangeEvent)greenChangeEvents[rightIter]).distanceToHorizon >= lastRight.distanceToHorizon) &&
    00709               (((GreenChangeEvent)greenChangeEvents[rightIter]).lineID) > lastRight.lineID)
    00710             {
    00711               //current event is closer to horizon than last one, and on a smaler scanline
    00712               if (!((GreenChangeEvent)greenChangeEvents[rightIter]).startEvent)
    00713               {
    00714                 //not a startevent, so go one step back
    00715                 rightIter--;
    00716
    00717                 tmpEvent = greenChangeEvents[rightIter];
    00718
    00719                 if (lastRight.lineID == ((GreenChangeEvent)greenChangeEvents[rightIter]).lineID)
    00720                 {
    00721                   //back on old scanline, so draw pink debug drawing
    00722 #ifdef RS_DEBUG_FEET
    00723                   LINE (imageProcessor_robot_detection, lastRight.positionOnImage.x, lastRight.positionOnImage.y, greenChangeEvents[rightIter].positionOnImage.x, greenChangeEvents[rightIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::red);
    00724 #endif
    00725
    00726                   rightIter = NULL; //switched back, error
    00727
    00728                   continue;
    00729                 }
    00730                 else
    00731                   if (!((GreenChangeEvent)greenChangeEvents[rightIter]).startEvent)
    00732                   {
    00733                     //start event expected, so draw black
    00734 #ifdef RS_DEBUG_FEET
    00735                     LINE (imageProcessor_robot_detection, lastRight.positionOnImage.x, lastRight.positionOnImage.y, greenChangeEvents[rightIter].positionOnImage.x, greenChangeEvents[rightIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::black);
    00736 #endif
    00737
    00738                     rightIter = NULL; //switched back, error
    00739
    00740                     continue;
    00741                   }
    00742               }
    00743
    00744               nextRightStepFound = true;
    00745             }
    00746
    00747             if (nextRightStepFound)
    00748             {
    00749               //check gradient
    00750               delta = lastRight.distanceToHorizon - ((GreenChangeEvent)greenChangeEvents[rightIter]).distanceToHorizon;
    00751
    00752               if (delta > RS_FOOT_GRADIENT)
    00753               { //up
    00754                 switch (rightGradient)
    00755                 {
    00756                 case up:
    00757                   {
    00758                     rightGradient = up;
    00759
    00760                     break;
    00761                   };
    00762                 case equal:
    00763                 case down:
    00764                   {
    00765                     footPoint = maxRightPoint;
    00766
    00767 #ifdef RS_DEBUG_FEET
    00768                     CIRCLE (imageProcessor_robot_detection, footPoint.x, footPoint.y, 2, 1, Drawings::ps_solid, Drawings::pink);
    00769 #endif
    00770
    00771                     return true;
    00772                   };
    00773                 }
    00774               }
    00775               else
    00776               {
    00777                 if (delta < -RS_FOOT_GRADIENT)
    00778                 { //down
    00779                   rightGradient = down;
    00780                 }
    00781                 else
    00782                 {
    00783                   rightGradient = equal;
    00784                 }
    00785               }
    00786
    00787 #ifdef RS_DEBUG_FEET
    00788               LINE (imageProcessor_robot_detection, lastRight.positionOnImage.x, lastRight.positionOnImage.y, greenChangeEvents[rightIter].positionOnImage.x, greenChangeEvents[rightIter].positionOnImage.y, 1, Drawings::ps_solid, Drawings::pink);
    00789 #endif
    00790
    00791               lastRight = greenChangeEvents[rightIter];
    00792
    00793               if (maxRightDistance < lastRight.distanceToHorizon)
    00794               {
    00795                 maxRightDistance = lastRight.distanceToHorizon;
    00796
    00797                 maxRightPoint = lastRight.positionOnImage;
    00798               };
    00799
    00800               nextRightStepFound = false;
    00801
    00802               break;
    00803             };
    00804
    00805             rightIter++;
    00806           };
    00807         }
    00808
    00809         footPoint.x = -1;
    00810         footPoint.y = -1;
    00811
    00812         return false;
    00813       }
    00814
    00815     footIterator++;
    00816   };
    00817
    00818   footIterator = greenChangeEvents.getFirst ();
    00819
    00820   footPoint.x = -1;
    00821   footPoint.y = -1;
    00822
    00823   return false;
    00824 };
    00825
    00826 void VLCRobotSpecialist::clusterCandidates (colorClass color)
    00827 {
    00828   //init
    00829   RobotCandidate *candidate = NULL, *lastCandidate = NULL;
    00830
    00831   int element = bottomIndex + 1;
    00832
    00833   CandidateCluster  robotCluster;
    00834
    00835   robotCluster.startPosition = -1;
    00836   robotCluster.stopPosition = -1;
    00837
    00838 #ifdef RS_DEBUG_CONSOLE
    00839   OUTPUT (idText, text, "--------");
    00840 #endif
    00841
    00842 #ifdef  RS_DEBUG_SHOW_GREENCHANGE
    00843   //print greenchange events
    00844   dbg_PrintGreenchanges ();
    00845 #endif
    00846
    00847   footIterator = greenChangeEvents.getFirst ();
    00848
    00849   if (element == topIndex)
    00850   {
    00851     //no element found, so return
    00852     return;
    00853   }
    00854
    00855   bool firstLoop = true;
    00856
    00857   int i = 0;
    00858   while (element < topIndex)
    00859   {
    00860     CRASH_CHECK(i); i++;
    00861
    00862     //get next candidate
    00863     candidate = &candidates[element];
    00864
    00865     //check color
    00866     if (candidate->color != color)
    00867     {
    00868       //wrong color, so continue
    00869       element++;
    00870
    00871       continue;
    00872     }
    00873
    00874     //do we find a foot point ?
    00875     if (!findNearestFootPoint (candidate->footPoint))
    00876     {
    00877       //no, so go on
    00878 //      candidate->color = noColor;
    00879
    00880       element++;
    00881
    00882 #ifdef RS_DEBUG
    00883       CIRCLE (imageProcessor_robot_detection, candidate->candidateStart.x, candidate->candidateStart.y, 3, 1, Drawings::ps_solid, Drawings::orange);
    00884 #endif
    00885
    00886       continue;
    00887     };
    00888
    00889     //are we in the first loop ?
    00890     if (firstLoop)
    00891     {
    00892       //yes, so init cluster
    00893       robotCluster.startPosition = element;
    00894       robotCluster.stopPosition = element;
    00895
    00896       element++;
    00897       lastCandidate = candidate;
    00898
    00899       //not in first loop anymore
    00900       firstLoop = false;
    00901
    00902       continue;
    00903     };
    00904
    00905     //calculate the distance on field between last and actual footpoint
    00906     int distance = distanceOnField (lastCandidate->footPoint, candidate->footPoint, cameraMatrix, CameraInfo::CameraInfo ());
    00907
    00908     //generate colorspace between them
    00909     ColorSpace colorSpace = generateColorspace (lastCandidate->candidateStart, candidate->candidateStart);
    00910
    00911     //normalize colorspace
    00912     colorSpace.Normalize ();
    00913
    00914     //check colorspace and distance
    00915     if (
    00916         ((colorSpace.colorCount[green] >= 0.1) && (candidate->lineID - lastCandidate->lineID >= 3))
    00917         || (distance > 400)
    00918         || ((candidate->color == red) && (colorSpace.colorCount[blue] >= 0.1))
    00919         || ((candidate->color == blue) && (colorSpace.colorCount[red] >= 0.1))
    00920       )
    00921     {
    00922       //new percept found, so reset everything
    00923 #ifdef RS_DEBUG
    00924
    00925 #ifdef RS_DEBUG_SHOW_CLUSTER_INFO
    00926       if (colorSpace.colorCount[green] >= 0.1)
    00927         OUTPUT (idText, text, "clustering : too much green found: " << colorSpace.colorCount[green] << "; line " << candidate->lineID);
    00928
    00929       if (distance > 400)
    00930         OUTPUT (idText, text, "clustering : too far away: " << distance << " ; line " << candidate->lineID);
    00931
    00932       if ((candidate->color == red) && (colorSpace.colorCount[blue] >= 0.1))
    00933         OUTPUT (idText, text, "clustering : wrong color found : blue; line " << candidate->lineID);
    00934
    00935       if ((candidate->color == blue) && (colorSpace.colorCount[red] >= 0.1))
    00936         OUTPUT (idText, text, "clustering : wrong color found : red; line " << candidate->lineID);
    00937 #endif
    00938
    00939 #endif
    00940       //generate percept for last cluster
    00941       if (lastCandidate->color == red || lastCandidate->color == blue)
    00942         generatePercept (robotCluster);
    00943
    00944       //start new cluster
    00945       robotCluster.startPosition = element;
    00946       robotCluster.stopPosition = element;
    00947     }
    00948     else
    00949     {
    00950       //increase cluster
    00951       robotCluster.stopPosition = element;
    00952
    00953 #ifdef  RS_DEBUG
    00954       //debug output, show connection between points
    00955       if (candidate->color == blue)
    00956       {
    00957 #ifdef RS_DEBUG_SHOW_CONNECTIONS
    00958         LINE (imageProcessor_robot_detection, lastCandidate->candidateStart.x, lastCandidate->candidateStart.y,
    00959           candidate->candidateStart.x, candidate->candidateStart.y, 2, Drawings::ps_dot, Drawings::blue);
    00960 #endif
    00961       }
    00962       else
    00963       {
    00964 #ifdef RS_DEBUG_SHOW_CONNECTIONS
    00965         LINE (imageProcessor_robot_detection, lastCandidate->candidateStart.x, lastCandidate->candidateStart.y,
    00966           candidate->candidateStart.x, candidate->candidateStart.y, 2, Drawings::ps_dot, Drawings::red);
    00967 #endif
    00968       };
    00969 #endif
    00970     }
    00971
    00972     //go on to next candidate
    00973     element++;
    00974     lastCandidate = candidate;
    00975   };
    00976
    00977   //there is still a cluster left
    00978   if (robotCluster.stopPosition != -1)
    00979     generatePercept (robotCluster);
    00980
    00981   return;
    00982 }
    00983
    00984 void VLCRobotSpecialist::generatePercept (CandidateCluster  cluster)
    00985 {
    00986   int       element;
    00987
    00988   Vector2<int>  average = Vector2<int>(0, 0);
    00989   Vector2<int>  filter  = Vector2<int>(0, 0);
    00990   VLCRectangle  body;
    00991   VLCRectangle  foot;
    00992
    00993   int       count = 0;
    00994
    00995   int       varianz = 0;
    00996
    00997   double      aspectRatio = 0;
    00998
    00999   RobotCandidate  candidate;
    01000   RobotCandidate  lastCandidate;
    01001
    01002   RobotDirection  robotDirection;
    01003
    01004   int       longestTrikotRun = 0;
    01005
    01006 //debugging variables
    01007 #ifdef RS_DEBUG_CONSOLE
    01008   int       dbg_i;
    01009 #endif
    01010
    01011   //init percept information
    01012   PerceptInfo   perceptInfo;
    01013
    01014   perceptInfo.minBounding = Vector2<int>(1024, 1024);
    01015   perceptInfo.maxBounding = Vector2<int>(0, 0);
    01016   perceptInfo.bottomLine  = Vector2<int>(0, 0);
    01017   perceptInfo.flags = 0;
    01018
    01019   //only one sample, so throw away
    01020 //  if (cluster.stopPosition - cluster.startPosition < 1)
    01021 //    return;
    01022   perceptInfo.lines = cluster.stopPosition - cluster.startPosition + 1;
    01023
    01024   //generate boundings
    01025   count = generateBoundings (cluster, body, foot, filter);
    01026
    01027   //do
    01028   if (count > 0)
    01029   {
    01030     filter.x /= count;
    01031     filter.y /= count;
    01032
    01033     varianz = foot.y2 - foot.y1;
    01034
    01035     int threshold = filter.y - (int) (varianz * 0.1);
    01036
    01037     count = 0;
    01038
    01039     lastCandidate.lineID = -1;
    01040
    01041     //calculate bottom line
    01042     int i = 0;
    01043     for (element = cluster.startPosition; element <= cluster.stopPosition; element++)
    01044     {
    01045       CRASH_CHECK(i); i++;
    01046
    01047       candidate = candidates[element];
    01048
    01049       if (longestTrikotRun < (candidate.candidateStart.y - candidate.candidateStop.y))
    01050         longestTrikotRun = candidate.candidateStart.y - candidate.candidateStop.y;
    01051
    01052       if (candidate.footPoint.y >= threshold)
    01053       {
    01054         average.x += candidate.footPoint.x;
    01055         average.y += candidate.footPoint.y;
    01056
    01057         if (lastCandidate.lineID == -1)
    01058           lastCandidate = candidate;
    01059
    01060         perceptInfo.bottomLine.x += candidate.footPoint.x - lastCandidate.footPoint.x;
    01061         perceptInfo.bottomLine.y += candidate.footPoint.y - lastCandidate.footPoint.y;
    01062
    01063         count++;
    01064         lastCandidate = candidate;
    01065
    01066 #ifdef  RS_DEBUG
    01067         CIRCLE (imageProcessor_robot_detection, candidate.footPoint.x, candidate.footPoint.y, 1, 1, Drawings::ps_solid, Drawings::black);
    01068 #endif
    01069       }
    01070       else
    01071       {
    01072 #ifdef  RS_DEBUG
    01073         CIRCLE (imageProcessor_robot_detection, candidate.footPoint.x, candidate.footPoint.y, 1, 1, Drawings::ps_solid, Drawings::dark_gray);
    01074 #endif
    01075       }
    01076     }
    01077
    01078     if (count > 0)
    01079     {
    01080       //calculation average from footpoints
    01081       average.x /= count;
    01082       average.y /= count;
    01083
    01084       //calclulate bounding box
    01085       {
    01086         perceptInfo.minBounding.x = min (body.x1, body.x1);
    01087         perceptInfo.minBounding.y = min (body.y1, body.y1);
    01088         perceptInfo.maxBounding.x = max (body.x2, body.x2);
    01089         perceptInfo.maxBounding.y = max (body.y2, average.y);
    01090       }
    01091
    01092       unsigned char y, u, v, ly = 0;
    01093       //grow bounding box
    01094 #ifdef RS_GROW_BB
    01095       {
    01096         int xcoord, ycoord;
    01097
    01098         xcoord = minBounding.x;
    01099         ycoord = (int)((double)((maxBounding.y - minBounding.y) * 0.8) + minBounding.y);
    01100
    01101         int k = 0;
    01102         while (xcoord > 0)
    01103         {
    01104           CRASH_CHECK(k); k++;
    01105           // <copied from imp>
    01106           if (xcoord < 0 || xcoord >= image.cameraInfo.resolutionWidth || ycoord < 0 || ycoord >= image.cameraInfo.resolutionHeight)
    01107             y = u = v = 0;
    01108           else
    01109           {
    01110             y = colorCorrector.correct(xcoord, ycoord, 0, image.image[ycoord][0][xcoord]);
    01111             u = colorCorrector.correct(xcoord, ycoord, 1, image.image[ycoord][1][xcoord]);
    01112             v = colorCorrector.correct(xcoord, ycoord, 2, image.image[ycoord][2][xcoord]);
    01113           }
    01114           colorClass color = COLOR_CLASS(y, u, v);
    01115           //<copied end>
    01116
    01117           if (color == green)
    01118             break;
    01119
    01120 #ifdef RS_DEBUG
    01121           DOT (imageProcessor_robot_detection, xcoord, ycoord, ColorClasses::colorClassToDrawingsColor (color), ColorClasses::colorClassToDrawingsColor (color));
    01122 #endif
    01123
    01124           xcoord--;
    01125         };
    01126
    01127         if (xcoord != 0)
    01128           minBounding.x = xcoord + 1;
    01129
    01130         xcoord = maxBounding.x;
    01131
    01132         k = 0;
    01133         while (xcoord < image.cameraInfo.resolutionWidth)
    01134         {
    01135           CRASH_CHECK(k); k++;
    01136           // <copied from imp>
    01137           if (xcoord < 0 || xcoord >= image.cameraInfo.resolutionWidth || ycoord < 0 || ycoord >= image.cameraInfo.resolutionHeight)
    01138             y = u = v = 0;
    01139           else
    01140           {
    01141             y = colorCorrector.correct(xcoord, ycoord, 0, image.image[ycoord][0][xcoord]);
    01142             u = colorCorrector.correct(xcoord, ycoord, 1, image.image[ycoord][1][xcoord]);
    01143             v = colorCorrector.correct(xcoord, ycoord, 2, image.image[ycoord][2][xcoord]);
    01144           }
    01145           colorClass color = COLOR_CLASS(y, u, v);
    01146           //<copied end>
    01147
    01148           if (color == green)
    01149             break;
    01150
    01151 #ifdef RS_DEBUG
    01152           DOT (imageProcessor_robot_detection, xcoord, ycoord, ColorClasses::colorClassToDrawingsColor (color), ColorClasses::colorClassToDrawingsColor (color));
    01153 #endif
    01154
    01155           xcoord++;
    01156         };
    01157
    01158         if (xcoord != image.cameraInfo.resolutionWidth)
    01159           maxBounding.x = xcoord - 1;
    01160       }
    01161 #endif
    01162       //test for size
    01163       {
    01164         Vector2<int>  fieldPosition;
    01165         Vector2<int>  vec2;
    01166
    01167         Geometry::calculatePointOnField(perceptInfo.minBounding.x, perceptInfo.maxBounding.y, cameraMatrix, CameraInfo::CameraInfo (), fieldPosition);
    01168         Geometry::calculatePointOnField(perceptInfo.maxBounding.x, perceptInfo.maxBounding.y, cameraMatrix, CameraInfo::CameraInfo (), vec2);
    01169
    01170         vec2.x -= fieldPosition.x;
    01171         vec2.y -= fieldPosition.y;
    01172
    01173         if ((vec2.abs() < 100) || (vec2.abs() > 500))
    01174           perceptInfo.flags |= MSHPPF::wrongSize;
    01175       }
    01176
    01177       //generate colorspace
    01178 #define NUMSUBSPACES  3
    01179       ColorSpace    colorSpace;
    01180       ColorSpace    subSpaces[NUMSUBSPACES];
    01181       int       subSpace;
    01182       {
    01183         int xcoord, ycoord;
    01184
    01185         int k = 0;
    01186         for (xcoord = perceptInfo.minBounding.x; xcoord < perceptInfo.maxBounding.x; xcoord += max ((perceptInfo.maxBounding.x - perceptInfo.minBounding.x) / 9, 1))
    01187         {
    01188           CRASH_CHECK(k); k++;
    01189           subSpace = (int)((xcoord - perceptInfo.minBounding.x) / (double)((perceptInfo.maxBounding.x - perceptInfo.minBounding.x) / (double)NUMSUBSPACES));
    01190
    01191           int l = 0;
    01192           for (ycoord = perceptInfo.minBounding.y; ycoord < perceptInfo.maxBounding.y; ycoord += max ((perceptInfo.maxBounding.y - perceptInfo.minBounding.y) / 10, 1))
    01193           {
    01194             CRASH_CHECK(l); l++;
    01195             // <copied from imp>
    01196             if (xcoord < 0 || xcoord >= image.cameraInfo.resolutionWidth || ycoord < 0 || ycoord >= image.cameraInfo.resolutionHeight)
    01197               y = u = v = 0;
    01198             else
    01199             {
    01200               y = colorCorrector.correct(xcoord, ycoord, 0, image.image[ycoord][0][xcoord]);
    01201               u = colorCorrector.correct(xcoord, ycoord, 1, image.image[ycoord][1][xcoord]);
    01202               v = colorCorrector.correct(xcoord, ycoord, 2, image.image[ycoord][2][xcoord]);
    01203             }
    01204             colorClass color = COLOR_CLASS(y, u, v);
    01205             //<copied end>
    01206
    01207             if (ycoord == perceptInfo.minBounding.y)
    01208               ly = y;
    01209
    01210             subSpaces[subSpace].AddSample (color, y, y - ly);
    01211           };
    01212         };
    01213
    01214         for (subSpace = 0; subSpace < NUMSUBSPACES; subSpace++)
    01215         {
    01216           CRASH_CHECK(subSpace);
    01217
    01218           colorSpace += subSpaces[subSpace];
    01219         };
    01220       }
    01221
    01222       //normalize
    01223       {
    01224         for (subSpace = 0; subSpace < NUMSUBSPACES; subSpace++)
    01225         {
    01226           CRASH_CHECK(subSpace);
    01227
    01228           subSpaces[subSpace].Normalize ();
    01229
    01230 #ifdef RS_DEBUG
    01231 //        RECTANGLE (imageProcessor_robot_detection, minBounding.x
    01232 #endif
    01233 #ifdef RS_DEBUG_CONSOLE
    01234           OUTPUT (idText, text, "y subspace " << subSpace << " : " << subSpaces[subSpace].y);
    01235 #endif
    01236         };
    01237
    01238         colorSpace.Normalize ();
    01239       }
    01240
    01241       //
    01242       //show statistics on console
    01243       {
    01244 #ifdef  RS_DEBUG_CONSOLE
    01245         for (dbg_i = 0; dbg_i < numOfColors; dbg_i++)
    01246         {
    01247           CRASH_CHECK(dbg_i);
    01248
    01249           OUTPUT (idText, text, ColorClasses::getColorName ((colorClass) dbg_i) << " : " << colorSpace.colorCount[dbg_i]);
    01250         };
    01251
    01252         OUTPUT (idText, text, "brightness : " << colorSpace.y);
    01253         OUTPUT (idText, text, "delta brightness : " << colorSpace.dY);
    01254 #endif
    01255       }
    01256
    01257       //
    01258       //do some filtering
    01259       {
    01260         unsigned int filterFlags = filterPercepts (colorSpace, candidate.color);
    01261
    01262         if (filterFlags)
    01263           perceptInfo.flags |= filterFlags;
    01264       }
    01265
    01266
    01267       //
    01268       //calculate aspect ratio of bounding box
    01269       {
    01270         if (perceptInfo.maxBounding.y != perceptInfo.minBounding.y)
    01271           aspectRatio = (perceptInfo.maxBounding.x - perceptInfo.minBounding.x) / (perceptInfo.maxBounding.y - perceptInfo.minBounding.y);
    01272         else
    01273           aspectRatio = 0;
    01274
    01275         if (aspectRatio < 0)
    01276         {
    01277           //higher than wide, so robot seems to be seen from front or back
    01278           robotDirection.AddSample (0);
    01279           robotDirection.AddSample (pi);
    01280         }
    01281         else
    01282         {
    01283           //wider than high, so robot seems to be seen from side
    01284           robotDirection.AddSample (pi_2);
    01285           robotDirection.AddSample (-pi_2);
    01286         }
    01287       }
    01288       //
    01289       //calculate direction from color dispersion
    01290       {
    01291         if (colorSpace.colorCount[red] > 0.15)
    01292         {
    01293           //much red, so robot seems to be seen from back
    01294           robotDirection.AddSample (0, 10);
    01295
    01296 #ifdef  RS_DEBUG_CONSOLE
    01297           OUTPUT (idText, text, "robot seen from back");
    01298 #endif
    01299         }
    01300       }
    01301
    01302       //
    01303       //calculate direction from brightness
    01304       {
    01305         if ((subSpaces[0].y < subSpaces[1].y) && (subSpaces[0].y < subSpaces[2].y))
    01306         { //robot seen from front / right
    01307           robotDirection.AddSample (pi + pi_4, 2);
    01308         }
    01309
    01310         if ((subSpaces[1].y < subSpaces[0].y) && (subSpaces[1].y < subSpaces[2].y))
    01311         { //robot seen from front or back
    01312           robotDirection.AddSample (pi, 2);
    01313           robotDirection.AddSample (0, 1);
    01314
    01315           if (colorSpace.y < 75)
    01316           {
    01317             //quite dark, so robot seems to be seen from front
    01318             robotDirection.AddSample (pi, 2);
    01319
    01320 #ifdef  RS_DEBUG_CONSOLE
    01321             OUTPUT (idText, text, "robot seen from front");
    01322 #endif
    01323           };
    01324         }
    01325
    01326         if ((subSpaces[2].y < subSpaces[0].y) && (subSpaces[2].y < subSpaces[1].y))
    01327         { //robot seen from front / left
    01328           robotDirection.AddSample (pi - pi_4, 2);
    01329         }
    01330       }
    01331
    01332       //generate percept
    01333       {
    01334         perceptInfo.position  = average;
    01335         perceptInfo.direction = robotDirection.getDirection ();
    01336         perceptInfo.validity  = robotDirection.getReliability ();
    01337         perceptInfo.color   = candidate.color;
    01338         perceptInfo.count   = count;
    01339
    01340         addPercept (perceptInfo);
    01341       }
    01342     }
    01343     else
    01344       if (longestTrikotRun > 25)// && (candidate.candidateStop != Vector2<int>(0, 0)))
    01345       { //robot is very near
    01346         perceptInfo.position  = Vector2<int> (candidate.candidateStop.x, image.cameraInfo.resolutionHeight);
    01347         perceptInfo.direction = 0;
    01348         perceptInfo.validity  = 0.75;
    01349         perceptInfo.color   = candidate.color;
    01350         perceptInfo.count   = 0;
    01351
    01352         addPercept (perceptInfo);
    01353       }
    01354   };
    01355 }
    01356
    01357 int VLCRobotSpecialist::noColorPixelsBetween (Vector2<int> startPoint, Vector2<int> endPoint, colorClass noColor)
    01358 {
    01359   // init Bresenham
    01360   BresenhamLineScan scanLine  = BresenhamLineScan (startPoint, endPoint);
    01361   Vector2<int>    actual    = startPoint;
    01362
    01363   unsigned char y, u, v;
    01364
    01365   int result = 0;
    01366
    01367   for (int i = 0; i <= scanLine.numberOfPixels; ++i)
    01368   {
    01369     CRASH_CHECK(i);
    01370     // <copied from imp>
    01371     if (actual.x < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y < 0 || actual.y >= image.cameraInfo.resolutionHeight)
    01372       y = u = v = 0;
    01373     else
    01374     {
    01375       y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    01376       u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    01377       v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    01378     }
    01379       colorClass color = COLOR_CLASS(y, u, v);
    01380     //<copied end>
    01381
    01382     if (color == noColor)
    01383       result++;
    01384
    01385     // Bresenham
    01386     scanLine.getNext (actual);
    01387   };
    01388
    01389   return result;
    01390 }
    01391
    01392 ColorSpace VLCRobotSpecialist::generateColorspace (Vector2<int> startPoint, Vector2<int> endPoint)
    01393 {
    01394   ColorSpace colorSpace;
    01395
    01396   // init Bresenham
    01397   BresenhamLineScan scanLine  = BresenhamLineScan (startPoint, endPoint);
    01398   Vector2<int>    actual    = startPoint;
    01399
    01400   unsigned char y, u, v, ly = 0;
    01401
    01402   int i;
    01403
    01404   for (i = 0; i < numOfColors; i++)
    01405   {
    01406     CRASH_CHECK(i);
    01407
    01408     colorSpace.colorCount[i] = 0;
    01409   }
    01410
    01411   for (i = 0; i <= scanLine.numberOfPixels; ++i)
    01412   {
    01413     CRASH_CHECK(i);
    01414
    01415     // <copied from imp>
    01416     if (actual.x < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y < 0 || actual.y >= image.cameraInfo.resolutionHeight)
    01417       y = u = v = 0;
    01418     else
    01419     {
    01420       y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    01421       u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    01422       v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    01423     }
    01424       colorClass color = COLOR_CLASS(y, u, v);
    01425     //<copied end>
    01426
    01427     if (i == 0)
    01428       ly = y;
    01429
    01430     colorSpace.colorCount[color]++;
    01431     colorSpace.y += y;
    01432     colorSpace.dY += y - ly;
    01433
    01434 #ifdef RS_DEBUG_SHOW_COLORSPACE_INFO
    01435     LINE(imageProcessor_scanLines, actual.x, actual.y, actual.x, actual.y, 1, Drawings::ps_solid, ColorClasses::colorClassToDrawingsColor (color));
    01436 #endif
    01437
    01438     // Bresenham
    01439     scanLine.getNext (actual);
    01440   };
    01441
    01442   return colorSpace;
    01443 }
    01444
    01445 unsigned int VLCRobotSpecialist::filterPercepts (ColorSpace colorSpace, colorClass candidateColor)
    01446 {
    01447   unsigned int result = 0;
    01448
    01449   if ((colorSpace.colorCount[red] < 0.01) && (colorSpace.colorCount[blue] < 0.01))
    01450   { //too few trickot found, so asume as noise
    01451 #ifdef  RS_DEBUG_CONSOLE
    01452     OUTPUT (idText, text, "too few trickot found, so asume as noise");
    01453     OUTPUT (idText, text, "");
    01454 #endif
    01455
    01456     result |= MSHPPF::fewTrickot;
    01457   }
    01458
    01459   if ((candidateColor == red) && (colorSpace.colorCount[white] > 0.5))
    01460   { //too much white, so asume something was seen on the wall
    01461 #ifdef  RS_DEBUG_CONSOLE
    01462     OUTPUT (idText, text, "too much white, so asume something was seen on the wall");
    01463     OUTPUT (idText, text, "");
    01464 #endif
    01465
    01466     result |= MSHPPF::tooMuchWhite;
    01467   }
    01468
    01469   if (colorSpace.colorCount[green] > 0.5)
    01470   { //too much green, so asume a ball was seen on the field
    01471 #ifdef  RS_DEBUG_CONSOLE
    01472     OUTPUT (idText, text, "too much green, so asume a ball was seen on the field");
    01473     OUTPUT (idText, text, "");
    01474 #endif
    01475
    01476     result |= MSHPPF::tooMuchGreen;
    01477   }
    01478
    01479   if (colorSpace.colorCount[skyblue] > 0.1)
    01480   { //too much skyblue, so it seems to be a goal, or landmark
    01481 #ifdef  RS_DEBUG_CONSOLE
    01482     OUTPUT (idText, text, "too much skyblue, so it seems to be a goal, or landmark");
    01483     OUTPUT (idText, text, "");
    01484 #endif
    01485
    01486     result |= MSHPPF::tooMuchSkyBlue;
    01487   }
    01488
    01489   if (colorSpace.colorCount[orange] > 0.2)
    01490   { //too much orange, so it seems to be a ball
    01491 #ifdef  RS_DEBUG_CONSOLE
    01492     OUTPUT (idText, text, "too much orange, so it seems to be a ball");
    01493     OUTPUT (idText, text, "");
    01494 #endif
    01495
    01496     result |= MSHPPF::tooMuchOrange;
    01497   }
    01498
    01499   if (colorSpace.dY < 35)
    01500   { //too few brightness changes
    01501 #ifdef  RS_DEBUG_CONSOLE
    01502     OUTPUT (idText, text, "too few brightness changes");
    01503     OUTPUT (idText, text, "");
    01504 #endif
    01505
    01506     result |= MSHPPF::fewBrightnessChange;
    01507   }
    01508
    01509   return result;
    01510 }
    01511
    01512 void VLCRobotSpecialist::addPercept (PerceptInfo &perceptInfo)
    01513 {
    01514   Vector2<int>  fieldPosition;
    01515
    01516   MSHSinglePlayerPercept percept;
    01517
    01518   percept.flags = perceptInfo.flags;
    01519   percept.lines = perceptInfo.lines;
    01520
    01521   //calculate positions from footpoints relative to own robot
    01522   Geometry::calculatePointOnField(perceptInfo.position.x, perceptInfo.position.y, cameraMatrix, CameraInfo::CameraInfo (), fieldPosition);
    01523
    01524   /* translate from robot to playing field space*/
    01525   Pose2D pose = Pose2D (fieldPosition);
    01526
    01527   Pose2D poseOnField = robotPose + pose;
    01528
    01529   percept.offset = poseOnField.translation;
    01530
    01531   /* filtering should be done in playerslocator */
    01532   if (
    01533     (percept.offset.x > xPosOpponentGoal) ||
    01534     (percept.offset.x < -xPosOpponentGoal) ||
    01535     (percept.offset.y > yPosLeftSideline) ||
    01536     (percept.offset.y < -yPosLeftSideline)
    01537     )
    01538   {
    01539     //percept is outside
    01540     percept.flags |= MSHPPF::outOfField;
    01541
    01542 #ifdef  RS_DEBUG_CONSOLE
    01543     OUTPUT (idText, text, "");
    01544     OUTPUT (idText, text, "percept outside field");
    01545     OUTPUT (idText, text, "");
    01546 #endif
    01547   }
    01548
    01549   percept.direction = robotPose.getAngle () + perceptInfo.direction;
    01550   percept.validity = perceptInfo.validity;
    01551
    01552
    01553 #ifdef  RS_DEBUG_CONSOLE
    01554   OUTPUT (idText, text, "robot at position : " << fieldPosition.x << " , " << fieldPosition.y);
    01555   OUTPUT (idText, text, "distance : " << fieldPosition.abs ());
    01556   OUTPUT (idText, text, "");
    01557 #endif
    01558
    01559 #ifdef  RS_DEBUG
    01560   ARROW (percepts_robot, percept.offset.x, percept.offset.y, percept.offset.x + cos (percept.direction) * 200, percept.offset.y - sin (percept.direction) * 200, 1, Drawings::ps_solid, Drawings::black);
    01561 #endif
    01562
    01563   if (perceptInfo.color == blue)
    01564   {
    01565     playersPercept.addBluePlayer (percept);
    01566
    01567 #ifdef  RS_DEBUG
    01568
    01569
    01570 #ifdef  RS_DEBUG_SHOW_BOUNDINGBOX
    01571     RECTANGLE (imageProcessor_robot_detection, perceptInfo.minBounding.x, perceptInfo.minBounding.y, perceptInfo.maxBounding.x, perceptInfo.maxBounding.y, 1, Drawings::ps_dot, Drawings::blue);
    01572 #endif
    01573
    01574     perceptInfo.gradient = 0;
    01575
    01576     if (perceptInfo.bottomLine.x != 0)
    01577       perceptInfo.gradient = (((double) perceptInfo.bottomLine.y) / ((double) perceptInfo.bottomLine.x)) / ((double)perceptInfo.count) * 2.0;
    01578
    01579     Vector2<int>  start;
    01580     Vector2<int>  stop;
    01581
    01582     start.x = perceptInfo.minBounding.x;
    01583     start.y = perceptInfo.position.y + ((int)((perceptInfo.minBounding.x - perceptInfo.position.x) * perceptInfo.gradient));
    01584
    01585     stop.x = perceptInfo.maxBounding.x;
    01586     stop.y = perceptInfo.position.y + ((int)((perceptInfo.maxBounding.x - perceptInfo.position.x) * perceptInfo.gradient));
    01587
    01588 #ifdef RS_DEBUG_SHOW_PERCEPT
    01589     LINE (imageProcessor_robot_detection, start.x, start.y, stop.x, stop.y, 1, Drawings::ps_dot, Drawings::blue);
    01590
    01591     CIRCLE (imageProcessor_robot_detection, perceptInfo.position.x, perceptInfo.position.y, 0, 1, Drawings::ps_solid, Drawings::blue);
    01592     CIRCLE (percepts_robot, percept.offset.x, percept.offset.y, 50, 10, Drawings::ps_solid, Drawings::blue);
    01593 #endif
    01594 #endif
    01595   }
    01596   else
    01597   {
    01598     playersPercept.addRedPlayer (percept);
    01599
    01600 #ifdef  RS_DEBUG
    01601 #ifdef  RS_DEBUG_SHOW_BOUNDINGBOX
    01602     RECTANGLE (imageProcessor_robot_detection, perceptInfo.minBounding.x, perceptInfo.minBounding.y, perceptInfo.maxBounding.x, perceptInfo.maxBounding.y, 1, Drawings::ps_dot, Drawings::red);
    01603 #endif
    01604
    01605     perceptInfo.gradient = 0;
    01606
    01607     if (perceptInfo.bottomLine.x != 0)
    01608       perceptInfo.gradient = (((double) perceptInfo.bottomLine.y) / ((double) perceptInfo.bottomLine.x)) / ((double)perceptInfo.count) * 2.0;
    01609
    01610     Vector2<int>  start;
    01611     Vector2<int>  stop;
    01612
    01613     start.x = perceptInfo.minBounding.x;
    01614     start.y = perceptInfo.position.y + ((int)((perceptInfo.minBounding.x - perceptInfo.position.x) * perceptInfo.gradient));
    01615
    01616     stop.x = perceptInfo.maxBounding.x;
    01617     stop.y = perceptInfo.position.y + ((int)((perceptInfo.maxBounding.x - perceptInfo.position.x) * perceptInfo.gradient));
    01618
    01619 #ifdef RS_DEBUG_SHOW_PERCEPT
    01620     LINE (imageProcessor_robot_detection, start.x, start.y, stop.x, stop.y, 1, Drawings::ps_dot, Drawings::red);
    01621
    01622     CIRCLE (imageProcessor_robot_detection, perceptInfo.position.x, perceptInfo.position.y, 0, 1, Drawings::ps_solid, Drawings::orange);
    01623     CIRCLE (percepts_robot, percept.offset.x, percept.offset.y, 50, 10, Drawings::ps_solid, Drawings::red);
    01624 #endif
    01625 #endif
    01626   };
    01627
    01628   return;
    01629 };
    01630
    01631
    01632 void VLCRobotSpecialist::dbg_PrintGreenchanges (void)
    01633 {
    01634   List<GreenChangeEvent>::Pos iter = greenChangeEvents.getFirst ();
    01635
    01636   int i = 0;
    01637   while (iter)
    01638   {
    01639     CRASH_CHECK(i); i++;
    01640     GreenChangeEvent event = greenChangeEvents[iter];
    01641
    01642     if (event.startEvent)
    01643     {
    01644       CIRCLE (imageProcessor_robot_detection, event.positionOnImage.x, event.positionOnImage.y, 2, 1, Drawings::ps_dot, Drawings::gray);
    01645     }
    01646     else
    01647     {
    01648       CIRCLE (imageProcessor_robot_detection, event.positionOnImage.x, event.positionOnImage.y, 2, 1, Drawings::ps_dot, Drawings::dark_gray);
    01649     }
    01650
    01651     iter++;
    01652   };
    01653 };
    01654
    01655 int VLCRobotSpecialist::distanceOnField (Vector2<int> vec1, Vector2<int> vec2, CameraMatrix cameraMatrix, CameraInfo cameraInfo)
    01656 {
    01657   Vector2<int>  pointOnField1, pointOnField2;
    01658
    01659   Geometry::calculatePointOnField (vec1.x, vec1.y, cameraMatrix, cameraInfo, pointOnField1);
    01660
    01661   Geometry::calculatePointOnField (vec2.x, vec2.y, cameraMatrix, cameraInfo, pointOnField2);
    01662
    01663   pointOnField2 -= pointOnField1;
    01664
    01665   return pointOnField2.abs ();
    01666 }
    01667
    01668 int VLCRobotSpecialist::generateBoundings (CandidateCluster cluster, VLCRectangle &body, VLCRectangle &foot, Vector2<int> &footAVG)
    01669 {
    01670   RobotCandidate  candidate;
    01671
    01672   int       element = 0;
    01673   int       count = 0;
    01674
    01675   //init values
    01676   body.x1 = 1024; body.x2 = 0; body.y1 = 1024; body.y2 = 0;
    01677   foot.x1 = 1024; foot.x2 = 0; foot.y1 = 1024; foot.y2 = 0;
    01678
    01679   //do the stuff
    01680   for (element = (int) cluster.startPosition; element <= (int)cluster.stopPosition; element++)
    01681   {
    01682     CRASH_CHECK(element);
    01683
    01684     candidate = candidates[element];
    01685
    01686     if (body.x2 < candidate.candidateStart.x)
    01687       body.x2 = candidate.candidateStart.x;
    01688
    01689     if (body.x1 > candidate.candidateStart.x)
    01690       body.x1 = candidate.candidateStart.x;
    01691
    01692     if (body.y2 < candidate.candidateStart.y)
    01693       body.y2 = candidate.candidateStart.y;
    01694
    01695     if (body.y1 > candidate.candidateStart.y)
    01696       body.y1 = candidate.candidateStart.y;
    01697
    01698     if ((candidate.footPoint.x != -1) &&
    01699       (candidate.footPoint.y != -1)
    01700       )
    01701     {
    01702       if (foot.x2 < candidate.footPoint.x)
    01703         foot.x2 = candidate.footPoint.x;
    01704
    01705       if (foot.x1 > candidate.footPoint.x)
    01706         foot.x1 = candidate.footPoint.x;
    01707
    01708       if (foot.y2 < candidate.footPoint.y)
    01709         foot.y2 = candidate.footPoint.y;
    01710
    01711       if (foot.y1 > candidate.footPoint.y)
    01712         foot.y1 = candidate.footPoint.y;
    01713
    01714       footAVG.x += candidate.footPoint.x;
    01715       footAVG.y += candidate.footPoint.y;
    01716     };
    01717
    01718     count++;
    01719   };
    01720
    01721   return count;
    01722 }
    01723
    01724 #undef min
    01725 #undef max