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

# Modules/ImageProcessor/GT2005ImageProcessor/GT2005BeaconDetector.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005BeaconDetector.cpp
    00003 *
    00004 * Implementation of class GT2005BeaconDetector.
    00005 *
    00006 * @author <a href="mailto:timlaue@tzi.de">Tim Laue</a>
    00007 * @author <a href="mailto:walter.nistico@uni-dortmund.de">Walter Nistico</a>
    00008 */
    00009
    00010
    00011 #include "Representations/Perception/Image.h"
    00012 #ifdef CT32K_LAYOUT
    00013 #include "Representations/Perception/ColorTable32K.h"
    00014 #else
    00015 #include "Representations/Perception/ColorTable64.h"
    00016 #endif
    00017 #include "Representations/Perception/CameraMatrix.h"
    00018 #include "Representations/Perception/LandmarksPercept.h"
    00019 #include "Tools/FieldDimensions.h"
    00020 #include "Tools/RingBuffer.h"
    00021 #include "Tools/Math/Vector2.h"
    00022 #include "Modules/ImageProcessor/ImageProcessorTools/ColorCorrector.h"
    00023 #include "GT2005ImageProcessorTools.h"
    00024 #include "GT2005BeaconDetector.h"
    00025
    00026
    00027 const int Y(0),
    00028           U(cameraResolutionWidth_ERS7), /**< Relative offset of U component. */
    00029           V(2 * cameraResolutionWidth_ERS7); /**< Relative offset of V component. */
    00030
    00031
    00032 GT2005BeaconDetector::GT2005BeaconDetector(const Image& image,
    00033                        const CameraMatrix& cameraMatrix,
    00034                        const CameraMatrix& prevCameraMatrix,
    00035                        const ImageInfo& imageInf,
    00036                        const ColorTable& colorTable,
    00037                        const ColorCorrector& colorCorrector,
    00038                        LandmarksPercept& landmarksPercept):
    00039                        image(image), cameraMatrix(cameraMatrix),
    00040                        prevCameraMatrix(prevCameraMatrix),
    00041                        imageInf(imageInf), colorTable(colorTable),
    00042                        colorCorrector(colorCorrector),
    00043                        landmarksPercept(landmarksPercept),
    00044                        flagSpecialist(colorCorrector),
    00045                        horizontalBaseOffset(1.6),
    00046                        numOfHorizontalScanLineAbove(10),
    00047                        numOfHorizontalScanLineBelow(3),
    00048                        horizontalOffsetModifier(0.9),
    00049                        clusteringDistanceTolerance(6),
    00050                        minPinkRunLength(2),
    00051                        clusteringAspectRatio(1.5),
    00052                        projectionAspectRatio(0.4),
    00053                        minFlagConfidence(0.5),
    00054                        edgeScanDepth(6),
    00055                        edgeDetectionU(edgeThresholdU),
    00056                        edgeDetectionV(edgeThresholdV)
    00057 {}
    00058
    00059
    00060 void GT2005BeaconDetector::execute()
    00061 {
    00062   flagSpecialist.init(image);
    00063   numOfBeaconCandidates = 0;
    00064   double dist(horizontalBaseOffset);
    00065   Geometry::Line scanLine;
    00066   scanLine.direction = imageInf.horizon.direction;
    00067   scanLine.base = imageInf.horizon.base;
    00068   int i;
    00069   for(i=0; i < numOfHorizontalScanLineBelow; i++)
    00070   {
    00071     scanLine.base += imageInf.vertLine.direction*dist;
    00072     dist += horizontalOffsetModifier;
    00073     Vector2<int> startPoint, endPoint;
    00074     if(Geometry::getIntersectionPointsOfLineAndRectangle(
    00075        Vector2<int>(0,0), imageInf.maxImageCoordinates, scanLine, startPoint, endPoint))
    00076     {
    00077       LINE(imageProcessor_general, startPoint.x, startPoint.y, endPoint.x, endPoint.y, 1, Drawings::ps_dash, Drawings::gray);
    00078       scanForPink(startPoint, endPoint);
    00079     }
    00080     else
    00081     {
    00082       break;
    00083     }
    00084   }
    00085   dist  = horizontalBaseOffset;
    00086   scanLine.base = imageInf.horizon.base;
    00087   for(i=0; i < numOfHorizontalScanLineAbove; i++)
    00088   {
    00089     scanLine.base -= imageInf.vertLine.direction*dist;
    00090     dist += horizontalOffsetModifier;
    00091     Vector2<int> startPoint, endPoint;
    00092     if(Geometry::getIntersectionPointsOfLineAndRectangle(
    00093        Vector2<int>(0,0), imageInf.maxImageCoordinates, scanLine, startPoint, endPoint))
    00094     {
    00095       LINE(imageProcessor_general, startPoint.x, startPoint.y, endPoint.x, endPoint.y, 1, Drawings::ps_dash, Drawings::yellow);
    00096       scanForPink(startPoint, endPoint);
    00097     }
    00098     else
    00099     {
    00100       break;
    00101     }
    00102   }
    00103   if(numOfBeaconCandidates)
    00104   {
    00105     clusterPinkBeaconParts();
    00106   }
    00107   flagSpecialist.getFlagPercept(cameraMatrix, prevCameraMatrix, image.cameraInfo, imageInf.horizon, landmarksPercept);
    00108 }
    00109
    00110
    00111 void GT2005BeaconDetector::scanForPink(const Vector2<int>& start,
    00112                                        const Vector2<int>& end)
    00113 {
    00114   BresenhamLineScan bresenham(start, end);
    00115   bresenham.init();
    00116   Vector2<int> pixel(start), lastPixel(start);
    00117   int Y(0),U(0),V(0),lastY(0),lastU(0),lastV(0);
    00118   Run currentRun;
    00119   bool isPinkRun(false);
    00120   bool currentPixelPink(false);
    00121   for(int i=0; i<bresenham.numberOfPixels; i++)
    00122   {
    00123     U = image.image[pixel.y][1][pixel.x];
    00124
    00125     U = colorCorrector.correct(pixel.x, pixel.y, 1, U);
    00126     if(U > minPinkUValue)
    00127     {
    00128       Y = image.image[pixel.y][0][pixel.x];
    00129       Y = colorCorrector.correct(pixel.x, pixel.y, 0, Y);
    00130       V = image.image[pixel.y][2][pixel.x];
    00131       V = colorCorrector.correct(pixel.x, pixel.y, 2, V);
    00132       currentPixelPink = (COLOR_CLASS(Y,U,V,colorTable) == pink);
    00133     }
    00134     else
    00135     {
    00136       currentPixelPink = false;
    00137     }
    00138     if(!isPinkRun)
    00139     {
    00140       if(currentPixelPink)
    00141       {
    00142         currentRun.scanLineStart = start;
    00143         currentRun.start = pixel;
    00144         currentRun.length = 1;
    00145         isPinkRun = true;
    00146       }
    00147     }
    00148     else //if(isPinkRun)
    00149     {
    00150       if(currentPixelPink)// || (COLOR_COMPONENT_DIST(U,lastU) < params.maxPinkUContrast))
    00151       {
    00152         currentRun.length++;
    00153         lastY = Y;
    00154         lastU = U;
    00155         lastV = V;
    00156       }
    00157       else
    00158       {
    00159         isPinkRun = false;
    00160         if(currentRun.length >= minPinkRunLength)
    00161         {
    00162           currentRun.end = lastPixel;
    00163           if(!addCandidate(currentRun)) break; //List is full, stop searching
    00164           LINE(imageProcessor_general, currentRun.start.x, currentRun.start.y,
    00165                currentRun.end.x, currentRun.end.y, 1, Drawings::ps_solid, Drawings::pink);
    00166         }
    00167       }
    00168     }
    00169     //Compute next position according to Bresenham algorithm
    00170     lastPixel = pixel;
    00171     bresenham.getNext(pixel);
    00172   }
    00173   //Finish the last Run
    00174   if(isPinkRun)
    00175   {
    00176     currentRun.end = pixel;
    00177     addCandidate(currentRun);
    00178     LINE(imageProcessor_general, currentRun.start.x, currentRun.start.y,
    00179                currentRun.end.x, currentRun.end.y, 1, Drawings::ps_solid, Drawings::pink);
    00180   }
    00181 }
    00182
    00183
    00184 bool GT2005BeaconDetector::addCandidate(const Run& pinkRun)
    00185 {
    00186   bool returnValue(numOfBeaconCandidates<MAX_NUMBER_OF_PINK_RUNS);
    00187   if(returnValue)
    00188   {
    00189     if((numOfBeaconCandidates>0) &&
    00190        (pinkRun.scanLineStart == beaconCandidates[numOfBeaconCandidates-1].scanLineStart) &&
    00191        ((beaconCandidates[numOfBeaconCandidates-1].end - pinkRun.start).abs()<4))
    00192     {
    00193       beaconCandidates[numOfBeaconCandidates-1].end = pinkRun.end;
    00194     }
    00195     else
    00196     {
    00197       beaconCandidates[numOfBeaconCandidates++] = pinkRun;
    00198     }
    00199   }
    00200   return returnValue;
    00201 }
    00202
    00203
    00204 void GT2005BeaconDetector::clusterPinkBeaconParts()
    00205 {
    00206   //Transform pink runs
    00207   double rotation(imageInf.horizon.direction.angle());
    00208   double ca(cos(-rotation));
    00209   double sa(sin(-rotation));
    00210   Matrix2x2<double> rotMat(Vector2<double>(ca,sa),Vector2<double>(-sa,ca));
    00211   Matrix2x2<double> invRotMat = rotMat.transpose(); //=rotMat.invert(), orthonormal matrix
    00212   for(int i=0; i<numOfBeaconCandidates; i++)
    00213   {
    00214     transformedCandidates[i].transform(beaconCandidates[i], rotMat, i);
    00215   }
    00216   //Sort by starting position from left to right
    00217   for(int j=0; j<numOfBeaconCandidates; j++)
    00218   {
    00219     int leftest(j);
    00220     for(int k=j; k<numOfBeaconCandidates; k++)
    00221     {
    00222       if(transformedCandidates[k].start.x<transformedCandidates[leftest].start.x)
    00223       {
    00224         leftest = k;
    00225       }
    00226     }
    00227     if(leftest != j)
    00228     {
    00229       TransformedRun help(transformedCandidates[j]);
    00230       transformedCandidates[j] = transformedCandidates[leftest];
    00231       transformedCandidates[leftest] = help;
    00232     }
    00233   }
    00234   //Find overlapping runs
    00235   int beginOfBeacon(0), endOfBeacon(0);
    00236   double xRight(transformedCandidates[0].end.x);
    00237   double xLeft(transformedCandidates[0].start.x);
    00238   double yRight(transformedCandidates[0].end.y);
    00239   double yLeft(transformedCandidates[0].start.y);
    00240   double runLength(xRight-xLeft);
    00241   double yMax, yMin;
    00242   double originMinY, originMaxY;
    00243   if (yRight > yLeft)
    00244   {
    00245     yMax = yRight;
    00246     yMin = yLeft;
    00247   }
    00248   else
    00249   {
    00250     yMax = yLeft;
    00251     yMin = yRight;
    00252   }
    00253   originMinY = beaconCandidates[0].scanLineStart.y;
    00254   originMaxY = beaconCandidates[0].scanLineStart.y;
    00255   for(int l=1; l<numOfBeaconCandidates; l++)
    00256   {
    00257     if(transformedCandidates[l].start.x < xRight  + (double)clusteringDistanceTolerance)
    00258     {// within merging distance
    00259       if(transformedCandidates[l].end.x > xRight)
    00260       { // new piece could extend the run
    00261         double candidateLength = transformedCandidates[l].end.x-xLeft;
    00262         bool newRunHigher = (transformedCandidates[l].end.y-yLeft > 0.0);
    00263         double candidateHeight = newRunHigher ? (transformedCandidates[l].end.y-yLeft) : (yLeft-transformedCandidates[l].end.y);
    00264         if (candidateHeight < candidateLength*clusteringAspectRatio) // the proportions of the landmark are consistent (1:1 ideally)
    00265         {
    00266           xRight = transformedCandidates[l].end.x;
    00267           yRight = transformedCandidates[l].end.y;
    00268           endOfBeacon = l;
    00269           if (transformedCandidates[l].end.y-yMax > 0.0)
    00270           {
    00271             yMax = transformedCandidates[l].end.y;
    00272             originMaxY = beaconCandidates[l].scanLineStart.y;
    00273           }
    00274           else if (yMin - transformedCandidates[l].end.y > 0.0)
    00275           {
    00276             yMin = transformedCandidates[l].end.y;
    00277             originMinY = beaconCandidates[l].scanLineStart.y;
    00278           }
    00279         }
    00280         else
    00281         { // we have to choose one run, the other is perhaps a false positive
    00282           double newLength = transformedCandidates[l].end.x - transformedCandidates[l].start.x;
    00283           if (newLength > runLength) // the new run wins if longest
    00284           {
    00285             xRight = transformedCandidates[l].end.x;
    00286             xLeft = transformedCandidates[l].start.x;
    00287             yRight = transformedCandidates[l].end.y;
    00288             yLeft = transformedCandidates[l].start.y;
    00289             endOfBeacon = l;
    00290             beginOfBeacon = l;
    00291             runLength = newLength;
    00292             if (yRight > yLeft)
    00293             {
    00294               yMax = yRight;
    00295               yMin = yLeft;
    00296             }
    00297             else
    00298             {
    00299               yMax = yLeft;
    00300               yMin = yRight;
    00301             }
    00302             originMinY = beaconCandidates[l].scanLineStart.y;
    00303             originMaxY = beaconCandidates[l].scanLineStart.y;
    00304           }
    00305         }
    00306       }
    00307       else
    00308       { // the new run is horizontally contained
    00309         double newRunHeight = (transformedCandidates[l].end.y + transformedCandidates[l].start.y)/2.0;
    00310         bool candidateMax = (newRunHeight-yMax > 0.0);
    00311         bool candidateMin = (yMin - newRunHeight > 0.0);
    00312         if (candidateMax)
    00313         {
    00314           double candidateHeight = newRunHeight - yMin;
    00315           double candidateLength = xRight-xLeft;
    00316           if (candidateHeight < candidateLength*clusteringAspectRatio) // the proportions of the landmark are consistent (1:1 ideally)
    00317           {
    00318             yMax = newRunHeight;
    00319             originMaxY = beaconCandidates[l].scanLineStart.y;
    00320           }
    00321         }
    00322         else if (candidateMin)
    00323         {
    00324           double candidateHeight = yMax - newRunHeight;
    00325           double candidateLength = xRight-xLeft;
    00326           if (candidateHeight < candidateLength*clusteringAspectRatio) // the proportions of the landmark are consistent (1:1 ideally)
    00327           {
    00328             yMin = newRunHeight;
    00329             originMinY = beaconCandidates[l].scanLineStart.y;
    00330           }
    00331         }
    00332       }
    00333     }
    00334     else
    00335     {
    00336       double originX = (beaconCandidates[beginOfBeacon].scanLineStart.x + beaconCandidates[endOfBeacon].scanLineStart.x)/2.0;
    00337       //~ double originY = (beaconCandidates[beginOfBeacon].scanLineStart.y + beaconCandidates[endOfBeacon].scanLineStart.y)/2.0;
    00338       double originY = (originMinY + originMaxY)/2;
    00339       double mergedScanLineLength = xRight-xLeft;
    00340       Vector2<double> leftOfBeacon, rightOfBeacon;
    00341       leftOfBeacon.x = xLeft - originX;
    00342       rightOfBeacon.x = xRight - originX;
    00343       leftOfBeacon.y = (yMax+yMin)/2.0 - originY;
    00344       rightOfBeacon.y = leftOfBeacon.y;
    00345       leftOfBeacon = (invRotMat*leftOfBeacon);
    00346       rightOfBeacon = (invRotMat*rightOfBeacon);
    00347       leftOfBeacon.x += originX;
    00348       leftOfBeacon.y += originY;
    00349       rightOfBeacon.x += originX;
    00350       rightOfBeacon.y += originY;
    00351     Geometry::clipPointInsideRectange(Vector2<int>(0,0), Vector2<int>(image.cameraInfo.resolutionWidth,image.cameraInfo.resolutionHeight), leftOfBeacon);
    00352     Geometry::clipPointInsideRectange(Vector2<int>(0,0), Vector2<int>(image.cameraInfo.resolutionWidth,image.cameraInfo.resolutionHeight), rightOfBeacon);
    00353       LINE(imageProcessor_general, int(leftOfBeacon.x+0.5), int(leftOfBeacon.y+0.5),
    00354             int(rightOfBeacon.x+0.5), int(rightOfBeacon.y+0.5), 1, Drawings::ps_dash, Drawings::red);
    00355       analyzeBeacon(leftOfBeacon, mergedScanLineLength);
    00356       beginOfBeacon = l;
    00357       endOfBeacon = l;
    00358       xRight = transformedCandidates[l].end.x;
    00359       yRight = transformedCandidates[l].end.y;
    00360       xLeft = transformedCandidates[l].start.x;
    00361       yLeft = transformedCandidates[l].start.y;
    00362       if (yRight > yLeft)
    00363       {
    00364         yMax = yRight;
    00365         yMin = yLeft;
    00366       }
    00367       else
    00368       {
    00369         yMax = yLeft;
    00370         yMin = yRight;
    00371       }
    00372       originMinY = beaconCandidates[l].scanLineStart.y;
    00373       originMaxY = beaconCandidates[l].scanLineStart.y;
    00374     }
    00375   }
    00376   double originX = (beaconCandidates[beginOfBeacon].scanLineStart.x + beaconCandidates[endOfBeacon].scanLineStart.x)/2.0;
    00377   //~ double originY = (beaconCandidates[beginOfBeacon].scanLineStart.y + beaconCandidates[endOfBeacon].scanLineStart.y)/2.0;
    00378   double originY = (originMinY + originMaxY)/2;
    00379   double mergedScanLineLength = xRight-xLeft;
    00380   Vector2<double> leftOfBeacon, rightOfBeacon;
    00381   leftOfBeacon.x = xLeft - originX;
    00382   rightOfBeacon.x = xRight - originX;
    00383   leftOfBeacon.y = (yMax+yMin)/2.0 - originY;
    00384   rightOfBeacon.y = leftOfBeacon.y;
    00385   leftOfBeacon = (invRotMat*leftOfBeacon);
    00386   rightOfBeacon = (invRotMat*rightOfBeacon);
    00387   leftOfBeacon.x += originX;
    00388   leftOfBeacon.y += originY;
    00389   rightOfBeacon.x += originX;
    00390   rightOfBeacon.y += originY;
    00391   Geometry::clipPointInsideRectange(Vector2<int>(0,0), Vector2<int>(image.cameraInfo.resolutionWidth,image.cameraInfo.resolutionHeight), leftOfBeacon);
    00392   Geometry::clipPointInsideRectange(Vector2<int>(0,0), Vector2<int>(image.cameraInfo.resolutionWidth,image.cameraInfo.resolutionHeight), rightOfBeacon);
    00393   LINE(imageProcessor_general, int(leftOfBeacon.x+0.5), int(leftOfBeacon.y+0.5),
    00394         int(rightOfBeacon.x+0.5), int(rightOfBeacon.y+0.5), 1, Drawings::ps_dash, Drawings::red);
    00395   analyzeBeacon(leftOfBeacon, mergedScanLineLength);
    00396 }
    00397
    00398 void GT2005BeaconDetector::analyzeBeacon(const Vector2<double>& left, const double pinkRunWidth)
    00399 {
    00400   if (pinkRunWidth >= 3.0)
    00401   {
    00402     double rotation(imageInf.horizon.direction.angle());
    00403     double ca(cos(rotation));
    00404     double sa(sin(rotation));
    00405     Matrix2x2<double> rotMat(Vector2<double>(ca,sa),Vector2<double>(-sa,ca));
    00406     int flagReliability[4] = {0, 0, 0, 0};
    00407     Flag::FlagType flagFound;
    00408     Vector2<int> topEdge[4];
    00409     Vector2<int> bottomEdge[4];
    00410     int i;
    00411     int totalWeight = 0;
    00412     int highestReliability = 0;
    00413     int mostReliableFlag = -1;
    00414     for (i=0; i<4; i++)
    00415     {
    00416       topEdge[i].x = 0;
    00417       topEdge[i].y = 0;
    00418       bottomEdge[i].x = 0;
    00419       bottomEdge[i].y = 0;
    00420     }
    00421     Vector2<double> currentPos;
    00422     Vector2<double> step;
    00423     int numberOfScanCol;
    00424     if (pinkRunWidth <= 8.0)
    00425     { // only 3 scan lines
    00426       Vector2<double> displacement((pinkRunWidth-3.0)/2.0, 0); //1 pixel inner than theoretical border
    00427       Vector2<double> init(1.0, 0.0);
    00428       displacement = rotMat*displacement;
    00429       init = rotMat*init;
    00430       step = displacement;
    00431       currentPos = left+init;
    00432       numberOfScanCol = 3;
    00433     }
    00434     else
    00435     {
    00436       Vector2<double> displacement((pinkRunWidth-5.0)/3.0, 0); //2 pixels inner than theoretical border
    00437       Vector2<double> init(2.0, 0.0);
    00438       displacement = rotMat*displacement;
    00439       init = rotMat*init;
    00440       step = displacement;
    00441       currentPos = left+init;
    00442       numberOfScanCol = 4;
    00443     }
    00444     for (i=0; i<numberOfScanCol; i++)
    00445     {
    00446       Vector2<int> top, bottom;
    00447       int reliability = scanForBeaconEdges(Vector2<int>(int(currentPos.x+0.5), int(currentPos.y+0.5)),
    00448             pinkRunWidth, flagFound, top, bottom);
    00449       if (reliability != 0)
    00450       {
    00451         flagReliability[(int)flagFound] += reliability;
    00452         topEdge[(int)flagFound] += top;
    00453         bottomEdge[(int)flagFound] += bottom;
    00454       }
    00455       currentPos += step;
    00456     }
    00457     for (i=0; i<4; i++)
    00458     {
    00459       totalWeight += flagReliability[i];
    00460       if (flagReliability[i] > highestReliability)
    00461       {
    00462         highestReliability = flagReliability[i];
    00463         mostReliableFlag = i;
    00464       }
    00465     }
    00466     if (mostReliableFlag != -1)
    00467     {
    00468       double confidence = highestReliability/totalWeight;
    00469       if (confidence >= minFlagConfidence)
    00470       {
    00471         int posX = (int)((topEdge[mostReliableFlag].x + bottomEdge[mostReliableFlag].x)/(2*flagReliability[mostReliableFlag]) + 0.5);
    00472         int posY = (int)((topEdge[mostReliableFlag].y + bottomEdge[mostReliableFlag].y)/(2*flagReliability[mostReliableFlag]) + 0.5);
    00473         Vector2<int> center(posX, posY);
    00474         const Vector2<int> border(0,0);
    00475         Geometry::clipPointInsideRectange(border, imageInf.maxImageCoordinates-border, center);
    00476         DOT(imageProcessor_ball4, center.x, center.y, Drawings::black, Drawings::blue);
    00477         switch ((Flag::FlagType)mostReliableFlag)
    00478         {
    00479           case Flag::pinkAboveYellow:
    00480               flagSpecialist.searchFlags(image, colorTable, cameraMatrix, yellow, true, imageInf.horizon, center.x, center.y);
    00481               break;
    00482           case Flag::pinkAboveSkyblue:
    00483               flagSpecialist.searchFlags(image, colorTable, cameraMatrix, skyblue, true, imageInf.horizon, center.x, center.y);
    00484               break;
    00485           case Flag::yellowAbovePink:
    00486               flagSpecialist.searchFlags(image, colorTable, cameraMatrix, yellow, false, imageInf.horizon, center.x, center.y);
    00487               break;
    00488           case Flag::skyblueAbovePink:
    00489               flagSpecialist.searchFlags(image, colorTable, cameraMatrix, skyblue, false, imageInf.horizon, center.x, center.y);
    00490               break;
    00491         }
    00492       }
    00493     }
    00494   }
    00495 }
    00496
    00497 int GT2005BeaconDetector::scanForBeaconEdges(const Vector2<int>& position, const double pinkRunWidth,
    00498         Flag::FlagType& flagType, Vector2<int>& topEdge, Vector2<int>& bottomEdge)
    00499 {
    00500   Vector2<int> beaconCenter(position);
    00501   Vector2<int> start, end;
    00502   Geometry::Line vertLine;
    00503   vertLine.direction = imageInf.vertLine.direction;
    00504   vertLine.base.x = (double)beaconCenter.x;
    00505   vertLine.base.y = (double)beaconCenter.y;
    00506   Geometry::getIntersectionPointsOfLineAndRectangle(Vector2<int>(1,1), // 1 pixel wide border has to be discarded
    00507               imageInf.maxImageCoordinates-Vector2<int>(1,1), vertLine, start, end); // due to the edge detection convolution mask
    00508   Vector2<int> pos, pos2, pinkBottomEdge, pinkTopEdge, colorBottomEdge, colorTopEdge;
    00509   colorClass bottomColor(noColor),topColor(noColor), baseColor(noColor), dontCare(noColor);
    00510   bool topColorEdgeFound(false), pinkTopFound(false);
    00511   bool unreliable(false); // this flag signals that an unsafe beacon hypothesis has been accepted
    00512   Flag::FlagType beaconType(Flag::pinkAboveYellow);
    00513   int reliability;
    00514   // Scan to bottom of pink beacon part
    00515   if(scanForBeaconPart(beaconCenter, end, pos, pinkBottomEdge, bottomColor))
    00516   {
    00517     if(bottomColor == white || bottomColor == gray)
    00518     {
    00519       // Scan to top of pink beacon part
    00520       if(scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor))
    00521       {
    00522         if(topColor == skyblue)
    00523         {
    00524           beaconType = Flag::skyblueAbovePink;
    00525           topColorEdgeFound = scanForBeaconPart(pos, start, pos2, colorTopEdge, dontCare);
    00526         }
    00527         else if(topColor == yellow)
    00528         {
    00529           beaconType = Flag::yellowAbovePink;
    00530           topColorEdgeFound = scanForBeaconPart(pos, start, pos2, colorTopEdge, dontCare);
    00531         }
    00532         else
    00533         {
    00534           return 0; // unknown color above pink
    00535         }
    00536       }
    00537       else
    00538       {
    00539         return 0; // pink part is at the upper border of the image
    00540       }
    00541     }
    00542     else if((bottomColor == skyblue) || (bottomColor == yellow))
    00543     {// Scan to bottom of color part
    00544       if(scanForBeaconPart(pos, end, pos2, colorBottomEdge, baseColor))
    00545       {
    00546         if(baseColor == white || baseColor == gray)
    00547         {
    00548           if(bottomColor == skyblue)
    00549           {
    00550             beaconType = Flag::pinkAboveSkyblue;
    00551             pinkTopFound = scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor);
    00552           }
    00553           else
    00554           {
    00555             beaconType = Flag::pinkAboveYellow;
    00556             pinkTopFound = scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor);
    00557           }
    00558         }
    00559         else
    00560         { // perhaps we missed the real end of the colored part
    00561           if (scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor))
    00562           { // estimate colorBottomEdge from pinkTopEdge
    00563             Vector2<int> toPink(pinkTopEdge - pinkBottomEdge);
    00564             double distanceToPink = toPink.abs();
    00565             double similarityPink;
    00566             if (distanceToPink > pinkRunWidth)
    00567               similarityPink = (pinkRunWidth/distanceToPink);
    00568             else
    00569               similarityPink = (distanceToPink/pinkRunWidth);
    00570             if (similarityPink > projectionAspectRatio) // make sure that this proposed edge is compatible with the aspect ratio
    00571             {
    00572               colorBottomEdge = pinkBottomEdge + (pinkBottomEdge - pinkTopEdge);
    00573               pinkTopFound = true;
    00574               if(bottomColor == skyblue)
    00575                 beaconType = Flag::pinkAboveSkyblue;
    00576               else
    00577                 beaconType = Flag::pinkAboveYellow;
    00578             }
    00579             else
    00580               return 0;
    00581           }
    00582           else
    00583             return 0; // unable to determine top edge of pink part
    00584         }
    00585       }
    00586       else
    00587       { // we were unable to find the end of the colored part (no contrast?), and reached the border of the image
    00588         // before giving up, let's see if we can at least find the top edge of the pink part
    00589         if (scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor))
    00590         { // estimate colorBottomEdge from pinkTopEdge
    00591           Vector2<int> toPink(pinkTopEdge - pinkBottomEdge);
    00592           double distanceToPink = toPink.abs();
    00593           double similarityPink;
    00594           if (distanceToPink > pinkRunWidth)
    00595             similarityPink = (pinkRunWidth/distanceToPink);
    00596           else
    00597             similarityPink = (distanceToPink/pinkRunWidth);
    00598           if (similarityPink > projectionAspectRatio) // make sure that this proposed edge is compatible with the aspect ratio
    00599           {
    00600             colorBottomEdge = pinkBottomEdge + (pinkBottomEdge - pinkTopEdge);
    00601             pinkTopFound = true;
    00602             if(bottomColor == skyblue)
    00603               beaconType = Flag::pinkAboveSkyblue;
    00604             else
    00605               beaconType = Flag::pinkAboveYellow;
    00606           }
    00607           else
    00608             return 0;
    00609         }
    00610         else
    00611           return 0; // unable to determine top edge of pink part
    00612       }
    00613     }
    00614     else
    00615     {
    00616       return 0; // unknown color below pink
    00617     }
    00618   }
    00619   else // unable to determine bottom edge (no gradient, or border of the image touched)
    00620   { // so. try to scan to top
    00621     if(scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor))
    00622     {
    00623       DOT(imageProcessor_ball4, pinkTopEdge.x, pinkTopEdge.y, Drawings::black, Drawings::pink);
    00624       if(topColor == skyblue)
    00625       {
    00626         beaconType = Flag::skyblueAbovePink;
    00627         topColorEdgeFound = scanForBeaconPart(pos, start, pos2, colorTopEdge, dontCare);
    00628         if (!topColorEdgeFound)
    00629           return 0; // unable to calculate the vertical edges of the landmark
    00630         DOT(imageProcessor_ball4, colorTopEdge.x, colorTopEdge.y, Drawings::black, Drawings::red);
    00631       }
    00632       else if(topColor == yellow)
    00633       {
    00634         beaconType = Flag::yellowAbovePink;
    00635         topColorEdgeFound = scanForBeaconPart(pos, start, pos2, colorTopEdge, dontCare);
    00636         if (!topColorEdgeFound)
    00637           return 0; // unable to calculate the vertical edges of the landmark
    00638         DOT(imageProcessor_ball4, colorTopEdge.x, colorTopEdge.y, Drawings::black, Drawings::red);
    00639       }
    00640       else // last attempt, try to use scan width information and top edge to make an hypothesis on bottom edge,
    00641       { // check if this holds by looking at the color underlying the projected edge (should be yellow or skyblue)
    00642         Vector2<double> projectedLenght(0, pinkRunWidth+3.0); //3 pixel more to make sure we jump over the pink/color edge
    00643         double rotation(imageInf.horizon.direction.angle());
    00644         double ca(cos(rotation));
    00645         double sa(sin(rotation));
    00646         Matrix2x2<double> rotMat(Vector2<double>(ca,sa),Vector2<double>(-sa,ca));
    00647         Vector2<double> topToBottomDisplacement = rotMat*projectedLenght;
    00648         Vector2<int> displacement((int)(topToBottomDisplacement.x+0.5), (int)(topToBottomDisplacement.y+0.5));
    00649         pinkBottomEdge = pinkTopEdge + displacement;
    00650         const Vector2<int> border(0,0);
    00651         Geometry::clipPointInsideRectange(border, imageInf.maxImageCoordinates-border, pinkBottomEdge);
    00652         unsigned char colorY(colorCorrector.correct(pinkBottomEdge.x,pinkBottomEdge.y,0,image.image[pinkBottomEdge.y][0][pinkBottomEdge.x]));
    00653         unsigned char colorU(colorCorrector.correct(pinkBottomEdge.x,pinkBottomEdge.y,1,image.image[pinkBottomEdge.y][1][pinkBottomEdge.x]));
    00654         unsigned char colorV(colorCorrector.correct(pinkBottomEdge.x,pinkBottomEdge.y,2,image.image[pinkBottomEdge.y][2][pinkBottomEdge.x]));
    00655         bottomColor = COLOR_CLASS(colorY,colorU,colorV,colorTable);
    00656         DOT(imageProcessor_ball4, pinkBottomEdge.x, pinkBottomEdge.y, Drawings::gray, ColorClasses::colorClassToDrawingsColor(bottomColor));
    00657         if((bottomColor == skyblue) || (bottomColor == yellow))
    00658         {
    00659           colorBottomEdge = pinkBottomEdge + displacement;
    00660           Geometry::clipPointInsideRectange(border, imageInf.maxImageCoordinates-border, colorBottomEdge);
    00661           colorY = colorCorrector.correct(colorBottomEdge.x,colorBottomEdge.y,0,image.image[colorBottomEdge.y][0][colorBottomEdge.x]);
    00662           colorU = colorCorrector.correct(colorBottomEdge.x,colorBottomEdge.y,1,image.image[colorBottomEdge.y][1][colorBottomEdge.x]);
    00663           colorV = colorCorrector.correct(colorBottomEdge.x,colorBottomEdge.y,2,image.image[colorBottomEdge.y][2][colorBottomEdge.x]);
    00664           baseColor = COLOR_CLASS(colorY,colorU,colorV,colorTable);
    00665           DOT(imageProcessor_ball4, colorBottomEdge.x, colorBottomEdge.y, Drawings::white, ColorClasses::colorClassToDrawingsColor(baseColor));
    00666           if (baseColor == white || baseColor == gray)
    00667           {
    00668             if (bottomColor == skyblue)
    00669             {
    00670               beaconType = Flag::pinkAboveSkyblue;
    00671               pinkTopFound = true;
    00672               unreliable = true;
    00673             }
    00674             else if (bottomColor == yellow)
    00675             {
    00676               beaconType = Flag::pinkAboveYellow;
    00677               pinkTopFound = true;
    00678               unreliable = true;
    00679             }
    00680             else
    00681               return 0;
    00682           }
    00683           else if (baseColor == pink) // we could have missed the middle edge of a landmark whose lower part is pink
    00684           {
    00685             if (bottomColor == skyblue)
    00686             {
    00687               beaconType = Flag::skyblueAbovePink;
    00688               pinkTopEdge = pinkBottomEdge; // we have to swap up and down, because it seems
    00689               pinkBottomEdge = colorBottomEdge; //that the assumption pinkAboveSkyblue was wrong
    00690               topColor = skyblue;
    00691               bottomColor = pink;
    00692               unreliable = true;
    00693             }
    00694             else if (bottomColor == yellow)
    00695             {
    00696               beaconType = Flag::yellowAbovePink;
    00697               pinkTopEdge = pinkBottomEdge; // we have to swap up and down, because it seems
    00698               pinkBottomEdge = colorBottomEdge; //that the assumption pinkAboveSkyblue was wrong
    00699               topColor = yellow;
    00700               bottomColor = pink;
    00701               unreliable = true;
    00702             }
    00703             else
    00704               return 0;
    00705           }
    00706           else
    00707             return 0;
    00708         }
    00709         else
    00710         {
    00711           return 0; // unknown color above pink
    00712         }
    00713       }
    00714     }
    00715     else
    00716     {
    00717       return 0;
    00718     }
    00719   }
    00720
    00721   //~ DOT(imageProcessor_ball4, pos.x, pos.y, Drawings::white, Drawings::black);
    00722   //~ DOT(imageProcessor_ball4, pos2.x, pos2.y, Drawings::black, Drawings::gray);
    00723
    00724   if(beaconType == Flag::pinkAboveYellow || beaconType == Flag::pinkAboveSkyblue)
    00725   {
    00726     if(pinkTopFound)
    00727     {
    00728       //~ DOT(imageProcessor_ball4, pinkBottomEdge.x, pinkBottomEdge.y, Drawings::black, Drawings::green);
    00729       //~ DOT(imageProcessor_ball4, pinkTopEdge.x, pinkTopEdge.y, Drawings::white, Drawings::black);
    00730       //~ DOT(imageProcessor_ball4, colorBottomEdge.x, colorBottomEdge.y, Drawings::black, Drawings::white);
    00731       Vector2<int> topPosition(pinkBottomEdge);
    00732       Vector2<int> toColor(pinkBottomEdge - colorBottomEdge);
    00733       Vector2<int> toPink(pinkTopEdge - pinkBottomEdge);
    00734       double distanceToColor = toColor.abs();
    00735       double distanceToPink = toPink.abs();
    00736       double similarityColor, similarityPink;
    00737       if (distanceToColor > pinkRunWidth)
    00738         similarityColor = (pinkRunWidth/distanceToColor);
    00739       else
    00740         similarityColor = (distanceToColor/pinkRunWidth);
    00741       if (distanceToPink > pinkRunWidth)
    00742         similarityPink = (pinkRunWidth/distanceToPink);
    00743       else
    00744         similarityPink = (distanceToPink/pinkRunWidth);
    00745       if (similarityColor > similarityPink)
    00746         topPosition += toColor;
    00747       else
    00748         topPosition += toPink;
    00749       //~ DOT(imageProcessor_ball4, topPosition.x, topPosition.y, Drawings::black, Drawings::gray);
    00750       //~ DOT(imageProcessor_ball4, pinkBottomEdge.x, topPosition.y+(pinkBottomEdge.y-topPosition.y)/2, Drawings::black, Drawings::blue);
    00751       if (unreliable)
    00752         reliability = lowReliability;
    00753       else
    00754         reliability = highReliability;
    00755       flagType = beaconType;
    00756       topEdge = topPosition*reliability;
    00757       bottomEdge = pinkBottomEdge*reliability;
    00758       return reliability;
    00759     }
    00760     else
    00761     {
    00762       // compute a theoretical position of the pink top edge:
    00763       Vector2<int> topPosition(pinkBottomEdge);
    00764       Vector2<int> relVec(pinkBottomEdge - colorBottomEdge);
    00765       topPosition += relVec;
    00766       //~ DOT(imageProcessor_ball4, pinkBottomEdge.x, topPosition.y+(pinkBottomEdge.y-topPosition.y)/2, Drawings::black, Drawings::blue);
    00767       //~ DOT(imageProcessor_ball4, pinkBottomEdge.x, pinkBottomEdge.y, Drawings::black, Drawings::red);
    00768       //~ DOT(imageProcessor_ball4, topPosition.x, topPosition.y, Drawings::white, Drawings::black);
    00769       reliability = mediumReliability;
    00770       flagType = beaconType;
    00771       topEdge = topPosition*reliability;
    00772       bottomEdge = pinkBottomEdge*reliability;
    00773       return reliability;
    00774     }
    00775   }
    00776   else
    00777   { // pink is bottom
    00778     if (bottomColor == noColor && topColorEdgeFound) // no pink bottom edge found
    00779     { //theoretical position will be calculated from color top edge
    00780       Vector2<int> bottomPosition(pinkTopEdge);
    00781       Vector2<int> relVec(pinkTopEdge - colorTopEdge);
    00782       bottomPosition += relVec;
    00783       flagSpecialist.searchFlags(image, colorTable, cameraMatrix, topColor, false,
    00784           imageInf.horizon, (int)((pinkTopEdge.x+bottomPosition.x+1.0)/2), (int)((pinkTopEdge.y+bottomPosition.y+1.0)/2));
    00785       DOT(imageProcessor_ball4, pinkTopEdge.x, pinkTopEdge.y, Drawings::white, Drawings::black);
    00786       DOT(imageProcessor_ball4, bottomPosition.x, bottomPosition.y, Drawings::black, Drawings::yellow);
    00787       DOT(imageProcessor_ball4, (int)((pinkTopEdge.x+bottomPosition.x+1.0)/2), (int)((pinkTopEdge.y+bottomPosition.y+1.0)/2), Drawings::black, Drawings::blue);
    00788       reliability = mediumReliability;
    00789       flagType = beaconType;
    00790       topEdge = pinkTopEdge*reliability;
    00791       bottomEdge = bottomPosition*reliability;
    00792       return reliability;
    00793     }
    00794     else{
    00795       Vector2<int> bottomPosition;
    00796       if (topColorEdgeFound) //color edge information used just for redundancy check
    00797       {
    00798         bottomPosition = pinkTopEdge;
    00799         Vector2<int> toColor(pinkTopEdge - colorTopEdge);
    00800         Vector2<int> toPink(pinkBottomEdge - pinkTopEdge);
    00801         double distanceToColor = toColor.abs();
    00802         double distanceToPink = toPink.abs();
    00803         double similarityColor, similarityPink;
    00804         if (distanceToColor > pinkRunWidth)
    00805           similarityColor = (pinkRunWidth/distanceToColor);
    00806         else
    00807           similarityColor = (distanceToColor/pinkRunWidth);
    00808         if (distanceToPink > pinkRunWidth)
    00809           similarityPink = (pinkRunWidth/distanceToPink);
    00810         else
    00811           similarityPink = (distanceToPink/pinkRunWidth);
    00812         if (similarityColor > similarityPink)
    00813           bottomPosition += toColor;
    00814         else
    00815           bottomPosition += toPink;
    00816         if (unreliable)
    00817           reliability = lowReliability;
    00818         else
    00819           reliability = highReliability;
    00820         flagType = beaconType;
    00821         topEdge = pinkTopEdge*reliability;
    00822         bottomEdge = bottomPosition*reliability;
    00823         return reliability;
    00824       }
    00825       else
    00826       {
    00827         if (unreliable)
    00828           reliability = lowReliability;
    00829         else
    00830           reliability = mediumReliability;
    00831         flagType = beaconType;
    00832         topEdge = pinkTopEdge*reliability;
    00833         bottomEdge = pinkBottomEdge*reliability;
    00834         return reliability;
    00835       }
    00836     }
    00837   }
    00838   return true;
    00839 }
    00840
    00841
    00842 bool GT2005BeaconDetector::scanForBeaconPart(const Vector2<int>& start, const Vector2<int>& end,
    00843                                              Vector2<int>& position, Vector2<int>& edge, colorClass& color)
    00844 {
    00845   // DOT(imageProcessor_ball4, start.x, start.y, Drawings::blue, Drawings::orange);
    00846   // DOT(imageProcessor_ball4, end.x, end.y, Drawings::yellow, Drawings::white);
    00847   BresenhamLineScan bresenham(start, end);
    00848   bresenham.init();
    00849   Vector2<int> pixel(start);
    00850   for(int i=0; i<bresenham.numberOfPixels; i++)
    00851   {
    00852     // bool edgeY = edgeDetectionY.isEdgePoint(image, pixel.x, pixel.y, SUSANEdgeDetectionLite::componentA);
    00853     bool edgeU = edgeDetectionU.isEdgePoint(image, pixel.x, pixel.y, SUSANEdgeDetectionLite::componentB);
    00854     bool edgeV = edgeDetectionV.isEdgePoint(image, pixel.x, pixel.y, SUSANEdgeDetectionLite::componentC);
    00855     DOT(imageProcessor_ball4, pixel.x, pixel.y, Drawings::black, Drawings::orange);
    00856
    00857     if (edgeU || edgeV)
    00858     {
    00859       position = pixel;
    00860       edge = pixel;
    00861       color = noColor;
    00862
    00863       if (edge.x < 0)
    00864         edge.x = 0;
    00865
    00866       if (edge.y < 0)
    00867         edge.y = 0;
    00868
    00869       unsigned char colorY(colorCorrector.correct(edge.x,edge.y,0,image.image[edge.y][0][edge.x]));
    00870       unsigned char colorU(colorCorrector.correct(edge.x,edge.y,1,image.image[edge.y][1][edge.x]));
    00871       unsigned char colorV(colorCorrector.correct(edge.x,edge.y,2,image.image[edge.y][2][edge.x]));
    00872       colorClass edgeColor = COLOR_CLASS(colorY,colorU,colorV,colorTable);
    00873       color = edgeColor;
    00874       int counter;
    00875       for (counter = 0; counter < edgeScanDepth &&
    00876             ((color == noColor)||(color==edgeColor)||(color==orange)||edgeU||edgeV); counter++)
    00877       {
    00878         bresenham.getNext(position);
    00879
    00880     if (position.x < 0)
    00881           position.x = 0;
    00882         if (position.y < 0)
    00883           position.y = 0;
    00884         unsigned char colorY(colorCorrector.correct(position.x,position.y,0,image.image[position.y][0][position.x]));
    00885         unsigned char colorU(colorCorrector.correct(position.x,position.y,1,image.image[position.y][1][position.x]));
    00886         unsigned char colorV(colorCorrector.correct(position.x,position.y,2,image.image[position.y][2][position.x]));
    00887         color = COLOR_CLASS(colorY,colorU,colorV,colorTable);
    00888         edgeU = edgeDetectionU.isEdgePoint(image, position.x, position.y, SUSANEdgeDetectionLite::componentB);
    00889         edgeV = edgeDetectionV.isEdgePoint(image, position.x, position.y, SUSANEdgeDetectionLite::componentC);
    00890       }
    00891       if (counter != edgeScanDepth)
    00892         return true;
    00893     }
    00894     //Compute next position according to Bresenham algorithm
    00895     bresenham.getNext(pixel);
    00896   }
    00897   return 0;
    00898 }
    00899
    00900 void GT2005BeaconDetector::analyzeColorTable()
    00901 {
    00902   Vector3<int> nearPoint,farPoint;
    00903 #ifdef CT32K_LAYOUT
    00904   ((ColorTable32K&) colorTable).getBoxAroundColorClass(pink,nearPoint,farPoint);
    00905 #else
    00906   ((ColorTable64&) colorTable).getBoxAroundColorClass(pink,nearPoint,farPoint);
    00907 #endif
    00908   minPinkUValue = nearPoint.y;
    00909 }