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

# Modules/ImageProcessor/SlamImageProcessor/SlamBeaconDetector.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file SlamBeaconDetector.cpp
    00003 *
    00004 * Implementation of class SlamBeaconDetector.
    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 "SlamImageProcessorTools.h"
    00024 #include "SlamBeaconDetector.h"
    00025
    00026 #include "Representations/Perception/SlamPercept.h"
    00027
    00028 #define slamPercept (*(SlamPercept *)specialPercept.slamData)
    00029
    00030 #define MIN_SIMILARITY 0.65
    00031
    00032 const int Y(0),
    00033           U(cameraResolutionWidth_ERS7), /**< Relative offset of U component. */
    00034           V(2 * cameraResolutionWidth_ERS7); /**< Relative offset of V component. */
    00035
    00036
    00037 SlamBeaconDetector::SlamBeaconDetector(const Image& image,
    00038                        const CameraMatrix& cameraMatrix,
    00039                        const CameraMatrix& prevCameraMatrix,
    00040                        const ImageInfo& imageInf,
    00041                        const ColorTable& colorTable,
    00042                        const ColorCorrector& colorCorrector,
    00043              const RobotPose& robotPose,
    00044                        LandmarksPercept& landmarksPercept,
    00045              SpecialPercept& specialPercept):
    00046                        image(image), cameraMatrix(cameraMatrix),
    00047                        prevCameraMatrix(prevCameraMatrix),
    00048                        imageInf(imageInf), colorTable(colorTable),
    00049                        colorCorrector(colorCorrector),
    00050              robotPose(robotPose),
    00051                        landmarksPercept(landmarksPercept),
    00052              specialPercept(specialPercept),
    00053                        flagSpecialist(colorCorrector),
    00054                        horizontalBaseOffset(1.6), // was 1.5
    00055                        numOfHorizontalScanLineAbove(10),
    00056 //                       numOfHorizontalScanLineBelow(3),
    00057                        numOfHorizontalScanLineBelow(10),
    00058                        horizontalOffsetModifier(0.9),  // was 0.7
    00059                        clusteringDistanceTolerance(6),
    00060                        minPinkRunLength(2),
    00061                        clusteringAspectRatio(1.5),
    00062                        projectionAspectRatio(0.4),
    00063                        minFlagConfidence(0.5),
    00064                        edgeScanDepth(6),
    00065                        edgeDetectionU(edgeThresholdU),
    00066                        edgeDetectionV(edgeThresholdV)
    00067 {
    00068   frameCount = 0;
    00069   numFlags = 0;
    00070 }
    00071
    00072
    00073 void SlamBeaconDetector::execute()
    00074 {
    00075   NDECLARE_DEBUGDRAWING("slam-ip:new-flag", "drawingOnImage", "New found flags in the image");
    00076
    00077   numOfClusteredBeaconCandidates = 0;
    00078
    00079   direction = 0;
    00080   directionCount = 0;
    00081
    00082   if (slamPercept.slamState == SlamPercept::LearnNewFlags)
    00083   {
    00084     CIRCLE(percepts_robot, robotPose.translation.x, robotPose.translation.y, 500, 50, Drawings::ps_solid, blue);
    00085   }
    00086   else
    00087   {
    00088     CIRCLE(percepts_robot, robotPose.translation.x, robotPose.translation.y, 500, 50, Drawings::ps_solid, red);
    00089   }
    00090
    00091   frameCount++;
    00092   flagSpecialist.init(image);
    00093   numOfBeaconCandidates = 0;
    00094   double dist(horizontalBaseOffset);
    00095   Geometry::Line scanLine;
    00096   scanLine.direction = imageInf.horizon.direction;
    00097   scanLine.base = imageInf.horizon.base;
    00098   int i;
    00099   for (i = 0; i < numFlags; i++)
    00100   {
    00101     double flagDirection = specialFlags[i].direction / ((double)specialFlags[i].directionCount);
    00102
    00103     LINE (percepts_robot, robotPose.translation.x, robotPose.translation.y, robotPose.translation.x + 500.0 * cos (flagDirection), robotPose.translation.y + 500.0 * sin (flagDirection), 30, Drawings::ps_solid, Drawings::red);
    00104   };
    00105
    00106   for(i=0; i < numOfHorizontalScanLineBelow; i++)
    00107   {
    00108     scanLine.base += imageInf.vertLine.direction*dist;
    00109     dist += horizontalOffsetModifier;
    00110     Vector2<int> startPoint, endPoint;
    00111     if(Geometry::getIntersectionPointsOfLineAndRectangle(
    00112        Vector2<int>(0,0), imageInf.maxImageCoordinates, scanLine, startPoint, endPoint))
    00113     {
    00114       LINE(imageProcessor_general, startPoint.x, startPoint.y, endPoint.x, endPoint.y, 1, Drawings::ps_dash, Drawings::gray);
    00115       scanForPink(startPoint, endPoint);
    00116     }
    00117     else
    00118     {
    00119       break;
    00120     }
    00121   }
    00122   dist  = horizontalBaseOffset;
    00123   scanLine.base = imageInf.horizon.base;
    00124   for(i=0; i < numOfHorizontalScanLineAbove; i++)
    00125   {
    00126     scanLine.base -= imageInf.vertLine.direction*dist;
    00127     dist += horizontalOffsetModifier;
    00128     Vector2<int> startPoint, endPoint;
    00129     if(Geometry::getIntersectionPointsOfLineAndRectangle(
    00130        Vector2<int>(0,0), imageInf.maxImageCoordinates, scanLine, startPoint, endPoint))
    00131     {
    00132       LINE(imageProcessor_general, startPoint.x, startPoint.y, endPoint.x, endPoint.y, 1, Drawings::ps_dash, Drawings::yellow);
    00133       scanForPink(startPoint, endPoint);
    00134     }
    00135     else
    00136     {
    00137       break;
    00138     }
    00139   }
    00140   if(numOfBeaconCandidates)
    00141   {
    00142     clusterPinkBeaconParts();
    00143   }
    00144   flagSpecialist.getFlagPercept(cameraMatrix, prevCameraMatrix, image.cameraInfo, imageInf.horizon, landmarksPercept);
    00145
    00146   if (directionCount > 0)
    00147   {
    00148   direction /= ((double)directionCount);
    00149
    00150   slamPercept.viewAngle = direction;
    00151   slamPercept.viewAngleValid = true;
    00152
    00153   ARROW (percepts_robot, robotPose.translation.x, robotPose.translation.y, 750 * cos (direction) + robotPose.translation.x, 750 * sin (direction) + robotPose.translation.y, 50, Drawings::ps_solid, Drawings::red);
    00154   }
    00155   else
    00156   slamPercept.viewAngleValid = false;
    00157 }
    00158
    00159
    00160 void SlamBeaconDetector::scanForPink(const Vector2<int>& start,
    00161                                        const Vector2<int>& end)
    00162 {
    00163   BresenhamLineScan bresenham(start, end);
    00164   bresenham.init();
    00165   Vector2<int> pixel(start), lastPixel(start);
    00166   int Y(0),U(0),V(0),lastY(0),lastU(0),lastV(0);
    00167   Run currentRun;
    00168   bool isPinkRun(false);
    00169   bool currentPixelPink(false);
    00170   for(int i=0; i<bresenham.numberOfPixels; i++)
    00171   {
    00172     U = image.image[pixel.y][1][pixel.x];
    00173     U = colorCorrector.correct(pixel.x, pixel.y, 1, U);
    00174     if(U > minPinkUValue)
    00175     {
    00176       Y = image.image[pixel.y][0][pixel.x];
    00177       Y = colorCorrector.correct(pixel.x, pixel.y, 0, Y);
    00178       V = image.image[pixel.y][2][pixel.x];
    00179       V = colorCorrector.correct(pixel.x, pixel.y, 2, V);
    00180       currentPixelPink = (COLOR_CLASS(Y,U,V) == pink);
    00181     }
    00182     else
    00183     {
    00184       currentPixelPink = false;
    00185     }
    00186     if(!isPinkRun)
    00187     {
    00188       if(currentPixelPink)
    00189       {
    00190         currentRun.scanLineStart = start;
    00191         currentRun.start = pixel;
    00192         currentRun.length = 1;
    00193         isPinkRun = true;
    00194       }
    00195     }
    00196     else //if(isPinkRun)
    00197     {
    00198       if(currentPixelPink)// || (COLOR_COMPONENT_DIST(U,lastU) < params.maxPinkUContrast))
    00199       {
    00200         currentRun.length++;
    00201         lastY = Y;
    00202         lastU = U;
    00203         lastV = V;
    00204       }
    00205       else
    00206       {
    00207         isPinkRun = false;
    00208         if(currentRun.length >= minPinkRunLength)
    00209         {
    00210           currentRun.end = lastPixel;
    00211           if(!addCandidate(currentRun)) break; //List is full, stop searching
    00212           LINE(imageProcessor_general, currentRun.start.x, currentRun.start.y,
    00213                currentRun.end.x, currentRun.end.y, 1, Drawings::ps_solid, Drawings::pink);
    00214         }
    00215       }
    00216     }
    00217     //Compute next position according to Bresenham algorithm
    00218     lastPixel = pixel;
    00219     bresenham.getNext(pixel);
    00220   }
    00221   //Finish the last Run
    00222   if(isPinkRun)
    00223   {
    00224     currentRun.end = pixel;
    00225     addCandidate(currentRun);
    00226     LINE(imageProcessor_general, currentRun.start.x, currentRun.start.y,
    00227                currentRun.end.x, currentRun.end.y, 1, Drawings::ps_solid, Drawings::pink);
    00228   }
    00229 }
    00230
    00231
    00232 bool SlamBeaconDetector::addCandidate(const Run& pinkRun)
    00233 {
    00234   bool returnValue(numOfBeaconCandidates<MAX_NUMBER_OF_PINK_RUNS);
    00235   if(returnValue)
    00236   {
    00237     if((numOfBeaconCandidates>0) &&
    00238        (pinkRun.scanLineStart == beaconCandidates[numOfBeaconCandidates-1].scanLineStart) &&
    00239        ((beaconCandidates[numOfBeaconCandidates-1].end - pinkRun.start).abs()<4))
    00240     {
    00241       beaconCandidates[numOfBeaconCandidates-1].end = pinkRun.end;
    00242     }
    00243     else
    00244     {
    00245       beaconCandidates[numOfBeaconCandidates++] = pinkRun;
    00246     }
    00247   }
    00248   return returnValue;
    00249 }
    00250
    00251
    00252 void SlamBeaconDetector::clusterPinkBeaconParts()
    00253 {
    00254   //Transform pink runs
    00255   double rotation(imageInf.horizon.direction.angle());
    00256   double ca(cos(-rotation));
    00257   double sa(sin(-rotation));
    00258   Matrix2x2<double> rotMat(Vector2<double>(ca,sa),Vector2<double>(-sa,ca));
    00259   Matrix2x2<double> invRotMat = rotMat.transpose(); //=rotMat.invert(), orthonormal matrix
    00260   for(int i=0; i<numOfBeaconCandidates; i++)
    00261   {
    00262     transformedCandidates[i].transform(beaconCandidates[i], rotMat, i);
    00263   }
    00264   //Sort by starting position from left to right
    00265   for(int j=0; j<numOfBeaconCandidates; j++)
    00266   {
    00267     int leftest(j);
    00268     for(int k=j; k<numOfBeaconCandidates; k++)
    00269     {
    00270       if(transformedCandidates[k].start.x<transformedCandidates[leftest].start.x)
    00271       {
    00272         leftest = k;
    00273       }
    00274     }
    00275     if(leftest != j)
    00276     {
    00277       TransformedRun help(transformedCandidates[j]);
    00278       transformedCandidates[j] = transformedCandidates[leftest];
    00279       transformedCandidates[leftest] = help;
    00280     }
    00281   }
    00282   //Find overlapping runs
    00283   int beginOfBeacon(0), endOfBeacon(0);
    00284   double xRight(transformedCandidates[0].end.x);
    00285   double xLeft(transformedCandidates[0].start.x);
    00286   double yRight(transformedCandidates[0].end.y);
    00287   double yLeft(transformedCandidates[0].start.y);
    00288   double runLength(xRight-xLeft);
    00289   double yMax, yMin;
    00290   double originMinY, originMaxY;
    00291   if (yRight > yLeft)
    00292   {
    00293     yMax = yRight;
    00294     yMin = yLeft;
    00295   }
    00296   else
    00297   {
    00298     yMax = yLeft;
    00299     yMin = yRight;
    00300   }
    00301   originMinY = beaconCandidates[0].scanLineStart.y;
    00302   originMaxY = beaconCandidates[0].scanLineStart.y;
    00303   for(int l=1; l<numOfBeaconCandidates; l++)
    00304   {
    00305     if(transformedCandidates[l].start.x < xRight  + (double)clusteringDistanceTolerance)
    00306     {// within merging distance
    00307       if(transformedCandidates[l].end.x > xRight)
    00308       { // new piece could extend the run
    00309         double candidateLength = transformedCandidates[l].end.x-xLeft;
    00310         bool newRunHigher = (transformedCandidates[l].end.y-yLeft > 0.0);
    00311         double candidateHeight = newRunHigher ? (transformedCandidates[l].end.y-yLeft) : (yLeft-transformedCandidates[l].end.y);
    00312         if (candidateHeight < candidateLength*clusteringAspectRatio) // the proportions of the landmark are consistent (1:1 ideally)
    00313         {
    00314           xRight = transformedCandidates[l].end.x;
    00315           yRight = transformedCandidates[l].end.y;
    00316           endOfBeacon = l;
    00317           if (transformedCandidates[l].end.y-yMax > 0.0)
    00318           {
    00319             yMax = transformedCandidates[l].end.y;
    00320             originMaxY = beaconCandidates[l].scanLineStart.y;
    00321           }
    00322           else if (yMin - transformedCandidates[l].end.y > 0.0)
    00323           {
    00324             yMin = transformedCandidates[l].end.y;
    00325             originMinY = beaconCandidates[l].scanLineStart.y;
    00326           }
    00327         }
    00328         else
    00329         { // we have to choose one run, the other is perhaps a false positive
    00330           double newLength = transformedCandidates[l].end.x - transformedCandidates[l].start.x;
    00331           if (newLength > runLength) // the new run wins if longest
    00332           {
    00333             xRight = transformedCandidates[l].end.x;
    00334             xLeft = transformedCandidates[l].start.x;
    00335             yRight = transformedCandidates[l].end.y;
    00336             yLeft = transformedCandidates[l].start.y;
    00337             endOfBeacon = l;
    00338             beginOfBeacon = l;
    00339             runLength = newLength;
    00340             if (yRight > yLeft)
    00341             {
    00342               yMax = yRight;
    00343               yMin = yLeft;
    00344             }
    00345             else
    00346             {
    00347               yMax = yLeft;
    00348               yMin = yRight;
    00349             }
    00350             originMinY = beaconCandidates[l].scanLineStart.y;
    00351             originMaxY = beaconCandidates[l].scanLineStart.y;
    00352           }
    00353         }
    00354       }
    00355       else
    00356       { // the new run is horizontally contained
    00357         double newRunHeight = (transformedCandidates[l].end.y + transformedCandidates[l].start.y)/2.0;
    00358         bool candidateMax = (newRunHeight-yMax > 0.0);
    00359         bool candidateMin = (yMin - newRunHeight > 0.0);
    00360         if (candidateMax)
    00361         {
    00362           double candidateHeight = newRunHeight - yMin;
    00363           double candidateLength = xRight-xLeft;
    00364           if (candidateHeight < candidateLength*clusteringAspectRatio) // the proportions of the landmark are consistent (1:1 ideally)
    00365           {
    00366             yMax = newRunHeight;
    00367             originMaxY = beaconCandidates[l].scanLineStart.y;
    00368           }
    00369         }
    00370         else if (candidateMin)
    00371         {
    00372           double candidateHeight = yMax - newRunHeight;
    00373           double candidateLength = xRight-xLeft;
    00374           if (candidateHeight < candidateLength*clusteringAspectRatio) // the proportions of the landmark are consistent (1:1 ideally)
    00375           {
    00376             yMin = newRunHeight;
    00377             originMinY = beaconCandidates[l].scanLineStart.y;
    00378           }
    00379         }
    00380       }
    00381     }
    00382     else
    00383     {
    00384       double originX = (beaconCandidates[beginOfBeacon].scanLineStart.x + beaconCandidates[endOfBeacon].scanLineStart.x)/2.0;
    00385       //~ double originY = (beaconCandidates[beginOfBeacon].scanLineStart.y + beaconCandidates[endOfBeacon].scanLineStart.y)/2.0;
    00386       double originY = (originMinY + originMaxY)/2;
    00387       double mergedScanLineLength = xRight-xLeft;
    00388       Vector2<double> leftOfBeacon, rightOfBeacon;
    00389       leftOfBeacon.x = xLeft - originX;
    00390       rightOfBeacon.x = xRight - originX;
    00391       leftOfBeacon.y = (yMax+yMin)/2.0 - originY;
    00392       rightOfBeacon.y = leftOfBeacon.y;
    00393       leftOfBeacon = (invRotMat*leftOfBeacon);
    00394       rightOfBeacon = (invRotMat*rightOfBeacon);
    00395       leftOfBeacon.x += originX;
    00396       leftOfBeacon.y += originY;
    00397       rightOfBeacon.x += originX;
    00398       rightOfBeacon.y += originY;
    00399       LINE(imageProcessor_general, int(leftOfBeacon.x+0.5), int(leftOfBeacon.y+0.5),
    00400             int(rightOfBeacon.x+0.5), int(rightOfBeacon.y+0.5), 1, Drawings::ps_dash, Drawings::red);
    00401       analyzeBeacon(leftOfBeacon, Vector2<int> ((int)((leftOfBeacon.x + rightOfBeacon.x) / 2), (int)((leftOfBeacon.y + rightOfBeacon.y) / 2)), mergedScanLineLength);
    00402       beginOfBeacon = l;
    00403       endOfBeacon = l;
    00404       xRight = transformedCandidates[l].end.x;
    00405       yRight = transformedCandidates[l].end.y;
    00406       xLeft = transformedCandidates[l].start.x;
    00407       yLeft = transformedCandidates[l].start.y;
    00408       if (yRight > yLeft)
    00409       {
    00410         yMax = yRight;
    00411         yMin = yLeft;
    00412       }
    00413       else
    00414       {
    00415         yMax = yLeft;
    00416         yMin = yRight;
    00417       }
    00418       originMinY = beaconCandidates[l].scanLineStart.y;
    00419       originMaxY = beaconCandidates[l].scanLineStart.y;
    00420     }
    00421   }
    00422   double originX = (beaconCandidates[beginOfBeacon].scanLineStart.x + beaconCandidates[endOfBeacon].scanLineStart.x)/2.0;
    00423   //~ double originY = (beaconCandidates[beginOfBeacon].scanLineStart.y + beaconCandidates[endOfBeacon].scanLineStart.y)/2.0;
    00424   double originY = (originMinY + originMaxY)/2;
    00425   double mergedScanLineLength = xRight-xLeft;
    00426   Vector2<double> leftOfBeacon, rightOfBeacon;
    00427   leftOfBeacon.x = xLeft - originX;
    00428   rightOfBeacon.x = xRight - originX;
    00429   leftOfBeacon.y = (yMax+yMin)/2.0 - originY;
    00430   rightOfBeacon.y = leftOfBeacon.y;
    00431   leftOfBeacon = (invRotMat*leftOfBeacon);
    00432   rightOfBeacon = (invRotMat*rightOfBeacon);
    00433   leftOfBeacon.x += originX;
    00434   leftOfBeacon.y += originY;
    00435   rightOfBeacon.x += originX;
    00436   rightOfBeacon.y += originY;
    00437   LINE(imageProcessor_general, int(leftOfBeacon.x+0.5), int(leftOfBeacon.y+0.5),
    00438         int(rightOfBeacon.x+0.5), int(rightOfBeacon.y+0.5), 1, Drawings::ps_dash, Drawings::red);
    00439   analyzeBeacon(leftOfBeacon, Vector2<int> ((int)((leftOfBeacon.x + rightOfBeacon.x) / 2), (int)((leftOfBeacon.y + rightOfBeacon.y) / 2)), mergedScanLineLength);
    00440 }
    00441
    00442 double SlamBeaconDetector::FindSimilarFlag(SpecialFlag &specialFlag)
    00443 {
    00444   double similarity = 0;
    00445   double maxSim = 0;
    00446   int maxSimFlag = -1;
    00447
    00448   for (int i = 0; i < numFlags; i++)
    00449   {
    00450     similarity = 0;
    00451
    00452     for (int sector = 0; sector < NUM_SECTORS; sector++)
    00453     {
    00454       if (specialFlags[i].sectors[sector] == specialFlag.sectors[sector])
    00455         similarity++;
    00456     }
    00457
    00458     similarity /= ((double)NUM_SECTORS);
    00459
    00460     if (similarity > maxSim)
    00461     {
    00462       maxSim = similarity;
    00463       maxSimFlag = i;
    00464     };
    00465   }
    00466
    00467   if (maxSimFlag != -1)
    00468   {
    00469     specialFlag = specialFlags[maxSimFlag];
    00470   };
    00471
    00472   return maxSim;
    00473 };
    00474
    00475 void SlamBeaconDetector::findSpecial(const Vector2<int>& center, const double pinkRunWidth)
    00476 {
    00477   if ((center.x < 20) || (center.y < 20) || (image.cameraInfo.resolutionWidth - center.x < 20) || (image.cameraInfo.resolutionHeight - center.y < 20))
    00478   {
    00479 //    OUTPUT (idText, text, "drop flag, too near to image border");
    00480
    00481     return;
    00482   };
    00483
    00484   double rotation(imageInf.horizon.direction.angle());
    00485
    00486   //first scan for special landmarks
    00487   SpecialFlag specialFlag;
    00488
    00489   //scan sectors
    00490   LINE (imageProcessor_general, center.x, center.y, center.x + sin (rotation) * 50, center.y + cos (rotation) * 50, 1, Drawings::ps_solid, Drawings::blue);
    00491
    00492   for (int sector = 0; sector < NUM_SECTORS; sector++)
    00493   {
    00494     double scan_direction = rotation - pi_2 + ((double)sector) * (pi2 / ((double) NUM_SECTORS));
    00495
    00496     Vector2<int> endPoint = Vector2<int> (center.x + (int)(sin (scan_direction - pi_2) * 2 * pinkRunWidth), center.y + (int)(cos (scan_direction - pi_2) * 2 * pinkRunWidth));
    00497
    00498     if (endPoint.x < 0)
    00499       endPoint.x = 0;
    00500
    00501     if (endPoint.x >= image.cameraInfo.resolutionWidth)
    00502       endPoint.x = image.cameraInfo.resolutionWidth - 1;
    00503
    00504     if (endPoint.y < 0)
    00505       endPoint.y = 0;
    00506
    00507     if (endPoint.y >= image.cameraInfo.resolutionHeight)
    00508       endPoint.y = image.cameraInfo.resolutionHeight - 1;
    00509
    00510     BresenhamLineScan bresenham (center, endPoint);
    00511
    00512     Vector2<int> pixel = center;
    00513
    00514     bresenham.init ();
    00515
    00516     char colors[(int)numOfColors];
    00517
    00518     memset (colors, 0x00, numOfColors);
    00519
    00520     int count = 0;
    00521     bool finished = false;
    00522
    00523     colorClass currentColor = noColor;
    00524     int currentColorCount = 0;
    00525
    00526 //    for (count = 0; count < bresenham.numberOfPixels; count++)
    00527     while (!finished)
    00528     {
    00529       bresenham.getNext (pixel);
    00530
    00531       if ((pixel.x < 0) | (pixel.y < 0) | (pixel.x >= image.cameraInfo.resolutionWidth) | (pixel.y >= image.cameraInfo.resolutionHeight) | (count++ > 25))
    00532       {
    00533         finished = true;
    00534
    00535         continue;
    00536       }
    00537
    00538       unsigned char colorY(colorCorrector.correct(pixel.x,pixel.y,0,image.image[pixel.y][0][pixel.x]));
    00539       unsigned char colorU(colorCorrector.correct(pixel.x,pixel.y,1,image.image[pixel.y][1][pixel.x]));
    00540       unsigned char colorV(colorCorrector.correct(pixel.x,pixel.y,2,image.image[pixel.y][2][pixel.x]));
    00541
    00542       colorClass pixelColor = COLOR_CLASS(colorY,colorU,colorV);
    00543
    00544 //      if (pixelColor != pink)
    00545 //        colors[pixelColor]++;
    00546
    00547       if ((pixelColor == pink) || (pixelColor == noColor))
    00548         continue;
    00549
    00550       currentColor = pixelColor;
    00551
    00552       break;
    00553
    00554       if (pixelColor == currentColor)
    00555       {
    00556         currentColorCount++;
    00557       }
    00558       else
    00559       {
    00560         currentColor = pixelColor;
    00561         currentColorCount = 0;
    00562       };
    00563
    00564       if (currentColorCount > min (pinkRunWidth / 2.0, (double)10))
    00565         finished = true;
    00566     };
    00567
    00568     //calc most found color
    00569     //count = 0;
    00570
    00571     //for (int i = 0; i < numOfColors; i++)
    00572     //  if (colors[i] > count)
    00573     //  {
    00574     //    specialFlag.sectors[sector] = (colorClass)i;
    00575
    00576     //    count = colors[i];
    00577     //  };
    00578
    00579     specialFlag.sectors[sector] = currentColor;
    00580
    00581     LINE (imageProcessor_general, center.x, center.y, pixel.x, pixel.y, 1, Drawings::ps_solid, ColorClasses::colorClassToDrawingsColor (specialFlag.sectors[sector]));
    00582   };
    00583
    00584   Vector2<int>  pointOnField;
    00585   Vector2<double> angles;
    00586
    00587   Geometry::calculateAnglesForPoint (center, cameraMatrix, image.cameraInfo, angles);
    00588
    00589   double flagDirection = angles.x + robotPose.getAngle ();
    00590
    00591   LINE (percepts_robot, robotPose.translation.y, robotPose.translation.y, 500.0 * cos (flagDirection), 500.0 * sin (flagDirection), 50, Drawings::ps_solid, Drawings::pink);
    00592
    00593   SpecialFlag originalFlag = specialFlag;
    00594
    00595   double similarity = FindSimilarFlag (specialFlag);
    00596
    00597   if (similarity > MIN_SIMILARITY)
    00598   {
    00599     //found corresponding flag
    00600     CIRCLE(imageProcessor_general, center.x, center.y, 10, 1, Drawings::ps_solid, blue);
    00601
    00602     OUTPUT (idText, text, "recognized flag " << specialFlag.index << " with similarity " << similarity);
    00603
    00604     switch (slamPercept.slamState)
    00605     {
    00606     case SlamPercept::DebugJustLearn:
    00607     case SlamPercept::LearnNewFlags:
    00608       {
    00609         specialFlag.direction += flagDirection;
    00610         specialFlag.directionCount++;
    00611
    00612         break;
    00613       };
    00614     case SlamPercept::DebugJustRecognize:
    00615     case SlamPercept::UseNewFlags:
    00616       {
    00617         if (specialFlag.directionCount > 1)
    00618         {
    00619           specialFlag.direction /= specialFlag.directionCount;
    00620           specialFlag.directionCount = 1;
    00621         };
    00622
    00623         direction += specialFlag.direction;
    00624         directionCount++;
    00625
    00626         break;
    00627       };
    00628     };
    00629   }
    00630   else
    00631   {
    00632     //add new
    00633     CIRCLE(imageProcessor_general, center.x, center.y, 10, 1, Drawings::ps_solid, red);
    00634
    00635     if ((slamPercept.slamState == SlamPercept::LearnNewFlags) || (slamPercept.slamState == SlamPercept::DebugJustLearn))
    00636     {
    00637       OUTPUT (idText, text, "similarity only " << similarity);
    00638
    00639       originalFlag.direction = flagDirection;
    00640       originalFlag.directionCount = 1;
    00641       originalFlag.index = numFlags;
    00642
    00643       if (numFlags < NUM_SPECIAL_FLAGS)
    00644       {
    00645         OUTPUT (idText, text, "Learned new Flag " << numFlags << " @ " << originalFlag.direction);
    00646         OUTPUT (idText, text, "");
    00647
    00648         specialFlags[numFlags] = originalFlag;
    00649         numFlags++;
    00650       }
    00651       else
    00652       {
    00653         OUTPUT (idText, text, "Found to much Flags!!!");
    00654         OUTPUT (idText, text, "new Flag could not be learned:" << originalFlag.direction);
    00655         OUTPUT (idText, text, "");
    00656       }
    00657     }
    00658   };
    00659 };
    00660
    00661 void SlamBeaconDetector::characterizeNewFlag(const Vector2<int>& center)
    00662 {
    00663   SlamFlagLocator::flagCharacteristics left,right,above,below;
    00664   Vector2<double> direction = imageInf.horizon.direction; // directs right I hope   ;)
    00665   right = scanForFlagCharacterization(center,direction);
    00666   direction.rotateRight();
    00667   below = scanForFlagCharacterization(center,direction);
    00668   direction.rotateRight();
    00669   left = scanForFlagCharacterization(center,direction);
    00670   direction.rotateRight();
    00671   above = scanForFlagCharacterization(center,direction);
    00672
    00673   // insert the result into the NewFlagModell
    00674   Vector2<double> angles;
    00675   Geometry::calculateAnglesForPoint(center,cameraMatrix,image.cameraInfo,angles);
    00676   int characterisationNumber = slamPercept.flagLocator.getCharacterisationNumber(above,below,left,right);
    00677   slamPercept.flagLocator.addFlagDirection(robotPose,angles.x,characterisationNumber);
    00678 }
    00679
    00680 SlamFlagLocator::flagCharacteristics SlamBeaconDetector::scanForFlagCharacterization(const Vector2<int>& start, const Vector2<double>& direction)
    00681 {
    00682   BresenhamLineScan bls(start,direction,image.cameraInfo);
    00683   Vector2<int> actual(start);
    00684   int noPinkSince = 0;
    00685   int blueCount = 0;
    00686   int yellowCount = 0;
    00687   int somethingElseCount = 0;
    00688   bool stillOnPink = true;
    00689   bool onPinkAgain = false;
    00690
    00691   unsigned char y,u,v;
    00692   colorClass c1,c2,c3;
    00693
    00694   bool vertical = (fabs(direction.x) >= fabs(direction.y));
    00695
    00696   bool use3PixelScanLine = true;
    00697
    00698   DEBUG_RESPONSE("slam-ip:only-use-a-single-scanlines-to-characterise-flags",
    00699     use3PixelScanLine = false;
    00700   );
    00701
    00702
    00703
    00704   while (   (actual.x>0 && actual.x<image.cameraInfo.resolutionWidth-1 && actual.y>0 && actual.y<image.cameraInfo.resolutionHeight-1)
    00705          && (noPinkSince<=5)
    00706          && ( !onPinkAgain)    )
    00707   {
    00708     y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    00709     u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    00710     v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    00711     c1 = COLOR_CLASS(y, u, v);
    00712
    00713     if (use3PixelScanLine) // use all three pixels?
    00714     {
    00715       if (vertical)
    00716       {
    00717         // left of actual
    00718         y = colorCorrector.correct(actual.x-1, actual.y, 0, image.image[actual.y][0][actual.x-1]);
    00719         u = colorCorrector.correct(actual.x-1, actual.y, 1, image.image[actual.y][1][actual.x-1]);
    00720         v = colorCorrector.correct(actual.x-1, actual.y, 2, image.image[actual.y][2][actual.x-1]);
    00721         c2 = COLOR_CLASS(y, u, v);
    00722         // right of actual
    00723         y = colorCorrector.correct(actual.x+1, actual.y, 0, image.image[actual.y][0][actual.x+1]);
    00724         u = colorCorrector.correct(actual.x+1, actual.y, 1, image.image[actual.y][1][actual.x+1]);
    00725         v = colorCorrector.correct(actual.x+1, actual.y, 2, image.image[actual.y][2][actual.x+1]);
    00726         c3 = COLOR_CLASS(y, u, v);
    00727       }
    00728       else
    00729       {
    00730         // below actual
    00731         y = colorCorrector.correct(actual.x, actual.y-1, 0, image.image[actual.y-1][0][actual.x]);
    00732         u = colorCorrector.correct(actual.x, actual.y-1, 1, image.image[actual.y-1][1][actual.x]);
    00733         v = colorCorrector.correct(actual.x, actual.y-1, 2, image.image[actual.y-1][2][actual.x]);
    00734         c2 = COLOR_CLASS(y, u, v);
    00735         // above actual
    00736         y = colorCorrector.correct(actual.x, actual.y+1, 0, image.image[actual.y+1][0][actual.x]);
    00737         u = colorCorrector.correct(actual.x, actual.y+1, 1, image.image[actual.y+1][1][actual.x]);
    00738         v = colorCorrector.correct(actual.x, actual.y+1, 2, image.image[actual.y+1][2][actual.x]);
    00739         c3 = COLOR_CLASS(y, u, v);
    00740       }
    00741
    00742       if ( (c1==pink && c2==pink) || (c2==pink && c3==pink) || (c1==pink && c3==pink) )
    00743       {
    00744         if (!stillOnPink && noPinkSince>1)
    00745         {
    00746           onPinkAgain = true;
    00747         }
    00748         else
    00749         {
    00750           stillOnPink = true;
    00751           noPinkSince=0;
    00752           somethingElseCount=0; // forget the noise pixels inside the pink part
    00753         }
    00754       }
    00755       else
    00756       {
    00757         stillOnPink = false;
    00758         noPinkSince++;
    00759       }
    00760
    00761       switch (c1)
    00762       {
    00763         case pink:
    00764           break;
    00765         case skyblue:
    00766           blueCount++;
    00767           break;
    00768         case yellow:
    00769           yellowCount++;
    00770           break;
    00771         default:
    00772           somethingElseCount++;
    00773           break;
    00774       }
    00775       switch (c2)
    00776       {
    00777         case pink:
    00778           break;
    00779         case skyblue:
    00780           blueCount++;
    00781           break;
    00782         case yellow:
    00783           yellowCount++;
    00784           break;
    00785         default:
    00786           somethingElseCount++;
    00787           break;
    00788       }
    00789       switch (c3)
    00790       {
    00791         case pink:
    00792           break;
    00793         case skyblue:
    00794           blueCount++;
    00795           break;
    00796         case yellow:
    00797           yellowCount++;
    00798           break;
    00799         default:
    00800           somethingElseCount++;
    00801           break;
    00802       }
    00803
    00804       //
    00805     }
    00806     else
    00807     {
    00808       // use just a one-pixel-scanline only
    00809       switch (c1)
    00810       {
    00811         case pink:
    00812           if (!stillOnPink && noPinkSince>1)
    00813           {
    00814             onPinkAgain = true;
    00815           }
    00816           else
    00817           {
    00818             stillOnPink = true;
    00819             noPinkSince=0;
    00820             somethingElseCount=0; // forget the noise pixels inside the pink part
    00821           }
    00822           break;
    00823         case skyblue:
    00824           stillOnPink = false;
    00825           noPinkSince++;
    00826           blueCount++;
    00827           break;
    00828         case yellow:
    00829           stillOnPink = false;
    00830           noPinkSince++;
    00831           yellowCount++;
    00832           break;
    00833         default:
    00834           stillOnPink = false;
    00835           noPinkSince++;
    00836           somethingElseCount++;
    00837           break;
    00838       }
    00839     }
    00840
    00841     bls.getNext(actual);
    00842   }
    00843
    00844   int threshold;
    00845   if (use3PixelScanLine)
    00846   {
    00847     threshold = 6;
    00848   }
    00849   else
    00850   {
    00851     threshold = 2;
    00852   }
    00853
    00854   SlamFlagLocator::flagCharacteristics characteristics;
    00855
    00856   // interpret the result of the scanning
    00857   if (blueCount>=threshold && yellowCount<threshold && blueCount>somethingElseCount)
    00858   {
    00859     // characteristics = blue
    00860     characteristics = SlamFlagLocator::blueOnThisSide;
    00861   }
    00862   else if (yellowCount>=threshold && blueCount<threshold && yellowCount>somethingElseCount)
    00863   {
    00864     // characteristics = yellow
    00865     characteristics = SlamFlagLocator::yellowOnThisSide;
    00866   }
    00867   else
    00868   {
    00869     // characteristics = somethingElse
    00870     characteristics = SlamFlagLocator::somethingElseOnThisSide;
    00871   }
    00872
    00873   // drawings
    00874   NLINE("slam-ip:new-flag",start.x,start.y,actual.x,actual.y,1,Drawings::ps_solid,
    00875     (characteristics==SlamFlagLocator::blueOnThisSide)?(Drawings::blue):((characteristics==SlamFlagLocator::yellowOnThisSide)?(Drawings::yellow):(Drawings::dark_gray)));
    00876
    00877   return characteristics;
    00878 }
    00879
    00880 void SlamBeaconDetector::analyzeBeacon(const Vector2<double>& left, const Vector2<int>& center, const double pinkRunWidth)
    00881 {
    00882   if (pinkRunWidth >= 3.0)
    00883   {
    00884     numOfClusteredBeaconCandidates++;
    00885
    00886     findSpecial (center, pinkRunWidth);
    00887
    00888     characterizeNewFlag(center);
    00889
    00890   double rotation(imageInf.horizon.direction.angle());
    00891   double ca(cos(rotation));
    00892     double sa(sin(rotation));
    00893     Matrix2x2<double> rotMat(Vector2<double>(ca,sa),Vector2<double>(-sa,ca));
    00894     int flagReliability[4] = {0, 0, 0, 0};
    00895     Flag::FlagType flagFound;
    00896     Vector2<int> topEdge[4];
    00897     Vector2<int> bottomEdge[4];
    00898     int i;
    00899     int totalWeight = 0;
    00900     int highestReliability = 0;
    00901     int mostReliableFlag = -1;
    00902     for (i=0; i<4; i++)
    00903     {
    00904       topEdge[i].x = 0;
    00905       topEdge[i].y = 0;
    00906       bottomEdge[i].x = 0;
    00907       bottomEdge[i].y = 0;
    00908     }
    00909     Vector2<double> currentPos;
    00910     Vector2<double> step;
    00911     int numberOfScanCol;
    00912     if (pinkRunWidth <= 8.0)
    00913     { // only 3 scan lines
    00914       Vector2<double> displacement((pinkRunWidth-3.0)/2.0, 0); //1 pixel inner than theoretical border
    00915       Vector2<double> init(1.0, 0.0);
    00916       displacement = rotMat*displacement;
    00917       init = rotMat*init;
    00918       step = displacement;
    00919       currentPos = left+init;
    00920       numberOfScanCol = 3;
    00921     }
    00922     else
    00923     {
    00924       Vector2<double> displacement((pinkRunWidth-5.0)/3.0, 0); //2 pixels inner than theoretical border
    00925       Vector2<double> init(2.0, 0.0);
    00926       displacement = rotMat*displacement;
    00927       init = rotMat*init;
    00928       step = displacement;
    00929       currentPos = left+init;
    00930       numberOfScanCol = 4;
    00931     }
    00932     for (i=0; i<numberOfScanCol; i++)
    00933     {
    00934       Vector2<int> top, bottom;
    00935       int reliability = scanForBeaconEdges(Vector2<int>(int(currentPos.x+0.5), int(currentPos.y+0.5)),
    00936             pinkRunWidth, flagFound, top, bottom);
    00937       if (reliability != 0)
    00938       {
    00939         flagReliability[(int)flagFound] += reliability;
    00940         topEdge[(int)flagFound] += top;
    00941         bottomEdge[(int)flagFound] += bottom;
    00942       }
    00943       currentPos += step;
    00944     }
    00945     for (i=0; i<4; i++)
    00946     {
    00947       totalWeight += flagReliability[i];
    00948       if (flagReliability[i] > highestReliability)
    00949       {
    00950         highestReliability = flagReliability[i];
    00951         mostReliableFlag = i;
    00952       }
    00953     }
    00954     if (mostReliableFlag != -1)
    00955     {
    00956       double confidence = highestReliability/totalWeight;
    00957       if (confidence >= minFlagConfidence)
    00958       {
    00959         int posX = (int)((topEdge[mostReliableFlag].x + bottomEdge[mostReliableFlag].x)/(2*flagReliability[mostReliableFlag]) + 0.5);
    00960         int posY = (int)((topEdge[mostReliableFlag].y + bottomEdge[mostReliableFlag].y)/(2*flagReliability[mostReliableFlag]) + 0.5);
    00961         Vector2<int> center(posX, posY);
    00962         const Vector2<int> border(0,0);
    00963         Geometry::clipPointInsideRectange(border, imageInf.maxImageCoordinates-border, center);
    00964         DOT(imageProcessor_ball4, center.x, center.y, Drawings::black, Drawings::blue);
    00965         switch ((Flag::FlagType)mostReliableFlag)
    00966         {
    00967           case Flag::pinkAboveYellow:
    00968               flagSpecialist.searchFlags(image, colorTable, cameraMatrix, yellow, true, imageInf.horizon, center.x, center.y);
    00969               break;
    00970           case Flag::pinkAboveSkyblue:
    00971               flagSpecialist.searchFlags(image, colorTable, cameraMatrix, skyblue, true, imageInf.horizon, center.x, center.y);
    00972               break;
    00973           case Flag::yellowAbovePink:
    00974               flagSpecialist.searchFlags(image, colorTable, cameraMatrix, yellow, false, imageInf.horizon, center.x, center.y);
    00975               break;
    00976           case Flag::skyblueAbovePink:
    00977               flagSpecialist.searchFlags(image, colorTable, cameraMatrix, skyblue, false, imageInf.horizon, center.x, center.y);
    00978               break;
    00979         }
    00980       }
    00981     }
    00982   }
    00983 }
    00984
    00985 int SlamBeaconDetector::scanForBeaconEdges(const Vector2<int>& position, const double pinkRunWidth,
    00986         Flag::FlagType& flagType, Vector2<int>& topEdge, Vector2<int>& bottomEdge)
    00987 {
    00988   Vector2<int> beaconCenter(position);
    00989   Vector2<int> start, end;
    00990   Geometry::Line vertLine;
    00991   vertLine.direction = imageInf.vertLine.direction;
    00992   vertLine.base.x = (double)beaconCenter.x;
    00993   vertLine.base.y = (double)beaconCenter.y;
    00994   Geometry::getIntersectionPointsOfLineAndRectangle(Vector2<int>(1,1), // 1 pixel wide border has to be discarded
    00995               imageInf.maxImageCoordinates-Vector2<int>(1,1), vertLine, start, end); // due to the edge detection convolution mask
    00996   Vector2<int> pos, pos2, pinkBottomEdge, pinkTopEdge, colorBottomEdge, colorTopEdge;
    00997   colorClass bottomColor(noColor),topColor(noColor), baseColor(noColor), dontCare(noColor);
    00998   bool topColorEdgeFound(false), pinkTopFound(false);
    00999   bool unreliable(false); // this flag signals that an unsafe beacon hypothesis has been accepted
    01000   Flag::FlagType beaconType(Flag::pinkAboveYellow);
    01001   int reliability;
    01002   // Scan to bottom of pink beacon part
    01003   if(scanForBeaconPart(beaconCenter, end, pos, pinkBottomEdge, bottomColor))
    01004   {
    01005     if(bottomColor == white || bottomColor == gray)
    01006     {
    01007       // Scan to top of pink beacon part
    01008       if(scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor))
    01009       {
    01010         if(topColor == skyblue)
    01011         {
    01012           beaconType = Flag::skyblueAbovePink;
    01013           topColorEdgeFound = scanForBeaconPart(pos, start, pos2, colorTopEdge, dontCare);
    01014         }
    01015         else if(topColor == yellow)
    01016         {
    01017           beaconType = Flag::yellowAbovePink;
    01018           topColorEdgeFound = scanForBeaconPart(pos, start, pos2, colorTopEdge, dontCare);
    01019         }
    01020         else
    01021         {
    01022           return 0; // unknown color above pink
    01023         }
    01024       }
    01025       else
    01026       {
    01027         return 0; // pink part is at the upper border of the image
    01028       }
    01029     }
    01030     else if((bottomColor == skyblue) || (bottomColor == yellow))
    01031     {// Scan to bottom of color part
    01032       if(scanForBeaconPart(pos, end, pos2, colorBottomEdge, baseColor))
    01033       {
    01034         if(baseColor == white || baseColor == gray)
    01035         {
    01036           if(bottomColor == skyblue)
    01037           {
    01038             beaconType = Flag::pinkAboveSkyblue;
    01039             pinkTopFound = scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor);
    01040           }
    01041           else
    01042           {
    01043             beaconType = Flag::pinkAboveYellow;
    01044             pinkTopFound = scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor);
    01045           }
    01046         }
    01047         else
    01048         { // perhaps we missed the real end of the colored part
    01049           if (scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor))
    01050           { // estimate colorBottomEdge from pinkTopEdge
    01051             Vector2<int> toPink(pinkTopEdge - pinkBottomEdge);
    01052             double distanceToPink = toPink.abs();
    01053             double similarityPink;
    01054             if (distanceToPink > pinkRunWidth)
    01055               similarityPink = (pinkRunWidth/distanceToPink);
    01056             else
    01057               similarityPink = (distanceToPink/pinkRunWidth);
    01058             if (similarityPink > projectionAspectRatio) // make sure that this proposed edge is compatible with the aspect ratio
    01059             {
    01060               colorBottomEdge = pinkBottomEdge + (pinkBottomEdge - pinkTopEdge);
    01061               pinkTopFound = true;
    01062               if(bottomColor == skyblue)
    01063                 beaconType = Flag::pinkAboveSkyblue;
    01064               else
    01065                 beaconType = Flag::pinkAboveYellow;
    01066             }
    01067             else
    01068               return 0;
    01069           }
    01070           else
    01071             return 0; // unable to determine top edge of pink part
    01072         }
    01073       }
    01074       else
    01075       { // we were unable to find the end of the colored part (no contrast?), and reached the border of the image
    01076         // before giving up, let's see if we can at least find the top edge of the pink part
    01077         if (scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor))
    01078         { // estimate colorBottomEdge from pinkTopEdge
    01079           Vector2<int> toPink(pinkTopEdge - pinkBottomEdge);
    01080           double distanceToPink = toPink.abs();
    01081           double similarityPink;
    01082           if (distanceToPink > pinkRunWidth)
    01083             similarityPink = (pinkRunWidth/distanceToPink);
    01084           else
    01085             similarityPink = (distanceToPink/pinkRunWidth);
    01086           if (similarityPink > projectionAspectRatio) // make sure that this proposed edge is compatible with the aspect ratio
    01087           {
    01088             colorBottomEdge = pinkBottomEdge + (pinkBottomEdge - pinkTopEdge);
    01089             pinkTopFound = true;
    01090             if(bottomColor == skyblue)
    01091               beaconType = Flag::pinkAboveSkyblue;
    01092             else
    01093               beaconType = Flag::pinkAboveYellow;
    01094           }
    01095           else
    01096             return 0;
    01097         }
    01098         else
    01099           return 0; // unable to determine top edge of pink part
    01100       }
    01101     }
    01102     else
    01103     {
    01104       return 0; // unknown color below pink
    01105     }
    01106   }
    01107   else // unable to determine bottom edge (no gradient, or border of the image touched)
    01108   { // so. try to scan to top
    01109     if(scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor))
    01110     {
    01111       DOT(imageProcessor_ball4, pinkTopEdge.x, pinkTopEdge.y, Drawings::black, Drawings::pink);
    01112       if(topColor == skyblue)
    01113       {
    01114         beaconType = Flag::skyblueAbovePink;
    01115         topColorEdgeFound = scanForBeaconPart(pos, start, pos2, colorTopEdge, dontCare);
    01116         if (!topColorEdgeFound)
    01117           return 0; // unable to calculate the vertical edges of the landmark
    01118         DOT(imageProcessor_ball4, colorTopEdge.x, colorTopEdge.y, Drawings::black, Drawings::red);
    01119       }
    01120       else if(topColor == yellow)
    01121       {
    01122         beaconType = Flag::yellowAbovePink;
    01123         topColorEdgeFound = scanForBeaconPart(pos, start, pos2, colorTopEdge, dontCare);
    01124         if (!topColorEdgeFound)
    01125           return 0; // unable to calculate the vertical edges of the landmark
    01126         DOT(imageProcessor_ball4, colorTopEdge.x, colorTopEdge.y, Drawings::black, Drawings::red);
    01127       }
    01128       else // last attempt, try to use scan width information and top edge to make an hypothesis on bottom edge,
    01129       { // check if this holds by looking at the color underlying the projected edge (should be yellow or skyblue)
    01130         Vector2<double> projectedLenght(0, pinkRunWidth+3.0); //3 pixel more to make sure we jump over the pink/color edge
    01131         double rotation(imageInf.horizon.direction.angle());
    01132         double ca(cos(rotation));
    01133         double sa(sin(rotation));
    01134         Matrix2x2<double> rotMat(Vector2<double>(ca,sa),Vector2<double>(-sa,ca));
    01135         Vector2<double> topToBottomDisplacement = rotMat*projectedLenght;
    01136         Vector2<int> displacement((int)(topToBottomDisplacement.x+0.5), (int)(topToBottomDisplacement.y+0.5));
    01137         pinkBottomEdge = pinkTopEdge + displacement;
    01138         const Vector2<int> border(0,0);
    01139         Geometry::clipPointInsideRectange(border, imageInf.maxImageCoordinates-border, pinkBottomEdge);
    01140         unsigned char colorY(colorCorrector.correct(pinkBottomEdge.x,pinkBottomEdge.y,0,image.image[pinkBottomEdge.y][0][pinkBottomEdge.x]));
    01141         unsigned char colorU(colorCorrector.correct(pinkBottomEdge.x,pinkBottomEdge.y,1,image.image[pinkBottomEdge.y][1][pinkBottomEdge.x]));
    01142         unsigned char colorV(colorCorrector.correct(pinkBottomEdge.x,pinkBottomEdge.y,2,image.image[pinkBottomEdge.y][2][pinkBottomEdge.x]));
    01143         bottomColor = COLOR_CLASS(colorY,colorU,colorV);
    01144         DOT(imageProcessor_ball4, pinkBottomEdge.x, pinkBottomEdge.y, Drawings::gray, ColorClasses::colorClassToDrawingsColor(bottomColor));
    01145         if((bottomColor == skyblue) || (bottomColor == yellow))
    01146         {
    01147           colorBottomEdge = pinkBottomEdge + displacement;
    01148           Geometry::clipPointInsideRectange(border, imageInf.maxImageCoordinates-border, colorBottomEdge);
    01149           colorY = colorCorrector.correct(colorBottomEdge.x,colorBottomEdge.y,0,image.image[colorBottomEdge.y][0][colorBottomEdge.x]);
    01150           colorU = colorCorrector.correct(colorBottomEdge.x,colorBottomEdge.y,1,image.image[colorBottomEdge.y][1][colorBottomEdge.x]);
    01151           colorV = colorCorrector.correct(colorBottomEdge.x,colorBottomEdge.y,2,image.image[colorBottomEdge.y][2][colorBottomEdge.x]);
    01152           baseColor = COLOR_CLASS(colorY,colorU,colorV);
    01153           DOT(imageProcessor_ball4, colorBottomEdge.x, colorBottomEdge.y, Drawings::white, ColorClasses::colorClassToDrawingsColor(baseColor));
    01154           if (baseColor == white || baseColor == gray)
    01155           {
    01156             if (bottomColor == skyblue)
    01157             {
    01158               beaconType = Flag::pinkAboveSkyblue;
    01159               pinkTopFound = true;
    01160               unreliable = true;
    01161             }
    01162             else if (bottomColor == yellow)
    01163             {
    01164               beaconType = Flag::pinkAboveYellow;
    01165               pinkTopFound = true;
    01166               unreliable = true;
    01167             }
    01168             else
    01169               return 0;
    01170           }
    01171           else if (baseColor == pink) // we could have missed the middle edge of a landmark whose lower part is pink
    01172           {
    01173             if (bottomColor == skyblue)
    01174             {
    01175               beaconType = Flag::skyblueAbovePink;
    01176               pinkTopEdge = pinkBottomEdge; // we have to swap up and down, because it seems
    01177               pinkBottomEdge = colorBottomEdge; //that the assumption pinkAboveSkyblue was wrong
    01178               topColor = skyblue;
    01179               bottomColor = pink;
    01180               unreliable = true;
    01181             }
    01182             else if (bottomColor == yellow)
    01183             {
    01184               beaconType = Flag::yellowAbovePink;
    01185               pinkTopEdge = pinkBottomEdge; // we have to swap up and down, because it seems
    01186               pinkBottomEdge = colorBottomEdge; //that the assumption pinkAboveSkyblue was wrong
    01187               topColor = yellow;
    01188               bottomColor = pink;
    01189               unreliable = true;
    01190             }
    01191             else
    01192               return 0;
    01193           }
    01194           else
    01195             return 0;
    01196         }
    01197         else
    01198         {
    01199           return 0; // unknown color above pink
    01200         }
    01201       }
    01202     }
    01203     else
    01204     {
    01205       return 0;
    01206     }
    01207   }
    01208
    01209   //~ DOT(imageProcessor_ball4, pos.x, pos.y, Drawings::white, Drawings::black);
    01210   //~ DOT(imageProcessor_ball4, pos2.x, pos2.y, Drawings::black, Drawings::gray);
    01211
    01212   if(beaconType == Flag::pinkAboveYellow || beaconType == Flag::pinkAboveSkyblue)
    01213   {
    01214     if(pinkTopFound)
    01215     {
    01216       //~ DOT(imageProcessor_ball4, pinkBottomEdge.x, pinkBottomEdge.y, Drawings::black, Drawings::green);
    01217       //~ DOT(imageProcessor_ball4, pinkTopEdge.x, pinkTopEdge.y, Drawings::white, Drawings::black);
    01218       //~ DOT(imageProcessor_ball4, colorBottomEdge.x, colorBottomEdge.y, Drawings::black, Drawings::white);
    01219       Vector2<int> topPosition(pinkBottomEdge);
    01220       Vector2<int> toColor(pinkBottomEdge - colorBottomEdge);
    01221       Vector2<int> toPink(pinkTopEdge - pinkBottomEdge);
    01222       double distanceToColor = toColor.abs();
    01223       double distanceToPink = toPink.abs();
    01224       double similarityColor, similarityPink;
    01225       if (distanceToColor > pinkRunWidth)
    01226         similarityColor = (pinkRunWidth/distanceToColor);
    01227       else
    01228         similarityColor = (distanceToColor/pinkRunWidth);
    01229       if (distanceToPink > pinkRunWidth)
    01230         similarityPink = (pinkRunWidth/distanceToPink);
    01231       else
    01232         similarityPink = (distanceToPink/pinkRunWidth);
    01233       if (similarityColor > similarityPink)
    01234         topPosition += toColor;
    01235       else
    01236         topPosition += toPink;
    01237       //~ DOT(imageProcessor_ball4, topPosition.x, topPosition.y, Drawings::black, Drawings::gray);
    01238       //~ DOT(imageProcessor_ball4, pinkBottomEdge.x, topPosition.y+(pinkBottomEdge.y-topPosition.y)/2, Drawings::black, Drawings::blue);
    01239       if (unreliable)
    01240         reliability = lowReliability;
    01241       else
    01242         reliability = highReliability;
    01243       flagType = beaconType;
    01244       topEdge = topPosition*reliability;
    01245       bottomEdge = pinkBottomEdge*reliability;
    01246       return reliability;
    01247     }
    01248     else
    01249     {
    01250       // compute a theoretical position of the pink top edge:
    01251       Vector2<int> topPosition(pinkBottomEdge);
    01252       Vector2<int> relVec(pinkBottomEdge - colorBottomEdge);
    01253       topPosition += relVec;
    01254       //~ DOT(imageProcessor_ball4, pinkBottomEdge.x, topPosition.y+(pinkBottomEdge.y-topPosition.y)/2, Drawings::black, Drawings::blue);
    01255       //~ DOT(imageProcessor_ball4, pinkBottomEdge.x, pinkBottomEdge.y, Drawings::black, Drawings::red);
    01256       //~ DOT(imageProcessor_ball4, topPosition.x, topPosition.y, Drawings::white, Drawings::black);
    01257       reliability = mediumReliability;
    01258       flagType = beaconType;
    01259       topEdge = topPosition*reliability;
    01260       bottomEdge = pinkBottomEdge*reliability;
    01261       return reliability;
    01262     }
    01263   }
    01264   else
    01265   { // pink is bottom
    01266     if (bottomColor == noColor && topColorEdgeFound) // no pink bottom edge found
    01267     { //theoretical position will be calculated from color top edge
    01268       Vector2<int> bottomPosition(pinkTopEdge);
    01269       Vector2<int> relVec(pinkTopEdge - colorTopEdge);
    01270       bottomPosition += relVec;
    01271       flagSpecialist.searchFlags(image, colorTable, cameraMatrix, topColor, false,
    01272           imageInf.horizon, (int)((pinkTopEdge.x+bottomPosition.x+1.0)/2), (int)((pinkTopEdge.y+bottomPosition.y+1.0)/2));
    01273       DOT(imageProcessor_ball4, pinkTopEdge.x, pinkTopEdge.y, Drawings::white, Drawings::black);
    01274       DOT(imageProcessor_ball4, bottomPosition.x, bottomPosition.y, Drawings::black, Drawings::yellow);
    01275       DOT(imageProcessor_ball4, (int)((pinkTopEdge.x+bottomPosition.x+1.0)/2), (int)((pinkTopEdge.y+bottomPosition.y+1.0)/2), Drawings::black, Drawings::blue);
    01276       reliability = mediumReliability;
    01277       flagType = beaconType;
    01278       topEdge = pinkTopEdge*reliability;
    01279       bottomEdge = bottomPosition*reliability;
    01280       return reliability;
    01281     }
    01282     else{
    01283       Vector2<int> bottomPosition;
    01284       if (topColorEdgeFound) //color edge information used just for redundancy check
    01285       {
    01286         bottomPosition = pinkTopEdge;
    01287         Vector2<int> toColor(pinkTopEdge - colorTopEdge);
    01288         Vector2<int> toPink(pinkBottomEdge - pinkTopEdge);
    01289         double distanceToColor = toColor.abs();
    01290         double distanceToPink = toPink.abs();
    01291         double similarityColor, similarityPink;
    01292         if (distanceToColor > pinkRunWidth)
    01293           similarityColor = (pinkRunWidth/distanceToColor);
    01294         else
    01295           similarityColor = (distanceToColor/pinkRunWidth);
    01296         if (distanceToPink > pinkRunWidth)
    01297           similarityPink = (pinkRunWidth/distanceToPink);
    01298         else
    01299           similarityPink = (distanceToPink/pinkRunWidth);
    01300         if (similarityColor > similarityPink)
    01301           bottomPosition += toColor;
    01302         else
    01303           bottomPosition += toPink;
    01304         if (unreliable)
    01305           reliability = lowReliability;
    01306         else
    01307           reliability = highReliability;
    01308         flagType = beaconType;
    01309         topEdge = pinkTopEdge*reliability;
    01310         bottomEdge = bottomPosition*reliability;
    01311         return reliability;
    01312       }
    01313       else
    01314       {
    01315         if (unreliable)
    01316           reliability = lowReliability;
    01317         else
    01318           reliability = mediumReliability;
    01319         flagType = beaconType;
    01320         topEdge = pinkTopEdge*reliability;
    01321         bottomEdge = pinkBottomEdge*reliability;
    01322         return reliability;
    01323       }
    01324     }
    01325   }
    01326   return true;
    01327 }
    01328
    01329
    01330 bool SlamBeaconDetector::scanForBeaconPart(const Vector2<int>& start, const Vector2<int>& end,
    01331                                              Vector2<int>& position, Vector2<int>& edge, colorClass& color)
    01332 {
    01333   // DOT(imageProcessor_ball4, start.x, start.y, Drawings::blue, Drawings::orange);
    01334   // DOT(imageProcessor_ball4, end.x, end.y, Drawings::yellow, Drawings::white);
    01335   BresenhamLineScan bresenham(start, end);
    01336   bresenham.init();
    01337   Vector2<int> pixel(start);
    01338   for(int i=0; i<bresenham.numberOfPixels; i++)
    01339   {
    01340     // bool edgeY = edgeDetectionY.isEdgePoint(image, pixel.x, pixel.y, SUSANEdgeDetectionLite::componentA);
    01341     bool edgeU = edgeDetectionU.isEdgePoint(image, pixel.x, pixel.y, SUSANEdgeDetectionLite::componentB);
    01342     bool edgeV = edgeDetectionV.isEdgePoint(image, pixel.x, pixel.y, SUSANEdgeDetectionLite::componentC);
    01343     DOT(imageProcessor_ball4, pixel.x, pixel.y, Drawings::black, Drawings::orange);
    01344
    01345     if (edgeU || edgeV)
    01346     {
    01347       position = pixel;
    01348       edge = pixel;
    01349       color = noColor;
    01350       unsigned char colorY(colorCorrector.correct(edge.x,edge.y,0,image.image[edge.y][0][edge.x]));
    01351       unsigned char colorU(colorCorrector.correct(edge.x,edge.y,1,image.image[edge.y][1][edge.x]));
    01352       unsigned char colorV(colorCorrector.correct(edge.x,edge.y,2,image.image[edge.y][2][edge.x]));
    01353       colorClass edgeColor = COLOR_CLASS(colorY,colorU,colorV);
    01354       color = edgeColor;
    01355       int counter;
    01356       for (counter = 0; counter < edgeScanDepth &&
    01357             ((color == noColor)||(color==edgeColor)||(color==orange)||edgeU||edgeV); counter++)
    01358       {
    01359         bresenham.getNext(position);
    01360         unsigned char colorY(colorCorrector.correct(position.x,position.y,0,image.image[position.y][0][position.x]));
    01361         unsigned char colorU(colorCorrector.correct(position.x,position.y,1,image.image[position.y][1][position.x]));
    01362         unsigned char colorV(colorCorrector.correct(position.x,position.y,2,image.image[position.y][2][position.x]));
    01363         color = COLOR_CLASS(colorY,colorU,colorV);
    01364         edgeU = edgeDetectionU.isEdgePoint(image, position.x, position.y, SUSANEdgeDetectionLite::componentB);
    01365         edgeV = edgeDetectionV.isEdgePoint(image, position.x, position.y, SUSANEdgeDetectionLite::componentC);
    01366       }
    01367       if (counter != edgeScanDepth)
    01368         return true;
    01369     }
    01370     //Compute next position according to Bresenham algorithm
    01371     bresenham.getNext(pixel);
    01372   }
    01373   return 0;
    01374 }
    01375
    01376 void SlamBeaconDetector::analyzeColorTable()
    01377 {
    01378   Vector3<int> nearPoint,farPoint;
    01379 #ifdef CT32K_LAYOUT
    01380   ((ColorTable32K&) colorTable).getBoxAroundColorClass(pink,nearPoint,farPoint);
    01381 #else
    01382   ((ColorTable64&) colorTable).getBoxAroundColorClass(pink,nearPoint,farPoint);
    01383 #endif
    01384   minPinkUValue = nearPoint.y;
    01385 }