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

# Modules/ImageProcessor/SlamImageProcessor/SlamFlagSpecialist.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file SlamFlagSpecialist.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 "Tools/Debugging/View.h"
    00012 #include "Modules/ImageProcessor/ImageProcessorTools/ColorCorrector.h"
    00013 #include "SlamFlagSpecialist.h"
    00014 #include "SlamImageProcessorTools.h"
    00015
    00016
    00017 SlamFlagSpecialist::SlamFlagSpecialist(const ColorCorrector& colorCorrector) :
    00018   colorCorrector(colorCorrector)
    00019 {
    00020 }
    00021
    00022 void SlamFlagSpecialist::init(const Image& image)
    00023 {
    00024   for(int i = 0; i < 6; i++)
    00025   {
    00026     numberOfBoundingBoxes[i] = 0;
    00027   }
    00028   INIT_DEBUG_IMAGE(imageProcessorFlags, image);
    00029 }
    00030
    00031 void SlamFlagSpecialist::searchFlags
    00032 (
    00033  const Image& image,
    00034  const ColorTable& colorTable,
    00035  const CameraMatrix& cameraMatrix,
    00036  colorClass color,
    00037  bool pinkIsTop,
    00038  const Geometry::Line horizonLine,
    00039  int x, int y
    00040  )
    00041 {
    00042   CameraInfo bwCameraInfo = image.cameraInfo;
    00043   bwCameraInfo.resolutionHeight*=2;
    00044   bwCameraInfo.resolutionWidth*=2;
    00045   bwCameraInfo.focalLength*=2;
    00046   bwCameraInfo.focalLengthInv/=2;
    00047   bwCameraInfo.opticalCenter.x*=2;
    00048   bwCameraInfo.opticalCenter.y*=2;
    00049   bool gridPointIsCovered = false;
    00050
    00051   // x and y are in old resolution, so no division
    00052   DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorFlags, x, y)
    00053
    00054   int i, j;
    00055
    00056   for(i = 0; i < 6; i++)
    00057   {
    00058     for(j = 0; j < numberOfBoundingBoxes[i]; j++)
    00059     {
    00060       if(x >= boundingBoxLeft[j][i] && x <= boundingBoxRight[j][i] &&
    00061         y >= boundingBoxBottom[j][i] && y <= boundingBoxTop[j][i] )
    00062       {
    00063         gridPointIsCovered = true;
    00064       }
    00065     }// j
    00066   }//i
    00067
    00068   if(gridPointIsCovered) return;
    00069
    00070   int pixelCounter = 0;
    00071   int topPixelCounter = 0;
    00072   int bottomPixelCounter = 0;
    00073
    00074   start.x = 2*x;
    00075   start.y = 2*y;
    00076
    00077   //find up
    00078   findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction,
    00079     up, north, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
    00080   //find down
    00081   findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction,
    00082     down, south, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
    00083   DOT(imageProcessor_ball4, south.x/2, south.y/2, Drawings::black, Drawings::green);
    00084   //find right
    00085   findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction,
    00086     right, east, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
    00087   //find left
    00088   findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction,
    00089     left, west, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
    00090
    00091   DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorFlags, north.x/2, north.y/2);
    00092   DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorFlags, east.x/2, east.y/2);
    00093   DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorFlags, south.x/2, south.y/2);
    00094   DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorFlags, west.x/2, west.y/2);
    00095
    00096   if(west == east)
    00097   {
    00098     Vector2<int> newStart((north.x + south.x)/2, (north.y + south.y)/2);
    00099     //find right
    00100     findEndOfFlag(image, bwCameraInfo, colorTable, newStart, horizonLine.direction,
    00101       right, east, color, pinkIsTop, false, horizontal, pixelCounter, topPixelCounter, bottomPixelCounter);
    00102     //find left
    00103     findEndOfFlag(image, bwCameraInfo, colorTable, newStart, horizonLine.direction,
    00104       left, west, color, pinkIsTop, false, horizontal, pixelCounter, topPixelCounter, bottomPixelCounter);
    00105   }
    00106   //go to center of pink
    00107   start.x = (west.x + east.x) / 2;
    00108   start.y = (west.y + east.y) / 2;
    00109   DOT(imageProcessor_ball4, start.x/2, start.y/2, Drawings::black, Drawings::red);
    00110   DOT(imageProcessor_ball4, east.x/2, east.y/2, Drawings::black, Drawings::yellow);
    00111   DOT(imageProcessor_ball4, west.x/2, west.y/2, Drawings::black, Drawings::yellow);
    00112
    00113   DEBUG_IMAGE_SET_PIXEL_YELLOW(imageProcessorFlags, start.x/2, start.y/2);
    00114
    00115   //find north
    00116   findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction,
    00117     up, north, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
    00118   //find south
    00119   findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction,
    00120     down, south, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
    00121
    00122   double distance = sqrt((double)sqr(south.x-north.x) + sqr(south.y-north.y));
    00123
    00124   // reset counters
    00125   pixelCounter = 0;
    00126   topPixelCounter = 0;
    00127   bottomPixelCounter = 0;
    00128
    00129   double rightDist = 0.0;
    00130   double leftDist = 0.0;
    00131   int rightValidCounter = 0;
    00132   int leftValidCounter = 0;
    00133   int scans = 0;
    00134
    00135   Vector2<int> temp;
    00136
    00137   Vector2<int> lastStart;
    00138   lastStart.x = 1000;
    00139   lastStart.y = 1000;
    00140
    00141   //scan horizontal lines
    00142   Vector2<double>step;
    00143   step.x = (south.x - north.x) / (numberOfHorizontalScans + 1.0);
    00144   step.y = (south.y - north.y) / (numberOfHorizontalScans + 1.0);
    00145
    00146   Geometry::Line leftVerticalLine;
    00147   leftVerticalLine.base = horizonLine.base;
    00148   leftVerticalLine.direction.x = -horizonLine.direction.y;
    00149   leftVerticalLine.direction.y = horizonLine.direction.x;
    00150
    00151   bool pointIsGood;
    00152   bool startIsInTop = true;
    00153   for(i = 1; i <= numberOfHorizontalScans; i++)
    00154   {
    00155     if(i==(numberOfHorizontalScans + 1)/2)
    00156     {
    00157       startIsInTop = false;
    00158       //no scan in the middle of the flag
    00159       continue;
    00160     }
    00161
    00162     start.x = (int)(north.x + step.x * i);
    00163     start.y = (int)(north.y + step.y * i);
    00164     if ( start == lastStart ) continue;
    00165     lastStart = start;
    00166
    00167     scans++;
    00168
    00169     //find right
    00170     pointIsGood = findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, right, temp,
    00171         color, pinkIsTop, startIsInTop, horizontal, pixelCounter, topPixelCounter, bottomPixelCounter);
    00172     double rightDistance = 0.5 + Geometry::getDistanceToLine(leftVerticalLine, Vector2<double>(temp.x,temp.y));
    00173     //~ double rightDistance = 0.5 + Geometry::getDistanceToLine(leftVerticalLine, Vector2<double>(east.x,east.y));
    00174     rightDist += rightDistance;
    00175     if ( pointIsGood ) rightValidCounter++;
    00176
    00177     //find left
    00178     pointIsGood = findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, left, temp,
    00179         color, pinkIsTop, startIsInTop, horizontal, pixelCounter, topPixelCounter, bottomPixelCounter);
    00180     double leftDistance = -0.5 + Geometry::getDistanceToLine(leftVerticalLine, Vector2<double>(temp.x,temp.y));
    00181     //~ double leftDistance = -0.5 + Geometry::getDistanceToLine(leftVerticalLine, Vector2<double>(west.x,west.y));
    00182     leftDist += leftDistance;
    00183     if ( pointIsGood ) leftValidCounter++;
    00184   }
    00185
    00186   rightDist = rightDist / scans;
    00187   bool rightValid = (double)rightValidCounter >= scans / 2;
    00188   leftDist = leftDist / scans;
    00189   bool leftValid = (double)leftValidCounter >= scans / 2;
    00190
    00191   distance = sqrt((double)sqr(east.x-west.x) + sqr(east.y-west.y));
    00192
    00193   //scan vertical lines
    00194   step.x = (east.x - west.x) / (numberOfVerticalScans + 1.0);
    00195   step.y = (east.y - west.y) / (numberOfVerticalScans + 1.0);
    00196
    00197   double topDist = 0.0;
    00198   double bottomDist = 0.0;
    00199   int topValidCounter = 0;
    00200   int bottomValidCounter = 0;
    00201   scans = 0;
    00202   lastStart.x = 1000;
    00203   lastStart.y = 1000;
    00204
    00205   for(i = 1; i <= numberOfVerticalScans; i++)
    00206   {
    00207     start.x = (int)(west.x + step.x * i);
    00208     start.y = (int)(west.y + step.y * i);
    00209
    00210     if ( start == lastStart ) continue;
    00211     lastStart = start;
    00212
    00213     scans++;
    00214
    00215     //find top
    00216     pointIsGood = findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, up, north,
    00217         color, pinkIsTop, pinkIsTop, vertical, pixelCounter, topPixelCounter, bottomPixelCounter);
    00218     topDist += 0.5 + Geometry::getDistanceToLine(horizonLine, Vector2<double>(north.x,north.y));
    00219     if ( pointIsGood ) topValidCounter++;
    00220
    00221     //find bottom
    00222     pointIsGood = findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, down, south,
    00223         color, pinkIsTop, pinkIsTop, vertical, pixelCounter, topPixelCounter, bottomPixelCounter);
    00224     bottomDist += -0.5 + Geometry::getDistanceToLine(horizonLine, Vector2<double>(south.x,south.y));
    00225     if ( pointIsGood ) bottomValidCounter++;
    00226   }
    00227
    00228   topDist = topDist / scans;
    00229   bool topValid = (double)topValidCounter >= scans / 2;
    00230   bottomDist = bottomDist / scans;
    00231   bool bottomValid = (double)bottomValidCounter >= scans / 2;
    00232
    00233   double topRatio = (double)topPixelCounter / (double)pixelCounter;
    00234   double bottomRatio = (double)bottomPixelCounter / (double)pixelCounter;
    00235
    00236   if (topRatio > 0.2 && bottomRatio > 0.2) // was 0.1
    00237   {
    00238     Flag::FlagType flagType = Flag::pinkAboveYellow;
    00239     switch(color)
    00240     {
    00241       case yellow:  flagType = pinkIsTop ? Flag::pinkAboveYellow  : Flag::yellowAbovePink;  break;
    00242       // case green:   flagType = pinkIsTop ? Flag::pinkAboveGreen   : Flag::greenAbovePink;   break;
    00243       case skyblue: flagType = pinkIsTop ? Flag::pinkAboveSkyblue : Flag::skyblueAbovePink; break;
    00244     }
    00245
    00246     int i = numberOfBoundingBoxes[flagType];
    00247
    00248     boundingBoxLeft[i][flagType] = leftDist;
    00249     boundingBoxRight[i][flagType] = rightDist;
    00250     boundingBoxTop[i][flagType] = topDist;
    00251     boundingBoxBottom[i][flagType] = bottomDist;
    00252
    00253     boundingBoxLeftValid[i][flagType] = leftValid;
    00254     boundingBoxRightValid[i][flagType] = rightValid;
    00255     boundingBoxTopValid[i][flagType] = topValid;
    00256     boundingBoxBottomValid[i][flagType] = bottomValid;
    00257
    00258     numberOfBoundingBoxes[flagType]++;
    00259     if(numberOfBoundingBoxes[flagType] >= maxNumberOfBoundingBoxes)
    00260     {
    00261       numberOfBoundingBoxes[flagType] = maxNumberOfBoundingBoxes - 1;
    00262     }
    00263
    00264     DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorFlags, north.x/2, north.y/2);
    00265     DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorFlags, east.x/2, east.y/2);
    00266     DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorFlags, south.x/2, south.y/2);
    00267     DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorFlags, west.x/2, west.y/2);
    00268   }
    00269  }
    00270
    00271 bool SlamFlagSpecialist::findEndOfFlag
    00272 (
    00273  const Image& image,
    00274  const CameraInfo& bwCameraInfo,
    00275  const ColorTable& colorTable,
    00276  const Vector2<int> start,
    00277  Vector2<double> horizonDirection,
    00278  Direction directionToGo,
    00279  Vector2<int>& destination,
    00280  colorClass color,
    00281  bool pinkIsTop,
    00282  bool startIsInTop,
    00283  DebugType type,
    00284  int& countPixel,
    00285  int& countTop,
    00286  int& countBottom
    00287  )
    00288 {
    00289   bool valid = true;
    00290   int counter = 0;
    00291   int blackCounter = 0;
    00292   int outsideCounter = 0;
    00293   colorClass current, expected, allowed;
    00294   colorClass topColor = pink;
    00295   colorClass bottomColor = pink;
    00296   colorClass startColor;
    00297
    00298   if(pinkIsTop) bottomColor = color; else topColor = color;
    00299   if(startIsInTop) startColor = topColor; else startColor = bottomColor;
    00300
    00301   switch(directionToGo)
    00302   {
    00303     case left:
    00304     case right:
    00305     default:
    00306       expected = startColor;
    00307       allowed = startColor;
    00308       break;
    00309     case up:
    00310       expected = topColor;
    00311       allowed = bottomColor;
    00312       break;
    00313     case down:
    00314       expected = bottomColor;
    00315       allowed = topColor;
    00316       break;
    00317   }
    00318
    00319   destination = start;
    00320   //if(startColor == green) return false;
    00321
    00322   Vector2<int> lastInsideFlag, lastDestination;
    00323   lastInsideFlag = start;
    00324
    00325   Vector2<double> direction;
    00326   switch(directionToGo)
    00327   {
    00328   case up:
    00329     direction.x = horizonDirection.y;
    00330     direction.y = -horizonDirection.x;
    00331     break;
    00332   case right:
    00333     direction = horizonDirection;
    00334     break;
    00335   case down:
    00336     direction.x = -horizonDirection.y;
    00337     direction.y = horizonDirection.x;
    00338     break;
    00339   case left:
    00340     direction.x = -horizonDirection.x;
    00341     direction.y = -horizonDirection.y;
    00342     break;
    00343   }
    00344   enum {incX, decX, incY, decY} mode;
    00345   if(direction.y < -fabs(direction.x)) mode = decY;
    00346   else if(direction.y > fabs(direction.x)) mode = incY;
    00347   else if(direction.x < -fabs(direction.y)) mode = decX;
    00348   else mode = incX;
    00349
    00350   Vector2<int> diff;
    00351
    00352   bool goOn = true;
    00353   while(goOn)
    00354   {
    00355     counter++;
    00356
    00357     // calculate new point
    00358     switch(mode)
    00359     {
    00360     case incX:
    00361       diff.x++;
    00362       diff.y = (int)(diff.x * direction.y / direction.x);
    00363       break;
    00364     case decX:
    00365       diff.x--;
    00366       diff.y = (int)(diff.x * direction.y / direction.x);
    00367       break;
    00368     case incY:
    00369       diff.y++;
    00370       diff.x = (int)(diff.y * direction.x / direction.y);
    00371       break;
    00372     case decY:
    00373       diff.y--;
    00374       diff.x = (int)(diff.y * direction.x / direction.y);
    00375       break;
    00376     }
    00377     lastDestination = destination;
    00378     destination = start + diff;
    00379
    00380     // debug output
    00381     if ( type == initial )
    00382     {
    00383       DEBUG_IMAGE_SET_PIXEL_Y(imageProcessorFlags, destination.x/2, destination.y/2, 180)
    00384     }
    00385     if ( type == horizontal )
    00386     {
    00387       DEBUG_IMAGE_SET_PIXEL_Y(imageProcessorFlags, destination.x/2, destination.y/2, 90)
    00388     }
    00389     if ( type == vertical )
    00390     {
    00391       DEBUG_IMAGE_SET_PIXEL_Y(imageProcessorFlags, destination.x/2, destination.y/2, 0)
    00392     }
    00393
    00394     if(destination.x < 1 || destination.x >= bwCameraInfo.resolutionWidth - 1 ||
    00395         destination.y < 1 || destination.y >= bwCameraInfo.resolutionHeight - 2)
    00396     {
    00397       valid = outsideCounter > 10;
    00398       goOn = false;
    00399       destination = lastInsideFlag;
    00400     }
    00401     else
    00402     {
    00403       current = CORRECTED_COLOR_CLASS(
    00404         destination.x/2,
    00405         destination.y/2,
    00406         image.getHighResY(destination.x,destination.y),
    00407         image.image[destination.y/2][1][destination.x/2],
    00408         image.image[destination.y/2][2][destination.x/2]);
    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 SlamFlagSpecialist::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 SlamFlagSpecialist::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 }