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

# Modules/ImageProcessor/VLCImageProcessor/VLCFlagSpecialist.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file VLCFlagSpecialist.cpp
    00003 *
    00004 * This file contains a class for Image Processing.
    00005 * @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Juengel</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 "VLCFlagSpecialist.h"
    00013 #include "VLCImageProcessorTools.h"
    00014
    00015
    00016 VLCFlagSpecialist::VLCFlagSpecialist(const ColorCorrector& colorCorrector) :
    00017   colorCorrector(colorCorrector)
    00018 {
    00019 }
    00020
    00021 void VLCFlagSpecialist::init(const Image& image)
    00022 {
    00023   for(int i = 0; i < 6; i++)
    00024   {
    00025     numberOfBoundingBoxes[i] = 0;
    00026   }
    00027   INIT_DEBUG_IMAGE(imageProcessorFlags, image);
    00028 }
    00029
    00030 void VLCFlagSpecialist::searchFlags
    00031 (
    00032  const Image& image,
    00033  const ColorTable& colorTable,
    00034  const CameraMatrix& cameraMatrix,
    00035  colorClass color,
    00036  bool pinkIsTop,
    00037  const Geometry::Line horizonLine,
    00038  int x, int y
    00039  )
    00040 {
    00041   CameraInfo bwCameraInfo = image.cameraInfo;
    00042   bwCameraInfo.resolutionHeight*=2;
    00043   bwCameraInfo.resolutionWidth*=2;
    00044   bwCameraInfo.focalLength*=2;
    00045   bwCameraInfo.focalLengthInv/=2;
    00046   bwCameraInfo.opticalCenter.x*=2;
    00047   bwCameraInfo.opticalCenter.y*=2;
    00048   bool gridPointIsCovered = false;
    00049
    00050   // x and y are in old resolution, so no division
    00051   DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorFlags, x, y)
    00052
    00053   int i, j;
    00054
    00055   for(i = 0; i < 6; i++)
    00056   {
    00057     for(j = 0; j < numberOfBoundingBoxes[i]; j++)
    00058     {
    00059       if(x >= boundingBoxLeft[j][i] && x <= boundingBoxRight[j][i] &&
    00060         y >= boundingBoxBottom[j][i] && y <= boundingBoxTop[j][i] )
    00061       {
    00062         gridPointIsCovered = true;
    00063       }
    00064     }// j
    00065   }//i
    00066
    00067   if(gridPointIsCovered) return;
    00068
    00069   int pixelCounter = 0;
    00070   int topPixelCounter = 0;
    00071   int bottomPixelCounter = 0;
    00072
    00073   start.x = 2*x;
    00074   start.y = 2*y;
    00075
    00076   //find up
    00077   findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction,
    00078     up, north, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
    00079   //find down
    00080   findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction,
    00081     down, south, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
    00082   DOT(imageProcessor_ball4, south.x/2, south.y/2, Drawings::black, Drawings::green);
    00083   //find right
    00084   findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction,
    00085     right, east, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
    00086   //find left
    00087   findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction,
    00088     left, west, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
    00089
    00090   DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorFlags, north.x/2, north.y/2);
    00091   DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorFlags, east.x/2, east.y/2);
    00092   DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorFlags, south.x/2, south.y/2);
    00093   DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorFlags, west.x/2, west.y/2);
    00094
    00095   if(west == east)
    00096   {
    00097     Vector2<int> newStart((north.x + south.x)/2, (north.y + south.y)/2);
    00098     //find right
    00099     findEndOfFlag(image, bwCameraInfo, colorTable, newStart, horizonLine.direction,
    00100       right, east, color, pinkIsTop, false, horizontal, pixelCounter, topPixelCounter, bottomPixelCounter);
    00101     //find left
    00102     findEndOfFlag(image, bwCameraInfo, colorTable, newStart, horizonLine.direction,
    00103       left, west, color, pinkIsTop, false, horizontal, pixelCounter, topPixelCounter, bottomPixelCounter);
    00104   }
    00105   //go to center of pink
    00106   start.x = (west.x + east.x) / 2;
    00107   start.y = (west.y + east.y) / 2;
    00108   DOT(imageProcessor_ball4, start.x/2, start.y/2, Drawings::black, Drawings::red);
    00109   DOT(imageProcessor_ball4, east.x/2, east.y/2, Drawings::black, Drawings::yellow);
    00110   DOT(imageProcessor_ball4, west.x/2, west.y/2, Drawings::black, Drawings::yellow);
    00111
    00112   DEBUG_IMAGE_SET_PIXEL_YELLOW(imageProcessorFlags, start.x/2, start.y/2);
    00113
    00114   //find north
    00115   findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction,
    00116     up, north, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
    00117   //find south
    00118   findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction,
    00119     down, south, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
    00120
    00121   double distance = sqrt((double)sqr(south.x-north.x) + sqr(south.y-north.y));
    00122
    00123   // reset counters
    00124   pixelCounter = 0;
    00125   topPixelCounter = 0;
    00126   bottomPixelCounter = 0;
    00127
    00128   double rightDist = 0.0;
    00129   double leftDist = 0.0;
    00130   int rightValidCounter = 0;
    00131   int leftValidCounter = 0;
    00132   int scans = 0;
    00133
    00134   Vector2<int> temp;
    00135
    00136   Vector2<int> lastStart;
    00137   lastStart.x = 1000;
    00138   lastStart.y = 1000;
    00139
    00140   //scan horizontal lines
    00141   Vector2<double>step;
    00142   step.x = (south.x - north.x) / (numberOfHorizontalScans + 1.0);
    00143   step.y = (south.y - north.y) / (numberOfHorizontalScans + 1.0);
    00144
    00145   Geometry::Line leftVerticalLine;
    00146   leftVerticalLine.base = horizonLine.base;
    00147   leftVerticalLine.direction.x = -horizonLine.direction.y;
    00148   leftVerticalLine.direction.y = horizonLine.direction.x;
    00149
    00150   bool pointIsGood;
    00151   bool startIsInTop = true;
    00152   for(i = 1; i <= numberOfHorizontalScans; i++)
    00153   {
    00154     if(i==(numberOfHorizontalScans + 1)/2)
    00155     {
    00156       startIsInTop = false;
    00157       //no scan in the middle of the flag
    00158       continue;
    00159     }
    00160
    00161     start.x = (int)(north.x + step.x * i);
    00162     start.y = (int)(north.y + step.y * i);
    00163     if ( start == lastStart ) continue;
    00164     lastStart = start;
    00165
    00166     scans++;
    00167
    00168     //find right
    00169     pointIsGood = findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, right, temp,
    00170         color, pinkIsTop, startIsInTop, horizontal, pixelCounter, topPixelCounter, bottomPixelCounter);
    00171     double rightDistance = 0.5 + Geometry::getDistanceToLine(leftVerticalLine, Vector2<double>(temp.x,temp.y));
    00172     //~ double rightDistance = 0.5 + Geometry::getDistanceToLine(leftVerticalLine, Vector2<double>(east.x,east.y));
    00173     rightDist += rightDistance;
    00174     if ( pointIsGood ) rightValidCounter++;
    00175
    00176     //find left
    00177     pointIsGood = findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, left, temp,
    00178         color, pinkIsTop, startIsInTop, horizontal, pixelCounter, topPixelCounter, bottomPixelCounter);
    00179     double leftDistance = -0.5 + Geometry::getDistanceToLine(leftVerticalLine, Vector2<double>(temp.x,temp.y));
    00180     //~ double leftDistance = -0.5 + Geometry::getDistanceToLine(leftVerticalLine, Vector2<double>(west.x,west.y));
    00181     leftDist += leftDistance;
    00182     if ( pointIsGood ) leftValidCounter++;
    00183   }
    00184
    00185   rightDist = rightDist / scans;
    00186   bool rightValid = (double)rightValidCounter >= scans / 2;
    00187   leftDist = leftDist / scans;
    00188   bool leftValid = (double)leftValidCounter >= scans / 2;
    00189
    00190   distance = sqrt((double)sqr(east.x-west.x) + sqr(east.y-west.y));
    00191
    00192   //scan vertical lines
    00193   step.x = (east.x - west.x) / (numberOfVerticalScans + 1.0);
    00194   step.y = (east.y - west.y) / (numberOfVerticalScans + 1.0);
    00195
    00196   double topDist = 0.0;
    00197   double bottomDist = 0.0;
    00198   int topValidCounter = 0;
    00199   int bottomValidCounter = 0;
    00200   scans = 0;
    00201   lastStart.x = 1000;
    00202   lastStart.y = 1000;
    00203
    00204   for(i = 1; i <= numberOfVerticalScans; i++)
    00205   {
    00206     start.x = (int)(west.x + step.x * i);
    00207     start.y = (int)(west.y + step.y * i);
    00208
    00209     if ( start == lastStart ) continue;
    00210     lastStart = start;
    00211
    00212     scans++;
    00213
    00214     //find top
    00215     pointIsGood = findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, up, north,
    00216         color, pinkIsTop, pinkIsTop, vertical, pixelCounter, topPixelCounter, bottomPixelCounter);
    00217     topDist += 0.5 + Geometry::getDistanceToLine(horizonLine, Vector2<double>(north.x,north.y));
    00218     if ( pointIsGood ) topValidCounter++;
    00219
    00220     //find bottom
    00221     pointIsGood = findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, down, south,
    00222         color, pinkIsTop, pinkIsTop, vertical, pixelCounter, topPixelCounter, bottomPixelCounter);
    00223     bottomDist += -0.5 + Geometry::getDistanceToLine(horizonLine, Vector2<double>(south.x,south.y));
    00224     if ( pointIsGood ) bottomValidCounter++;
    00225   }
    00226
    00227   topDist = topDist / scans;
    00228   bool topValid = (double)topValidCounter >= scans / 2;
    00229   bottomDist = bottomDist / scans;
    00230   bool bottomValid = (double)bottomValidCounter >= scans / 2;
    00231
    00232   double topRatio = (double)topPixelCounter / (double)pixelCounter;
    00233   double bottomRatio = (double)bottomPixelCounter / (double)pixelCounter;
    00234
    00235   if (topRatio > 0.2 && bottomRatio > 0.2) // was 0.1
    00236   {
    00237     Flag::FlagType flagType = Flag::pinkAboveYellow;
    00238     switch(color)
    00239     {
    00240       case yellow:  flagType = pinkIsTop ? Flag::pinkAboveYellow  : Flag::yellowAbovePink;  break;
    00241       // case green:   flagType = pinkIsTop ? Flag::pinkAboveGreen   : Flag::greenAbovePink;   break;
    00242       case skyblue: flagType = pinkIsTop ? Flag::pinkAboveSkyblue : Flag::skyblueAbovePink; break;
    00243     }
    00244
    00245     int i = numberOfBoundingBoxes[flagType];
    00246
    00247     boundingBoxLeft[i][flagType] = leftDist;
    00248     boundingBoxRight[i][flagType] = rightDist;
    00249     boundingBoxTop[i][flagType] = topDist;
    00250     boundingBoxBottom[i][flagType] = bottomDist;
    00251
    00252     boundingBoxLeftValid[i][flagType] = leftValid;
    00253     boundingBoxRightValid[i][flagType] = rightValid;
    00254     boundingBoxTopValid[i][flagType] = topValid;
    00255     boundingBoxBottomValid[i][flagType] = bottomValid;
    00256
    00257     numberOfBoundingBoxes[flagType]++;
    00258     if(numberOfBoundingBoxes[flagType] >= maxNumberOfBoundingBoxes)
    00259     {
    00260       numberOfBoundingBoxes[flagType] = maxNumberOfBoundingBoxes - 1;
    00261     }
    00262
    00263     DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorFlags, north.x/2, north.y/2);
    00264     DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorFlags, east.x/2, east.y/2);
    00265     DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorFlags, south.x/2, south.y/2);
    00266     DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorFlags, west.x/2, west.y/2);
    00267   }
    00268  }
    00269
    00270 bool VLCFlagSpecialist::findEndOfFlag
    00271 (
    00272  const Image& image,
    00273  const CameraInfo& bwCameraInfo,
    00274  const ColorTable& colorTable,
    00275  const Vector2<int> start,
    00276  Vector2<double> horizonDirection,
    00277  Direction directionToGo,
    00278  Vector2<int>& destination,
    00279  colorClass color,
    00280  bool pinkIsTop,
    00281  bool startIsInTop,
    00282  DebugType type,
    00283  int& countPixel,
    00284  int& countTop,
    00285  int& countBottom
    00286  )
    00287 {
    00288   bool valid = true;
    00289   int counter = 0;
    00290   int blackCounter = 0;
    00291   int outsideCounter = 0;
    00292   colorClass current, expected, allowed;
    00293   colorClass topColor = pink;
    00294   colorClass bottomColor = pink;
    00295   colorClass startColor;
    00296
    00297   if(pinkIsTop) bottomColor = color; else topColor = color;
    00298   if(startIsInTop) startColor = topColor; else startColor = bottomColor;
    00299
    00300   switch(directionToGo)
    00301   {
    00302     case left:
    00303     case right:
    00304     default:
    00305       expected = startColor;
    00306       allowed = startColor;
    00307       break;
    00308     case up:
    00309       expected = topColor;
    00310       allowed = bottomColor;
    00311       break;
    00312     case down:
    00313       expected = bottomColor;
    00314       allowed = topColor;
    00315       break;
    00316   }
    00317
    00318   destination = start;
    00319   //if(startColor == green) return false;
    00320
    00321   Vector2<int> lastInsideFlag, lastDestination;
    00322   lastInsideFlag = start;
    00323
    00324   Vector2<double> direction;
    00325   switch(directionToGo)
    00326   {
    00327   case up:
    00328     direction.x = horizonDirection.y;
    00329     direction.y = -horizonDirection.x;
    00330     break;
    00331   case right:
    00332     direction = horizonDirection;
    00333     break;
    00334   case down:
    00335     direction.x = -horizonDirection.y;
    00336     direction.y = horizonDirection.x;
    00337     break;
    00338   case left:
    00339     direction.x = -horizonDirection.x;
    00340     direction.y = -horizonDirection.y;
    00341     break;
    00342   }
    00343   enum {incX, decX, incY, decY} mode;
    00344   if(direction.y < -fabs(direction.x)) mode = decY;
    00345   else if(direction.y > fabs(direction.x)) mode = incY;
    00346   else if(direction.x < -fabs(direction.y)) mode = decX;
    00347   else mode = incX;
    00348
    00349   Vector2<int> diff;
    00350
    00351   bool goOn = true;
    00352   while(goOn)
    00353   {
    00354     counter++;
    00355
    00356     // calculate new point
    00357     switch(mode)
    00358     {
    00359     case incX:
    00360       diff.x++;
    00361       diff.y = (int)(diff.x * direction.y / direction.x);
    00362       break;
    00363     case decX:
    00364       diff.x--;
    00365       diff.y = (int)(diff.x * direction.y / direction.x);
    00366       break;
    00367     case incY:
    00368       diff.y++;
    00369       diff.x = (int)(diff.y * direction.x / direction.y);
    00370       break;
    00371     case decY:
    00372       diff.y--;
    00373       diff.x = (int)(diff.y * direction.x / direction.y);
    00374       break;
    00375     }
    00376     lastDestination = destination;
    00377     destination = start + diff;
    00378
    00379     // debug output
    00380     if ( type == initial )
    00381     {
    00382       DEBUG_IMAGE_SET_PIXEL_Y(imageProcessorFlags, destination.x/2, destination.y/2, 180)
    00383     }
    00384     if ( type == horizontal )
    00385     {
    00386       DEBUG_IMAGE_SET_PIXEL_Y(imageProcessorFlags, destination.x/2, destination.y/2, 90)
    00387     }
    00388     if ( type == vertical )
    00389     {
    00390       DEBUG_IMAGE_SET_PIXEL_Y(imageProcessorFlags, destination.x/2, destination.y/2, 0)
    00391     }
    00392
    00393     if(destination.x < 1 || destination.x >= bwCameraInfo.resolutionWidth - 1 ||
    00394         destination.y < 1 || destination.y >= bwCameraInfo.resolutionHeight - 2)
    00395     {
    00396       valid = outsideCounter > 10;
    00397       goOn = false;
    00398       destination = lastInsideFlag;
    00399     }
    00400     else
    00401     {
    00402       current = CORRECTED_COLOR_CLASS(
    00403         destination.x/2,
    00404         destination.y/2,
    00405         image.getHighResY(destination.x,destination.y),
    00406         image.image[destination.y/2][1][destination.x/2],
    00407         image.image[destination.y/2][2][destination.x/2],
    00408         (*bestColorTable), colorCorrector);
    00409
    00410       if(current == expected)
    00411       {
    00412         lastInsideFlag = destination;
    00413         outsideCounter = 0;
    00414       }
    00415       else if(current != allowed)
    00416       {
    00417         blackCounter++;
    00418         outsideCounter++;
    00419         if(blackCounter > 20)
    00420         {
    00421           goOn = false;
    00422           destination = lastInsideFlag;
    00423         }
    00424       }
    00425
    00426       if(current == topColor) countTop++;
    00427       else if(current == bottomColor) countBottom++;
    00428
    00429     }// if inside image
    00430   }//while goOn
    00431
    00432   DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorFlags, destination.x/2, destination.y/2)
    00433
    00434   countPixel += counter - outsideCounter;
    00435
    00436   return valid;
    00437 }
    00438
    00439 void VLCFlagSpecialist::getFlagPercept
    00440 (
    00441  const CameraMatrix& cameraMatrix,
    00442  const CameraMatrix& prevCameraMatrix,
    00443  const CameraInfo& cameraInfo,
    00444  const Geometry::Line horizonLine,
    00445  LandmarksPercept& landmarksPercept
    00446  )
    00447 {
    00448   CameraInfo bwCameraInfo = cameraInfo;
    00449   bwCameraInfo.resolutionHeight*=2;
    00450   bwCameraInfo.resolutionWidth*=2;
    00451   bwCameraInfo.focalLength*=2;
    00452   bwCameraInfo.focalLengthInv/=2;
    00453   bwCameraInfo.opticalCenter.x*=2;
    00454   bwCameraInfo.opticalCenter.y*=2;
    00455   int flip = getPlayer().getTeamColor() == Player::blue ? -1 : 1;
    00456
    00457   Vector2<double> verticalDirection;
    00458   verticalDirection.x = -horizonLine.direction.y;
    00459   verticalDirection.y = horizonLine.direction.x;
    00460
    00461   //~ double factor = bwCameraInfo.resolutionWidth / 2 / tan(bwCameraInfo.openingAngleWidth / 2);
    00462   double factor = bwCameraInfo.focalLength;
    00463   for(int flagType = 0; flagType < 6; flagType++)
    00464   {
    00465     // find best bounding box
    00466     for(int i = 0; i < numberOfBoundingBoxes[flagType]; i++)
    00467     {
    00468     }
    00469     bestBoundingBox[flagType] = 0;
    00470
    00471     if(numberOfBoundingBoxes[flagType] > 0)
    00472     {
    00473       Vector2<double> right, left, top, bottom;
    00474
    00475       right = horizonLine.base +
    00476         horizonLine.direction * boundingBoxRight[bestBoundingBox[flagType]][flagType]
    00477         - verticalDirection *
    00478         (boundingBoxTop[bestBoundingBox[flagType]][flagType] +
    00479         boundingBoxBottom[bestBoundingBox[flagType]][flagType]
    00480         ) / 2;
    00481
    00482       left = horizonLine.base +
    00483         horizonLine.direction * boundingBoxLeft[bestBoundingBox[flagType]][flagType]
    00484         - verticalDirection *
    00485         (boundingBoxTop[bestBoundingBox[flagType]][flagType] +
    00486         boundingBoxBottom[bestBoundingBox[flagType]][flagType]
    00487         ) / 2;
    00488
    00489       top = horizonLine.base
    00490         - verticalDirection * boundingBoxTop[bestBoundingBox[flagType]][flagType]
    00491         + horizonLine.direction *
    00492         (boundingBoxLeft[bestBoundingBox[flagType]][flagType] +
    00493         boundingBoxRight[bestBoundingBox[flagType]][flagType]
    00494         ) / 2;
    00495
    00496       bottom = horizonLine.base
    00497         - verticalDirection * boundingBoxBottom[bestBoundingBox[flagType]][flagType]
    00498         + horizonLine.direction *
    00499         (boundingBoxLeft[bestBoundingBox[flagType]][flagType] +
    00500         boundingBoxRight[bestBoundingBox[flagType]][flagType]
    00501         ) / 2;
    00502
    00503 //  CANDIDATE FOR RADIAL DISTORTION CORRECTION
    00504       Vector3<double> vectorToLeft(factor,
    00505                                                     bwCameraInfo.opticalCenter.x - left.x,
    00506                                                     bwCameraInfo.opticalCenter.y - left.y);
    00507       Vector3<double> vectorToRight(factor,
    00508                                                     bwCameraInfo.opticalCenter.x - right.x,
    00509                                                     bwCameraInfo.opticalCenter.y - right.y);
    00510       Vector3<double> vectorToTop(factor,
    00511                                                     bwCameraInfo.opticalCenter.x - top.x,
    00512                                                     bwCameraInfo.opticalCenter.y - top.y);
    00513       Vector3<double> vectorToBottom(factor,
    00514                                                     bwCameraInfo.opticalCenter.x - bottom.x,
    00515                                                     bwCameraInfo.opticalCenter.y - bottom.y);
    00516
    00517       // note: y is the row of a large (bw) image
    00518       Vector3<double>
    00519         vectorToLeftWorld = Geometry::rayFromCamera(int(left.y / 2), cameraMatrix, prevCameraMatrix, vectorToLeft, cameraInfo),
    00520         vectorToRightWorld = Geometry::rayFromCamera(int(right.y / 2), cameraMatrix, prevCameraMatrix, vectorToRight, cameraInfo),
    00521         vectorToTopWorld = Geometry::rayFromCamera(int(top.y / 2), cameraMatrix, prevCameraMatrix, vectorToTop, cameraInfo),
    00522         vectorToBottomWorld = Geometry::rayFromCamera(int(bottom.y / 2), cameraMatrix, prevCameraMatrix, vectorToBottom, cameraInfo);
    00523
    00524       double
    00525         leftAngle = atan2(vectorToLeftWorld.y,vectorToLeftWorld.x),
    00526         rightAngle = atan2(vectorToRightWorld.y,vectorToRightWorld.x),
    00527         topAngle = atan2(vectorToTopWorld.z,sqrt((double)sqr(vectorToTopWorld.x) + sqr(vectorToTopWorld.y)) ),
    00528         bottomAngle = atan2(vectorToBottomWorld.z,sqrt((double)sqr(vectorToBottomWorld.x) + sqr(vectorToBottomWorld.y)) );
    00529
    00530
    00531       Vector2<double>flagPosition;
    00532
    00533       switch (flagType)
    00534       {
    00535       case Flag::pinkAboveYellow:
    00536         flagPosition.x = xPosBackFlags * flip;
    00537         flagPosition.y = yPosRightFlags * flip;
    00538         break;
    00539       case Flag::pinkAboveSkyblue:
    00540         flagPosition.x = xPosFrontFlags * flip;
    00541         flagPosition.y = yPosRightFlags * flip;
    00542         break;
    00543       case Flag::yellowAbovePink:
    00544         flagPosition.x = xPosBackFlags * flip;
    00545         flagPosition.y = yPosLeftFlags * flip;
    00546         break;
    00547       case Flag::skyblueAbovePink:
    00548         flagPosition.x = xPosFrontFlags * flip;
    00549         flagPosition.y = yPosLeftFlags * flip;
    00550         break;
    00551       }
    00552       ConditionalBoundary boundary;
    00553       boundary.addX(leftAngle,!boundingBoxLeftValid[bestBoundingBox[flagType]][flagType]);
    00554       boundary.addX(rightAngle,!boundingBoxRightValid[bestBoundingBox[flagType]][flagType]);
    00555       boundary.addY(topAngle,!boundingBoxTopValid[bestBoundingBox[flagType]][flagType]);
    00556       boundary.addY(bottomAngle,!boundingBoxBottomValid[bestBoundingBox[flagType]][flagType]);
    00557       // calculate flag corners in image coordinates;
    00558       Vector2<int> up((int) (top.x - bottom.x) / 4, (int) (top.y - bottom.y) / 4);
    00559       Vector2<int> topLeft = Vector2<int>((int)left.x/2,(int)left.y/2) + up;
    00560       Vector2<int> topRight = Vector2<int>((int)right.x/2,(int)right.y/2) + up;
    00561       Vector2<int> bottomLeft = Vector2<int>((int)left.x/2,(int)left.y/2) - up;
    00562       Vector2<int> bottomRight = Vector2<int>((int)right.x/2,(int)right.y/2) - up;
    00563       landmarksPercept.addFlag((Flag::FlagType)flagType, flagPosition, boundary, topLeft, topRight, bottomLeft, bottomRight);
    00564     } //if(numberOfBoundingBoxes[flagType]) > 0)
    00565   }
    00566   Vector2<double> cameraOffset(cameraMatrix.translation.x,
    00567                                               cameraMatrix.translation.y);
    00568
    00569   estimateOffsetForFlags(landmarksPercept, cameraOffset);
    00570   SEND_DEBUG_IMAGE(imageProcessorFlags);
    00571 }
    00572
    00573 void VLCFlagSpecialist::estimateOffsetForFlags
    00574 (
    00575  LandmarksPercept& landmarksPercept,
    00576  const Vector2<double>& cameraOffset
    00577  )
    00578 {
    00579   for(int i = 0;i < landmarksPercept.numberOfFlags; ++i)
    00580   {
    00581     Flag& flag = landmarksPercept.flags[i];
    00582
    00583     /** @todo improve, isOnBorder(flag.x.?) not checked */
    00584     double distance;
    00585     double direction = flag.x.getCenter();
    00586
    00587     if(!flag.isOnBorder(flag.y.min) && !flag.isOnBorder(flag.y.max))
    00588     {
    00589       if(flag.y.min != flag.y.max)
    00590       {
    00591         distance = flagHeight / (tan(flag.y.max) - tan(flag.y.min)) + flagRadius;
    00592         flag.distanceValidity = 0.8;
    00593       }
    00594       else
    00595       {
    00596         distance = 4500;
    00597         flag.distanceValidity = 0.05;
    00598       }
    00599     }
    00600     else
    00601     {
    00602       distance = flagRadius / sin(flag.x.getSize() / 2);
    00603       if(!flag.isOnBorder(flag.x.min) && !flag.isOnBorder(flag.x.max)) // Flag touches no vertical border
    00604         flag.distanceValidity = 0.7;
    00605       else
    00606         flag.distanceValidity = 0.2;
    00607     }
    00608     if(!flag.isOnBorder(flag.x.min) && !flag.isOnBorder(flag.x.max)) // Flag touches no vertical border
    00609       flag.angleValidity = 0.8;
    00610     else
    00611       flag.angleValidity = 0.7;
    00612
    00613     Pose2D p = Pose2D(cameraOffset) + Pose2D(direction)
    00614                        + Pose2D(Vector2<double>(distance,0));
    00615     flag.distance = p.translation.abs();
    00616     flag.angle = direction;
    00617     if (flag.distance > 6000)
    00618     {
    00619       flag.distance = 6000;
    00620       flag.distanceValidity=0; // flags far away are meassured very bad
    00621     }
    00622     else if (flag.distance > 3000)
    00623       flag.distanceValidity*=0.5; // flags medium far away are meassured medium bad
    00624   }
    00625 }