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

# Modules/ImageProcessor/VLCImageProcessor/VLCImageProcessor.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file VLCImageProcessor.cpp
    00003 *
    00004 * Implementation of class VLCImageProcessor
    00005 *
    00006 * @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
    00007 * @author <a href="mailto:roefer@tzi.de">Thomas Röfer</a>
    00008 * @author <a href="mailto:stefan.czarnetzki@uni-dortmund.de">Stefan Czarnetzki</a>
    00009 */
    00010
    00011 #include "VLCImageProcessor.h"
    00012 #include "Tools/Debugging/Debugging.h"
    00013 #include "Tools/Debugging/GenericDebugData.h"
    00014 #include "Tools/FieldDimensions.h"
    00015 #include "Tools/Math/Geometry.h"
    00016 #include "VLCImageProcessorTools.h"
    00017 #include "Representations/Perception/JPEGImage.h"
    00018 #include "Tools/Streams/InStreams.h"
    00019
    00020 // for VLC
    00021 #include "Tools/Location.h"
    00022 //
    00023
    00024 // Parameters for the scanning grid
    00025 const double VLCImageProcessor::verticalScanLineTopAboveHorizon = 15.0;
    00026 const double VLCImageProcessor::verticalScanLineLength13 = 55.0;
    00027 const double VLCImageProcessor::verticalScanLineLength2 = 75.0;
    00028 const double VLCImageProcessor::verticalScanLineSpacing = 4.0;
    00029
    00030
    00031 #define PLOT(p,c) COMPLEX_DRAWING(imageProcessor_general,plot(p,c))
    00032
    00033 const int Y = 0,
    00034           U = cameraResolutionWidth_ERS7, /**< Relative offset of U component. */
    00035           V = 2 * cameraResolutionWidth_ERS7; /**< Relative offset of V component. */
    00036
    00037 const double VLCImageProcessor::minAngleBetweenFlagAndGoal = 0.05;
    00038
    00039
    00040 VLCImageProcessor::VLCImageProcessor(const ImageProcessorInterfaces& interfaces)
    00041 : ImageProcessor(interfaces),
    00042 beaconDetector(image, cameraMatrix, imageInfo.previousCameraMatrix, imageInfo, colorTable, colorCorrector, landmarksPercept),
    00043 goalSpecialistY(yellow, interfaces, colorCorrector, imageInfo),
    00044 goalSpecialistB(skyblue, interfaces, colorCorrector, imageInfo),
    00045 ballSpecialist(colorCorrector),
    00046 ballClustering(),
    00047 lineFinder(colorCorrector, colorTable),
    00048 circleFinder(cameraMatrix, image)
    00049 {
    00050   yThreshold = 15;
    00051   vThreshold = 8;
    00052   beaconDetector.analyzeColorTable();
    00053   double c = cos(-3*pi/4);
    00054   double s = sin(-3*pi/4);
    00055   rotation2x2 = Matrix2x2<double>(
    00056     Vector2<double>(c,-s),
    00057     Vector2<double>(s,c)
    00058   );
    00059
    00060   // --- for VLC
    00061   averageYBuffer.init();
    00062
    00063   // instantiate the 4 colortables with this ugly code copied from the ColorTable64Mod.cpp
    00064   colorTableVeryDark = new ColorTable64();
    00065   InBinaryFile stream1(getLocation().getModelFilename("verydark.c64"));
    00066   colorTableVeryDark->setFormat(ColorTable64::CT64);
    00067   if (stream1.exists())
    00068   {
    00069     stream1 >> *colorTableVeryDark;
    00070   }else{
    00071     //OUTPUT(idText, text, "load verydark.c64 FAILED");
    00072     //colorTableVeryDark = &((ColorTable64)colorTable);
    00073   }
    00074
    00075   colorTableDark = new ColorTable64();
    00076   InBinaryFile stream2(getLocation().getModelFilename("dark.c64"));
    00077   colorTableDark->setFormat(ColorTable64::CT64);
    00078   if (stream2.exists())
    00079   {
    00080     stream2 >> *colorTableDark;
    00081   }else{
    00082     //OUTPUT(idText, text, "load dark.c64 FAILED");
    00083     //colorTableDark = &((ColorTable64)colorTable);
    00084   }
    00085
    00086   colorTableMedium = new ColorTable64();
    00087   InBinaryFile stream3(getLocation().getModelFilename("medium.c64"));
    00088   colorTableMedium->setFormat(ColorTable64::CT64);
    00089   if (stream3.exists())
    00090   {
    00091     stream3 >> *colorTableMedium;
    00092   }else{
    00093     //OUTPUT(idText, text, "load medium.c64 FAILED");
    00094     //colorTableMedium = &((ColorTable64)colorTable);
    00095   }
    00096
    00097   colorTableBright = new ColorTable64();
    00098   InBinaryFile stream4(getLocation().getModelFilename("bright.c64"));
    00099   colorTableBright->setFormat(ColorTable64::CT64);
    00100   if (stream4.exists())
    00101   {
    00102     stream4 >> *colorTableBright;
    00103   }else{
    00104     //OUTPUT(idText, text, "load bright.c64 FAILED");
    00105     //colorTableBright = &((ColorTable64)colorTable);
    00106   }
    00107
    00108   // read the thresholds
    00109   vd2d=d2m=m2b=0.0;
    00110
    00111   //read port
    00112   InConfigFile file(getLocation().getModelFilename("vlc.cfg"));
    00113   // format:
    00114   // VeryDark2Dark x.x
    00115   // Dark2Medium x.x
    00116   // Medium2Bright x.x
    00117   // spaces and nl are ignored
    00118   if(file.exists())
    00119   {
    00120     char buf[250];
    00121     while(!file.eof())
    00122     {
    00123       file >> buf;
    00124       if ( strcmp(buf,"VeryDark2Dark")==0 )
    00125       {
    00126         file >> vd2d;
    00127         // OUTPUT(idText, text, "VeryDark2Dark-Threshold = " << vd2d);
    00128       }
    00129       else if ( strcmp(buf,"Dark2Medium")==0 )
    00130       {
    00131         file >> d2m;
    00132         // OUTPUT(idText, text, "Dark2Medium-Threshold = " << d2m);
    00133       }
    00134       else if ( strcmp(buf,"Medium2Bright")==0 )
    00135       {
    00136         file >> m2b;
    00137         // OUTPUT(idText, text, "Medium2Bright-Threshold = " << m2b);
    00138       }
    00139     }
    00140   }
    00141
    00142   // just for initialisation
    00143   bestColorTable = colorTableMedium;
    00144
    00145   averageY = 100;
    00146   numberOfScannedPixels = 1;
    00147
    00148   // --- end (for VLC)
    00149 }
    00150
    00151 void VLCImageProcessor::execute()
    00152 {
    00153   INIT_DEBUG_IMAGE(imageProcessorPlayers, image);
    00154   N_INIT_DEBUG_GRAY_SCALE_IMAGE(scanLines, image);
    00155
    00156   xFactor = yFactor = image.cameraInfo.focalLength;
    00157
    00158
    00159   // for testing
    00160   for (int j = 0; j<15; j+=2)
    00161   {
    00162     RobotPose base;
    00163     base.translation.x=0;
    00164     base.translation.y=0;
    00165     base.rotation=0;
    00166     RobotPose testPose,testPose2;
    00167     Pose2D movement(fromDegrees(5),300,100);
    00168     Pose2D movement2(0,300,100);
    00169     base.calculateMovement(movement,j,testPose);
    00170     Vector2<double> temp(testPose.translation.x + 100 * cos(testPose.rotation),testPose.translation.y + 100 * sin(testPose.rotation));
    00171     ARROW(lineCrossingsField, testPose.translation.x, testPose.translation.y, temp.x, temp.y, 3, Drawings::ps_solid, Drawings::orange);
    00172     base.calculateMovement(movement2,j,testPose2);
    00173     Vector2<double> temp2(testPose2.translation.x + 100 * cos(testPose2.rotation),testPose2.translation.y + 100 * sin(testPose2.rotation));
    00174     ARROW(lineCrossingsField, testPose2.translation.x, testPose2.translation.y, temp2.x, temp2.y, 3, Drawings::ps_solid, Drawings::red);
    00175   }
    00176   //*/
    00177
    00178   // for VLC
    00179   averageY /= numberOfScannedPixels;
    00180   averageU /= numberOfScannedPixels;
    00181   averageV /= numberOfScannedPixels;
    00182   OUTPUT(idText,text,"Statistics:\t" << averageY << "\t" << averageU << "\t" << averageV);
    00183   if (averageY<255.0)
    00184   { // the first value may be fucked up and then messes up the first 5 frames (ok, who cares about five frames, but this way its totally correct)
    00185     averageYBuffer.add(averageY);
    00186   }
    00187
    00188
    00189   // decide which colorTable to use in this frame (based on the statistics of the last one)
    00190   int numberOfEntrys = averageYBuffer.getNumberOfEntries();
    00191   double smoothedY = 0;
    00192   for (int x=0; x<numberOfEntrys; x++)
    00193   {
    00194     smoothedY += averageYBuffer[x];
    00195   }
    00196   if (numberOfEntrys>0)
    00197   {
    00198     smoothedY /= numberOfEntrys;
    00199   }
    00200
    00201   if (smoothedY<vd2d)
    00202   {
    00203     bestColorTable = colorTableVeryDark;
    00204     //OUTPUT(idText,text,"VLCImageProcessor: very dark");
    00205   }
    00206   else if (smoothedY<d2m)
    00207   {
    00208     bestColorTable = colorTableDark;
    00209     //OUTPUT(idText,text,"VLCImageProcessor: dark");
    00210   }
    00211   else if (smoothedY<m2b)
    00212   {
    00213     bestColorTable = colorTableMedium;
    00214     //OUTPUT(idText,text,"VLCImageProcessor: medium");
    00215   }
    00216   else
    00217   {
    00218     bestColorTable = colorTableBright;
    00219     //OUTPUT(idText,text,"VLCImageProcessor: bright");
    00220   }
    00221
    00222   // reset statistics
    00223   averageY = 0;
    00224   averageU = 0;
    00225   averageV = 0;
    00226
    00227   beaconDetector.setBestColorTable(bestColorTable);
    00228   //goalRecognizer.setBestColorTable(bestColorTable);
    00229   goalSpecialistB.setBestColorTable(bestColorTable);
    00230   goalSpecialistY.setBestColorTable(bestColorTable);
    00231   lineFinder.setBestColorTable(bestColorTable);
    00232   // end (for VLC)
    00233
    00234   numberOfScannedPixels = 0;
    00235
    00236   // reset everything
    00237   landmarksPercept.reset(image.frameNumber);
    00238   linesPercept.reset(image.frameNumber);
    00239   ballPercept.reset(image.frameNumber);
    00240   playersPercept.reset(image.frameNumber);
    00241   obstaclesPercept.reset(image.frameNumber);
    00242   circleFinder.reset();
    00243   lineFinder.reset();
    00244   goalSpecialistY.notifyAboutNewImage();
    00245   goalSpecialistB.notifyAboutNewImage();
    00246
    00247
    00248   DEBUG_RESPONSE_NOT("gt05-ip:disable-multiple-ball-candidates",
    00249     ballClustering.reset();
    00250   );
    00251
    00252   /*
    00253   DEBUG_RESPONSE("gt05-ip:enable-edge-detection",
    00254     edgesPercept.reset(image.frameNumber);
    00255     edgeSpecialist.reset();
    00256   );
    00257   */
    00258
    00259   // variations of the camera matrix for different object heights
    00260   cmTricot = cameraMatrix;
    00261   cmTricot.translation.z -= 100; // upper tricot border
    00262
    00263   longestBallRun = 0;
    00264
    00265 //-----------------------------------------------------
    00266   Vector3<double> vectorInDirectionOfViewCamera(1,0,0);
    00267   Vector3<double> vectorInDirectionOfViewWorld;
    00268   vectorInDirectionOfViewWorld = cameraMatrix.rotation * vectorInDirectionOfViewCamera;
    00269
    00270   const int visualizationScale = 20;
    00271   Vector3<double> vectorInDirectionOfViewWorldOld;
    00272   vectorInDirectionOfViewWorldOld = imageInfo.previousCameraMatrix.rotation * vectorInDirectionOfViewCamera;
    00273   Vector3<double> cameraMotionVectorWorld = vectorInDirectionOfViewWorld - vectorInDirectionOfViewWorldOld;
    00274   long frameNumber = cameraMatrix.frameNumber,
    00275   prevFrameNumber = imageInfo.previousCameraMatrix.frameNumber;
    00276   const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    00277   double timeDiff = (frameNumber - prevFrameNumber) * r.motionCycleTime; // in seconds
    00278   if (prevFrameNumber == 0)
    00279     timeDiff = 0; // Previous camera matrix was uninitialized
    00280   Vector2<double> currentOnGround(vectorInDirectionOfViewWorld.x, vectorInDirectionOfViewWorld.y),
    00281       oldOnGround(vectorInDirectionOfViewWorldOld.x, vectorInDirectionOfViewWorldOld.y);
    00282   currentOnGround.normalize();
    00283   oldOnGround.normalize();
    00284   Vector3<double> cameraSpeedVectorWorld(0, 0, 0);
    00285   double panningVelocity = 0;
    00286   if (timeDiff > 0) //should never happen otherwise, but better safe than sorry
    00287   {
    00288     cameraSpeedVectorWorld = cameraMotionVectorWorld/timeDiff;
    00289     double cosAng = currentOnGround*oldOnGround;
    00290     if (cosAng > 1.0)
    00291       cosAng = 1.0;
    00292     else
    00293     if (cosAng <-1.0)
    00294       cosAng = -1.0;
    00295     panningVelocity = normalize(acos(cosAng))/timeDiff;
    00296   }
    00297   Vector3<double> cameraSpeedVectorCamera = cameraMatrix.invert().rotation * cameraSpeedVectorWorld;
    00298   Vector2<double> cameraSpeedVectorImg(cameraSpeedVectorCamera.y,
    00299         cameraSpeedVectorCamera.z);
    00300   cameraSpeedVectorImg /= cameraSpeedVectorCamera.x + // perspective projection,
    00301     image.cameraInfo.focalLength; // focal length is added to translate the vector away from the lens in case the distance
    00302                                     // is too small and hence the visualization would be greatly magnified
    00303   cameraSpeedVectorImg *= image.cameraInfo.focalLength*visualizationScale;
    00304   Drawings::Color arrowColor = Drawings::blue;
    00305   if (fabs(panningVelocity) > pi/6)
    00306     arrowColor = Drawings::red;
    00307   ARROW(imageProcessor_horizon,
    00308     image.cameraInfo.opticalCenter.x, image.cameraInfo.opticalCenter.y,
    00309     int(image.cameraInfo.opticalCenter.x-cameraSpeedVectorImg.x), int(image.cameraInfo.opticalCenter.y-cameraSpeedVectorImg.y),
    00310     3, Drawings::ps_solid, arrowColor);
    00311 //-----------------------------------------------------
    00312   angleBetweenDirectionOfViewAndGround =
    00313     toDegrees(
    00314     atan2(
    00315     vectorInDirectionOfViewWorld.z,
    00316     sqrt(sqr(vectorInDirectionOfViewWorld.x) + sqr(vectorInDirectionOfViewWorld.y))
    00317     )
    00318     );
    00319
    00320   // Compute information related to the horizon
    00321   imageInfo.maxImageCoordinates.x = image.cameraInfo.resolutionWidth - 1;
    00322   imageInfo.maxImageCoordinates.y = image.cameraInfo.resolutionHeight - 1;
    00323   imageInfo.horizon = Geometry::calculateHorizon(cameraMatrix, image.cameraInfo);
    00324   imageInfo.horizon.normalizeDirection();
    00325
    00326   imageInfo.horizonInImage = Geometry::getIntersectionPointsOfLineAndRectangle(
    00327       Vector2<int>(0,0), imageInfo.maxImageCoordinates, imageInfo.horizon,
    00328       imageInfo.horizonStart, imageInfo.horizonEnd);
    00329
    00330   imageInfo.vertLine = imageInfo.horizon;
    00331   imageInfo.vertLine.direction.x = -imageInfo.vertLine.direction.y;
    00332   imageInfo.vertLine.direction.y = imageInfo.horizon.direction.x;
    00333
    00334   // Make up the rotation matrix from the direction vector of the horizon (1st column)
    00335   // and the direction vector of the vertical line (2nd column).
    00336   imageInfo.rotation.c[0] = imageInfo.horizon.direction;
    00337   imageInfo.rotation.c[1] = imageInfo.vertLine.direction;
    00338   imageInfo.invRotation = imageInfo.rotation.invert();
    00339
    00340   // Compute the distance of the top left corner from the horizon. This is the same as
    00341   // the y-coordinate of the points on the horizon in the horizon-aligned coordinate
    00342   // system.
    00343   imageInfo.distanceTopLeftCorner = (imageInfo.invRotation*imageInfo.horizon.base).y;
    00344
    00345
    00346   // Reset horizontal counters (??)
    00347   noRedCount = noBlueCount = 100;
    00348   closestBottom = 40000;
    00349   goalAtBorder = false;
    00350
    00351
    00352   // Scan through image
    00353   scanColumns();
    00354
    00355   // finish clustering (??)
    00356   for(int i = 0; i < 3; ++i)
    00357   {
    00358     clusterRobots(0,false,false);
    00359   }
    00360
    00361   scanRows();
    00362
    00363   // Signalize end of scanning to specialists
    00364   goalSpecialistY.notifyAboutFinish();
    00365   goalSpecialistB.notifyAboutFinish();
    00366
    00367
    00368   circleFinder.determineCirclePoint();
    00369   Vector2<double> circleMidPoint;
    00370   lineFinder.execute(linesPercept, cameraMatrix, image, imageInfo, robotPose, circleFinder.getCircle(circleMidPoint), circleMidPoint);
    00371
    00372   DEBUG_DRAWING_FINISHED(circlePoints_image);
    00373   DEBUG_DRAWING_FINISHED(circlePoints_Field);
    00374
    00375
    00376   /*
    00377   DEBUG_RESPONSE("gt05-ip:enable-edge-detection",
    00378     // get line-point percepts from specialist
    00379     //edgeSpecialist.getEdgePointPercepts(linesPercept, cameraMatrix, imageInfo.previousCameraMatrix, image);
    00380     // get edge percepts from specialist
    00381     edgeSpecialist.getEdgesPercept(edgesPercept, cameraMatrix, imageInfo.previousCameraMatrix, image);
    00382   );
    00383   */
    00384
    00385   // clears the multiple ball percept list
    00386   ballSpecialist.resetMultiplePerceptsList(ballPercept);
    00387
    00388
    00389   // Analyze results
    00390   DEBUG_RESPONSE_NOT("gt05-ip:disable-multiple-ball-candidates",
    00391     if (longestBallRun > 2)
    00392     {
    00393       int numberOfCandidates = ballClustering.getSize();
    00394       for (int i=0; i<numberOfCandidates; i++)
    00395       {
    00396         Vector2<double> temp = ballClustering.getMidpoint(i);
    00397         ballCandidate.x = (int)temp.x;
    00398           ballCandidate.y = (int)temp.y;
    00399           DOT(imageProcessor_ball3, ballCandidate.x, ballCandidate.y, Drawings::black, Drawings::orange);
    00400         ballSpecialist.searchBall(image, *bestColorTable, cameraMatrix, imageInfo.previousCameraMatrix,
    00401                                           ballCandidate.x, ballCandidate.y, ballPercept);
    00402
    00403         //CRASH_CHECK(i);
    00404       }
    00405           DEBUG_DRAWING_FINISHED(imageProcessor_multipleBallPercepts);
    00406           DEBUG_DRAWING_FINISHED(multipleBallPerceptsField);
    00407     }
    00408   );
    00409   DEBUG_RESPONSE("gt05-ip:disable-multiple-ball-candidates",
    00410     if (longestBallRun > 2)
    00411     {
    00412       ballSpecialist.searchBall(image, *bestColorTable, cameraMatrix, imageInfo.previousCameraMatrix,
    00413                                       ballCandidate.x, ballCandidate.y, ballPercept);
    00414     }
    00415   );
    00416
    00417   ballSpecialist.forwardPercept(ballPercept);
    00418
    00419   //calculate the panning velocity value for the MultipleBallPerceptList
    00420   ballPercept.multiplePercepts.calculatePanningVelocityValue(panningVelocity);
    00421
    00422   // is not needed any more, because fieldLine-percepts now come from recognized lines
    00423   // -- reverted --
    00424   filterPercepts();
    00425
    00426   landmarksPercept.cameraOffset = cameraMatrix.translation;
    00427   if(imageInfo.horizonInImage)
    00428   {
    00429     beaconDetector.execute();
    00430   }
    00431
    00432   // Drawing stuff
    00433   if(imageInfo.horizonInImage)
    00434   {
    00435     LINE(imageProcessor_horizon,
    00436      imageInfo.horizonStart.x, imageInfo.horizonStart.y, imageInfo.horizonEnd.x,
    00437      imageInfo.horizonEnd.y, 1, Drawings::ps_solid, Drawings::white );
    00438   }
    00439   DEBUG_DRAWING_FINISHED(imageProcessor_horizon);
    00440   DEBUG_DRAWING_FINISHED(imageProcessor_scanLines);
    00441   DEBUG_DRAWING_FINISHED(imageProcessor_general);
    00442   DEBUG_DRAWING_FINISHED(imageProcessor_edges);
    00443
    00444   SEND_DEBUG_IMAGE(imageProcessorPlayers);
    00445   GENERATE_DEBUG_IMAGE(imageProcessorGeneral,
    00446     Image correctedImage(image);
    00447     colorCorrector.correct(correctedImage);
    00448     INIT_DEBUG_IMAGE(imageProcessorGeneral, correctedImage);
    00449   )
    00450   SEND_DEBUG_IMAGE(imageProcessorGeneral);
    00451
    00452   GENERATE_DEBUG_IMAGE(segmentedImage1,
    00453     Image correctedImage(image);
    00454     colorCorrector.correct(correctedImage);
    00455     (*bestColorTable).generateColorClassImage(correctedImage, segmentedImage1ColorClassImage);
    00456   )
    00457   SEND_DEBUG_COLOR_CLASS_IMAGE(segmentedImage1);
    00458
    00459   GENERATE_DEBUG_IMAGE(imageProcessorGradients,
    00460     SUSANEdgeDetectionLite edgeDetectionU(VLCBeaconDetector::edgeThresholdU);
    00461     SUSANEdgeDetectionLite edgeDetectionV(VLCBeaconDetector::edgeThresholdV);
    00462     Image edgeDetectionImage;
    00463     bool edgeY;
    00464     bool edgeU;
    00465     bool edgeV;
    00466     edgeDetectionImage.cameraInfo = image.cameraInfo;
    00467     for (int y=1; y<edgeDetectionImage.cameraInfo.resolutionHeight-1; y++)
    00468     {
    00469       for (int x=1; x<edgeDetectionImage.cameraInfo.resolutionWidth-1; x++)
    00470       {
    00471         edgeY = edgeDetectionU.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentA);
    00472         edgeU = edgeDetectionU.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentB);
    00473         edgeV = edgeDetectionV.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentC);
    00474         if (edgeU&&edgeV)
    00475         {
    00476           edgeDetectionImage.image[y][0][x] = 210;
    00477           edgeDetectionImage.image[y][1][x] = 127;
    00478           edgeDetectionImage.image[y][2][x] = 127;
    00479         }
    00480         else if (edgeU)
    00481         {
    00482           edgeDetectionImage.image[y][0][x] = 130;
    00483           edgeDetectionImage.image[y][1][x] = 170;
    00484           edgeDetectionImage.image[y][2][x] = 80;
    00485         }
    00486         else if (edgeV)
    00487         {
    00488           edgeDetectionImage.image[y][0][x] = 130;
    00489           edgeDetectionImage.image[y][1][x] = 80;
    00490           edgeDetectionImage.image[y][2][x] = 170;
    00491         }
    00492         else if (edgeY)
    00493         {
    00494           edgeDetectionImage.image[y][0][x] = 255;
    00495           edgeDetectionImage.image[y][1][x] = 127;
    00496           edgeDetectionImage.image[y][2][x] = 127;
    00497         }
    00498         else
    00499         {
    00500           edgeDetectionImage.image[y][0][x] = image.image[y][0][x]/2;
    00501           edgeDetectionImage.image[y][1][x] = image.image[y][1][x];
    00502           edgeDetectionImage.image[y][2][x] = image.image[y][2][x];
    00503         }
    00504         edgeDetectionImage.image[y][3][x] = 128;
    00505         edgeDetectionImage.image[y][4][x] = 128;
    00506         edgeDetectionImage.image[y][5][x] = 128;
    00507       }
    00508     }
    00509     INIT_DEBUG_IMAGE(imageProcessorGradients, edgeDetectionImage);
    00510   )
    00511   SEND_DEBUG_IMAGE(imageProcessorGradients);
    00512
    00513   GENERATE_DEBUG_IMAGE(imageProcessorBall,
    00514     Image correctedImage(image);
    00515     colorCorrector.correct(correctedImage);
    00516     for (int iPBix=0; iPBix<correctedImage.cameraInfo.resolutionWidth; iPBix++)
    00517     {
    00518       for (int iPBiy=0; iPBiy<correctedImage.cameraInfo.resolutionHeight; iPBiy++)
    00519       {
    00520         correctedImage.image[iPBiy][0][iPBix]=ballSpecialist.getSimilarityToOrange(correctedImage.image[iPBiy][0][iPBix],correctedImage.image[iPBiy][1][iPBix],correctedImage.image[iPBiy][2][iPBix]);
    00521         if (correctedImage.image[iPBiy][0][iPBix]==0)
    00522         {
    00523           correctedImage.image[iPBiy][1][iPBix]=127;
    00524           correctedImage.image[iPBiy][2][iPBix]=127;
    00525         }
    00526         else
    00527         {
    00528           if (correctedImage.image[iPBiy][0][iPBix]>30)
    00529             correctedImage.image[iPBiy][1][iPBix]=0;
    00530           else
    00531             correctedImage.image[iPBiy][1][iPBix]=255;
    00532           if (correctedImage.image[iPBiy][0][iPBix]>60)
    00533             correctedImage.image[iPBiy][2][iPBix]=0;
    00534           else
    00535             correctedImage.image[iPBiy][2][iPBix]=255;
    00536         }
    00537       }
    00538     }
    00539     INIT_DEBUG_IMAGE(imageProcessorBall, correctedImage);
    00540   )
    00541   SEND_DEBUG_IMAGE(imageProcessorBall);
    00542   DEBUG_DRAWING_FINISHED(imageProcessor_ball4);
    00543
    00544   //for(i = 0; i < LinesPercept::numberOfLineTypes; ++i)
    00545   //    numberOfPoints[i] = linesPercept.numberOfPoints[i];
    00546   // output number of found percepts
    00547   /*OUTPUT(idText,text,"VLCImageProcessor: percept-counting " << endl
    00548     << "\tflags: " << landmarksPercept.numberOfFlags << endl
    00549     << "\tgoals: " << landmarksPercept.numberOfGoals << endl
    00550     << "\tlines: " << linesPercept.numberOfPoints[1] << endl
    00551     << "\tballs: " << ballPercept.ballWasSeen << endl
    00552     << "\tblue:  " << playersPercept.numberOfBluePlayers << endl
    00553     << "\tred:   " << playersPercept.numberOfRedPlayers << endl
    00554     << "\tobst:  " << obstaclesPercept.numberOfPoints
    00555   );*/
    00556
    00557   imageInfo.previousCameraMatrix = cameraMatrix;
    00558   prevCmTricot = cmTricot;
    00559   /*OUTPUT(idText, text, "---");
    00560   for(int ii = 0; ii < linesPercept.numberOfPoints[1]; ++ii)
    00561     OUTPUT(idText, text, int(linesPercept.points[1][ii].angle * 180 / pi));*/
    00562
    00563
    00564 //////// New Debug Images
    00565   N_GENERATE_DEBUG_IMAGE(colorCorrected,
    00566     colorCorrectedImage = image;
    00567     colorCorrector.correct(colorCorrectedImage);
    00568   );
    00569   N_SEND_DEBUG_IMAGE_AS_JPEG(colorCorrected)
    00570
    00571   N_GENERATE_DEBUG_IMAGE(colorCorrected2,
    00572     colorCorrected2Image = image;
    00573     colorCorrector.correct(colorCorrected2Image);
    00574   );
    00575   N_SEND_DEBUG_IMAGE(colorCorrected2);
    00576
    00577   N_GENERATE_DEBUG_IMAGE(segmented,
    00578     Image colorCorrectedImage(image);
    00579     colorCorrector.correct(colorCorrectedImage);
    00580     colorTable.generateColorClassImage(colorCorrectedImage, segmentedImage);
    00581   );
    00582   N_SEND_DEBUG_COLOR_CLASS_IMAGE(segmented)
    00583
    00584   N_GENERATE_DEBUG_IMAGE(edgeDetection,
    00585     SUSANEdgeDetectionLite edgeDetectionU(VLCBeaconDetector::edgeThresholdU);
    00586     SUSANEdgeDetectionLite edgeDetectionV(VLCBeaconDetector::edgeThresholdV);
    00587     bool edgeY;
    00588     bool edgeU;
    00589     bool edgeV;
    00590     edgeDetectionImage.cameraInfo = image.cameraInfo;
    00591     for (int y=1; y<edgeDetectionImage.cameraInfo.resolutionHeight-1; y++)
    00592     {
    00593       for (int x=1; x<edgeDetectionImage.cameraInfo.resolutionWidth-1; x++)
    00594       {
    00595         edgeY = edgeDetectionU.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentA);
    00596         edgeU = edgeDetectionU.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentB);
    00597         edgeV = edgeDetectionV.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentC);
    00598         if (edgeU&&edgeV)
    00599         {
    00600           edgeDetectionImage.image[y][0][x] = 210;
    00601           edgeDetectionImage.image[y][1][x] = 127;
    00602           edgeDetectionImage.image[y][2][x] = 127;
    00603         }
    00604         else if (edgeU)
    00605         {
    00606           edgeDetectionImage.image[y][0][x] = 130;
    00607           edgeDetectionImage.image[y][1][x] = 170;
    00608           edgeDetectionImage.image[y][2][x] = 80;
    00609         }
    00610         else if (edgeV)
    00611         {
    00612           edgeDetectionImage.image[y][0][x] = 130;
    00613           edgeDetectionImage.image[y][1][x] = 80;
    00614           edgeDetectionImage.image[y][2][x] = 170;
    00615         }
    00616         else if (edgeY)
    00617         {
    00618           edgeDetectionImage.image[y][0][x] = 255;
    00619           edgeDetectionImage.image[y][1][x] = 127;
    00620           edgeDetectionImage.image[y][2][x] = 127;
    00621         }
    00622         else
    00623         {
    00624           edgeDetectionImage.image[y][0][x] = image.image[y][0][x]/2;
    00625           edgeDetectionImage.image[y][1][x] = image.image[y][1][x];
    00626           edgeDetectionImage.image[y][2][x] = image.image[y][2][x];
    00627         }
    00628         edgeDetectionImage.image[y][3][x] = 128;
    00629         edgeDetectionImage.image[y][4][x] = 128;
    00630         edgeDetectionImage.image[y][5][x] = 128;
    00631       }
    00632     }
    00633   );
    00634   N_SEND_DEBUG_IMAGE_AS_JPEG(edgeDetection);
    00635
    00636   N_GENERATE_DEBUG_IMAGE(ball,
    00637     ballImage = image;
    00638     colorCorrector.correct(ballImage);
    00639     for (int iPBix=0; iPBix<ballImage.cameraInfo.resolutionWidth; iPBix++)
    00640     {
    00641       for (int iPBiy=0; iPBiy<ballImage.cameraInfo.resolutionHeight; iPBiy++)
    00642       {
    00643         ballImage.image[iPBiy][0][iPBix]=ballSpecialist.getSimilarityToOrange(ballImage.image[iPBiy][0][iPBix],ballImage.image[iPBiy][1][iPBix],ballImage.image[iPBiy][2][iPBix]);
    00644         if (ballImage.image[iPBiy][0][iPBix]==0)
    00645         {
    00646           ballImage.image[iPBiy][1][iPBix]=127;
    00647           ballImage.image[iPBiy][2][iPBix]=127;
    00648         }
    00649         else
    00650         {
    00651           if (ballImage.image[iPBiy][0][iPBix]>30)
    00652             ballImage.image[iPBiy][1][iPBix]=0;
    00653           else
    00654             ballImage.image[iPBiy][1][iPBix]=255;
    00655           if (ballImage.image[iPBiy][0][iPBix]>60)
    00656             ballImage.image[iPBiy][2][iPBix]=0;
    00657           else
    00658             ballImage.image[iPBiy][2][iPBix]=255;
    00659         }
    00660       }
    00661     }
    00662   );
    00663   N_SEND_DEBUG_IMAGE_AS_JPEG(ball);
    00664
    00665   N_SEND_DEBUG_GRAY_SCALE_IMAGE(scanLines);
    00666 }
    00667
    00668 void VLCImageProcessor::scanColumns()
    00669 {
    00670   /** NB: HX and HY in variable names denote x- and y-values in the horizon-aligned
    00671       coordinate system. */
    00672
    00673
    00674   // Compute the y coordinate (in ha-coordinates) of the starting point of the scanlines
    00675   double scanLineStartHY = imageInfo.distanceTopLeftCorner - verticalScanLineTopAboveHorizon;
    00676
    00677   // Compute the leftmost scanline
    00678   // The leftmost scanline is determined either through the top left corner of the image
    00679   // or the bottom left corner. To decide which one is more left, both points are
    00680   // transformed into the horizon-aligned coordinate system and then the x-coordinates are
    00681   // compared.
    00682   // The following facts make the code more efficient yet less readable:
    00683   //  - The coodinates of the top left corner is (0, 0) in the horizon-aligned coordinate
    00684   //    system.
    00685   //  - As for the bottom right corner, the coordinate transformation only needs to be done
    00686   //    for the x-values, and as the x-values is zero (in image coordinates), the
    00687   //    transformation shrinks to a multiplication with one entry of the roation matrix.
    00688   //  NB: There is no translation between the horzion-aligned and the image coordinate
    00689   //  system.
    00690   double bottomLeftHX = imageInfo.invRotation.c[1].x * static_cast<double>(imageInfo.maxImageCoordinates.y);
    00691   Geometry::Line scanLine(
    00692     imageInfo.rotation * Vector2<double>(min(0.0, bottomLeftHX), scanLineStartHY),
    00693     imageInfo.vertLine.direction);
    00694
    00695     // Compute the horizontal offset between scanlines
    00696   Vector2<double> scanLineOffset(imageInfo.horizon.direction.x * verticalScanLineSpacing, imageInfo.horizon.direction.y * verticalScanLineSpacing);
    00697
    00698   // Compute the offset to the endpoints for the short scanlines near the horizon
    00699   Vector2<double> scanLineEnd2(imageInfo.vertLine.direction.x * verticalScanLineLength2, imageInfo.vertLine.direction.y * verticalScanLineLength2);
    00700   Vector2<double> scanLineEnd13(imageInfo.vertLine.direction.x * verticalScanLineLength13, imageInfo.vertLine.direction.y * verticalScanLineLength13);
    00701
    00702
    00703   // Flags for different specialists
    00704   bool relevantForLines;
    00705   int pixelsRelevantForGoal;
    00706
    00707   Vector2<int> topBorderIntersection, bottomBorderIntersection;
    00708   Vector2<int> topEndpoint, bottomEndpoint;
    00709
    00710   // For each scanline from left to right
    00711   for (int i = 0; i<=80; ++i)
    00712   {
    00713     // Shift scanline to the right
    00714     scanLine.base += scanLineOffset;
    00715
    00716     // Intersect scanline with image
    00717     if(!Geometry::getIntersectionPointsOfLineAndRectangle(Vector2<int>(0,0), imageInfo.maxImageCoordinates,
    00718           scanLine, topBorderIntersection, bottomBorderIntersection))
    00719     {
    00720       // End loop if no more intersection with image
    00721             break;
    00722     }
    00723
    00724     // The scanlines are actually rays or line segments. The just calculated intersection
    00725     // points however assume a line (infinite in both direcations). Thus we need to
    00726     // check, if the rectangle would also have intersected with the ray or line segment.
    00727     // If yes, the intersection point is used as endpoint for image scanning, if no, use
    00728     // the endpoint of the scanline.
    00729     // To do the checks, the intersection points are transformed into the horizon-aligned
    00730     // coordinate system, in which the scanlines are parallel to the y-axis. Then we can
    00731     // simply compare the y-coordinates.
    00732     double topBorderHY = imageInfo.invRotation.c[0].y * topBorderIntersection.x
    00733                + imageInfo.invRotation.c[1].y * topBorderIntersection.y;
    00734     double bottomBorderHY = imageInfo.invRotation.c[0].y * bottomBorderIntersection.x
    00735                 + imageInfo.invRotation.c[1].y * bottomBorderIntersection.y;
    00736
    00737     // Check if scanline ray/line segment is entirely below the image
    00738     if (bottomBorderHY < scanLineStartHY)
    00739     {
    00740       // Skip scanline
    00741       continue;
    00742     }
    00743
    00744     // Select upper endpoint
    00745     if (topBorderHY > scanLineStartHY)
    00746     {
    00747       // The start point of scanline is outside the image, so use the intersection point
    00748       topEndpoint = topBorderIntersection;
    00749     }
    00750     else
    00751     {
    00752       // Take the start point of the scanline
    00753             topEndpoint.x = static_cast<int>(scanLine.base.x);
    00754       topEndpoint.y = static_cast<int>(scanLine.base.y);
    00755       ASSERT(!Geometry::clipPointInsideRectange(Vector2<int>(0,0), imageInfo.maxImageCoordinates, topEndpoint));
    00756     }
    00757
    00758
    00759     // The lower endpoint depends on the length of the scanline
    00760         switch (i & 3)
    00761     {
    00762     // Short line segment
    00763     case 1:
    00764     case 3:
    00765       // Skip if line segment is entirely above the image
    00766       if (topBorderHY > scanLineStartHY + verticalScanLineLength13)
    00767         continue;
    00768
    00769       // Select lower endpoint
    00770       if (bottomBorderHY < scanLineStartHY + verticalScanLineLength13)
    00771       {
    00772         // The end point of the scanline is outside the image, so use the intersection
    00773                 bottomEndpoint = bottomBorderIntersection;
    00774       }
    00775       else
    00776       {
    00777         // Use the end point of the scanline
    00778         bottomEndpoint.x = static_cast<int>(scanLine.base.x + scanLineEnd13.x);
    00779         bottomEndpoint.y = static_cast<int>(scanLine.base.y + scanLineEnd13.y);
    00780         ASSERT(!Geometry::clipPointInsideRectange(Vector2<int>(0,0), imageInfo.maxImageCoordinates, bottomEndpoint));
    00781       }
    00782
    00783       // Scanline is not relevant for field line detection
    00784       relevantForLines = false;
    00785       break;
    00786
    00787
    00788     // Medium line segment
    00789     case 2:
    00790       // Skip if line segment is entirely above the image
    00791       if (topBorderHY > scanLineStartHY + verticalScanLineLength2)
    00792         continue;
    00793
    00794       // Select lower endpoint
    00795       if (bottomBorderHY < scanLineStartHY + verticalScanLineLength2)
    00796       {
    00797         // The end point of the scanline is outside the image, so use the intersection
    00798                 bottomEndpoint = bottomBorderIntersection;
    00799       }
    00800       else
    00801       {
    00802         // Use the end point of the scanline
    00803         bottomEndpoint.x = static_cast<int>(scanLine.base.x + scanLineEnd2.x);
    00804         bottomEndpoint.y = static_cast<int>(scanLine.base.y + scanLineEnd2.y);
    00805         ASSERT(!Geometry::clipPointInsideRectange(Vector2<int>(0,0), imageInfo.maxImageCoordinates, bottomEndpoint));
    00806       }
    00807
    00808       // Scanline is not relevant for field line detection
    00809       relevantForLines = false;
    00810       break;
    00811
    00812
    00813     // Ray (infinite length)
    00814     case 0:
    00815     default:
    00816       // Always use intersection point
    00817       bottomEndpoint = bottomBorderIntersection;
    00818
    00819       // Relevant for field line detection
    00820       relevantForLines = true;
    00821     }
    00822
    00823
    00824     /** The goal detection should be only be done near the horizon. Therefore I calculate
    00825       the (maximum) number of pixels on the current scanline that should be considered.
    00826       NB: Assumes the usage of the Bresenham line algorithm when scanning the lines. */
    00827
    00828     if (i & 1)
    00829     {
    00830       // Entire scanline, so just take a number greater than the number of pixels on the line
    00831       pixelsRelevantForGoal = 1024;
    00832     }
    00833     else
    00834     {
    00835       if (topBorderHY > scanLineStartHY + verticalScanLineLength13)
    00836       {
    00837         // Relevant part of the scanline is entirely outside (above) the image
    00838         pixelsRelevantForGoal = -1;
    00839       }
    00840       else
    00841       {
    00842         // Compute the bottom point for goal detection (might be outside the bottom of the image, but that doesn't matter)
    00843         int goalBottomX = static_cast<int>(scanLine.base.x + scanLineEnd13.x);
    00844         int goalBottomY = static_cast<int>(scanLine.base.y + scanLineEnd13.y);
    00845
    00846         // The number of pixels is the maximum of the differences of the coordinates
    00847         pixelsRelevantForGoal = 1 + max(abs(goalBottomX-topEndpoint.x), abs(goalBottomY-topEndpoint.y));
    00848       }
    00849     }
    00850
    00851     // Scan the image on the line between top and bottom endpoint
    00852     scan(topEndpoint, bottomEndpoint, true, !relevantForLines, pixelsRelevantForGoal);
    00853   }
    00854 }
    00855
    00856
    00857 void VLCImageProcessor::scanRows()
    00858 {
    00859   Geometry::Line scanLine(imageInfo.horizon);
    00860   double horizontalScanLineSpacing(20.0);
    00861   Vector2<double> scanOffset(imageInfo.vertLine.direction * horizontalScanLineSpacing);
    00862   scanLine.base += (scanOffset*2.0);
    00863   Vector2<int> startPoint,
    00864                  endPoint;
    00865   //Test if the horizon is inside the image
    00866   if(!Geometry::getIntersectionPointsOfLineAndRectangle(Vector2<int>(0,0),
    00867                                                         imageInfo.maxImageCoordinates,
    00868                                                         scanLine, startPoint, endPoint))
    00869   {
    00870     scanLine.base.x = (double)imageInfo.maxImageCoordinates.x / 2.0;
    00871     scanLine.base.y = 1;
    00872   }
    00873   while(true)
    00874   {
    00875     if(Geometry::getIntersectionPointsOfLineAndRectangle(
    00876                                         Vector2<int>(0,0),
    00877                                         imageInfo.maxImageCoordinates,
    00878                                         scanLine, startPoint, endPoint))
    00879     {
    00880       LINE(imageProcessor_general,startPoint.x,startPoint.y,endPoint.x,endPoint.y,
    00881        1,Drawings::ps_solid, Drawings::green);
    00882       if(rand() > RAND_MAX / 2)
    00883         scan(startPoint, endPoint, false, false, -1);
    00884       else
    00885         scan(startPoint, endPoint, false, false, -1);
    00886       scanLine.base += scanOffset;
    00887     }
    00888     else
    00889     {
    00890       return;
    00891     }
    00892   }
    00893 }
    00894
    00895 void VLCImageProcessor::scan(const Vector2<int>& start, const Vector2<int>& end,
    00896                                 bool vertical, bool noLines, int pixelsRelevantForGoal)
    00897 {
    00898   // Signalize the begin of a new scanline
    00899   if (vertical)
    00900   {
    00901     // GoalSpecialist only works on vertical scanlines
    00902     goalSpecialistY.notifyAboutNewScanline(start);
    00903     goalSpecialistB.notifyAboutNewScanline(start);
    00904   }
    00905
    00906   /*
    00907   DEBUG_RESPONSE("gt05-ip:enable-edge-detection",
    00908     edgeSpecialist.resetLine();
    00909   );
    00910   */
    00911
    00912   LinesPercept::LineType typeOfLinesPercept = LinesPercept::field;
    00913   int pixelsSinceGoal = 0;
    00914   int pixelsBetweenGoalAndObstacle = 0;
    00915
    00916   Vector2<int> actual = start,
    00917         diff = end - start,
    00918         step(sgn(diff.x),sgn(diff.y)),
    00919         size(abs(diff.x),abs(diff.y));
    00920   bool isVertical = abs(diff.y) > abs(diff.x);
    00921   int count,
    00922       sum;
    00923   double scanAngle = diff.angle();
    00924
    00925   LINE(imageProcessor_scanLines,
    00926       start.x, start.y, end.x, end.y,
    00927       1, Drawings::ps_solid, Drawings::gray );
    00928
    00929   // init Bresenham
    00930   if(isVertical)
    00931   {
    00932     count = size.y;
    00933     sum = size.y / 2;
    00934   }
    00935   else
    00936   {
    00937     count = size.x;
    00938     sum = size.x / 2;
    00939   }
    00940
    00941   if(count > 7)
    00942   {
    00943     int numberOfPoints[LinesPercept::numberOfLineTypes],
    00944         i;
    00945     for(i = 0; i < LinesPercept::numberOfLineTypes; ++i)
    00946       numberOfPoints[i] = linesPercept.points[i].numberOfPoints;
    00947     RingBuffer<const unsigned char*,7> pixelBuffer;
    00948     RingBuffer<const unsigned char*,7> pixelBufferFromUp;
    00949     RingBuffer<unsigned char, 7> yBuffer;
    00950     RingBuffer<unsigned char, 7> vBuffer;
    00951     RingBuffer<colorClass, 7> colorClassBuffer;
    00952
    00953     //the image is scanned vertically
    00954     for(i = 0; i < 6; ++i) // fill buffer
    00955     {
    00956       unsigned char y,u,v;
    00957       pixelBuffer.add(&image.image[actual.y][0][actual.x]);
    00958       if (actual.x < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y < 0 || actual.y >= image.cameraInfo.resolutionHeight)
    00959         y = u = v = 0;
    00960       else
    00961       {
    00962         y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    00963         u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    00964         v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    00965       }
    00966       yBuffer.add(y);
    00967       vBuffer.add(v);
    00968       colorClass color = COLOR_CLASS(y, u, v, (*bestColorTable));
    00969       colorClassBuffer.add(color);
    00970
    00971       /** Let specialists inspect the current pixel
    00972           (NB: current [dt: aktuell], NB [lat: nota bene] [dt: Hinweis]) */
    00973       /*
    00974       DEBUG_RESPONSE("gt05-ip:enable-edge-detection",
    00975         edgeSpecialist.checkPoint(actual, color, cameraMatrix, imageInfo.previousCameraMatrix, image);
    00976       );
    00977       */
    00978
    00979       // GoalSpecialist only works close to the horizon
    00980       if (i < pixelsRelevantForGoal)
    00981       {
    00982         goalSpecialistY.inspectPixel(actual, color);
    00983         goalSpecialistB.inspectPixel(actual, color);
    00984       }
    00985
    00986
    00987       // Bresenham
    00988         if(isVertical)
    00989         {
    00990           actual.y += step.y;
    00991           sum += size.x;
    00992            if(sum >= size.y)
    00993           {
    00994             sum -= size.y;
    00995             actual.x += step.x;
    00996           }
    00997         }
    00998         else
    00999         {
    01000           actual.x += step.x;
    01001           sum += size.y;
    01002           if(sum >= size.x)
    01003           {
    01004             sum -= size.x;
    01005             actual.y += step.y;
    01006           }
    01007         }
    01008     }
    01009     lineColor = Drawings::numberOfColors;
    01010
    01011     int redCount = 0,
    01012         blueCount = 0,
    01013         greenCount = 0,
    01014         noGreenCount = 100,
    01015         pinkCount = 0,
    01016         noPinkCount = 100,
    01017         yellowCount = 0,
    01018         noYellowCount = 100,
    01019         skyblueCount = 0,
    01020         noSkyblueCount = 100,
    01021         orangeCount = 0,
    01022         noOrangeCount = 100;
    01023     const unsigned char* firstRed = 0,
    01024                        * lastRed = 0,
    01025                        * firstBlue = 0,
    01026                        * lastBlue = 0,
    01027                        * firstGreen = 0,
    01028                        * firstOrange = 0,
    01029                        * lastOrange = 0,
    01030                        * firstPink = 0,
    01031                        * lastPink = 0,
    01032                        * pFirst = pixelBuffer[2],
    01033                        * up = pFirst;
    01034     enum{justUP, greenAndUP, down} state = justUP;
    01035     bool borderFound = false; // there can be only one border point per scan line
    01036
    01037     // obstacles
    01038     enum{noGroundFound, foundBeginOfGround, foundEndOfGround} groundState = noGroundFound;
    01039     Vector2<int> firstGround(0,0);
    01040     int numberOfPixelsSinceFirstOccurrenceOfGroundColor = 0;
    01041     int numberOfPixelsSinceFirstOccurrenceOfNonGroundColor = 0;
    01042     int numberOfPixelsSinceLastGround = 0;
    01043     int numberOfWhitePixelsSinceLastGround = 0;
    01044     int numberOfGroundPixels = 0;
    01045     int numberOfWhiteObstaclePixels = 0;
    01046     int numberOfNotWhiteObstaclePixels = 0;
    01047     bool beginOfGroundIsAtTheTopOfTheImage = false;
    01048
    01049     struct BorderCandidate
    01050     {
    01051       bool found;
    01052       Vector2<int> pointOnField;
    01053       Vector2<int> pointInImage;
    01054       double angleOnField;
    01055       double angleInImage;
    01056     };
    01057     BorderCandidate borderCandidate;
    01058     borderCandidate.found = false;
    01059
    01060     for(; i <= count; ++i)
    01061     {
    01062       pixelsSinceGoal++;
    01063       numberOfScannedPixels++;
    01064       unsigned char y,u,v;
    01065       pixelBuffer.add(&image.image[actual.y][0][actual.x]);
    01066       if (actual.x < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y < 0 || actual.y >= image.cameraInfo.resolutionHeight)
    01067         y = u = v = 0;
    01068       else
    01069       {
    01070         y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    01071         u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    01072         v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    01073       }
    01074       yBuffer.add(y);
    01075       vBuffer.add(v);
    01076       colorClass color = COLOR_CLASS(y, u, v, (*bestColorTable));
    01077       colorClassBuffer.add(color);
    01078
    01079       // for VLC
    01080       averageY += y;
    01081       averageU += u;
    01082       averageV += v;
    01083       //
    01084
    01085       N_SET_COLORED_PIXEL_IN_GRAY_SCALE_IMAGE(scanLines, actual.x, actual.y, color);
    01086
    01087       /** Let specialists inspect the current pixel. */
    01088       /*
    01089       DEBUG_RESPONSE("gt05-ip:enable-edge-detection",
    01090         edgeSpecialist.checkPoint(actual, color, cameraMatrix, imageInfo.previousCameraMatrix, image);
    01091       );
    01092       */
    01093
    01094     // GoalSpecialist only works close to the horizon
    01095       if (i < pixelsRelevantForGoal)
    01096       {
    01097         goalSpecialistY.inspectPixel(actual, color);
    01098         goalSpecialistB.inspectPixel(actual, color);
    01099       }
    01100
    01101
    01102       // line point state machine
    01103       const unsigned char* p3 = pixelBuffer[3];
    01104       //UP: y-component increases ////////////////////////////////////////
    01105       if(yBuffer[3] > yBuffer[4] + yThreshold)
    01106       {
    01107         up = p3;
    01108         // is there green in one of the pixels above?
    01109         if(colorClassBuffer[6] == green
    01110           || colorClassBuffer[5] == green
    01111           || colorClassBuffer[4] == green)
    01112         {
    01113           Vector2<int> coords = getCoords(p3),
    01114                        pointOnField; //position on field, relative to robot
    01115           pixelBufferFromUp.init();
    01116           if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
    01117             for(int k = pixelBuffer.getNumberOfEntries() - 1; k >= 0; --k)
    01118                pixelBufferFromUp.add(pixelBuffer.getEntry(k));
    01119           state = greenAndUP;
    01120         }
    01121         else
    01122           state = justUP;
    01123       }
    01124
    01125       //DOWN: y-component decreases //////////////////////////////////////
    01126       else if(yBuffer[3] <
    01127               yBuffer[4] - yThreshold ||
    01128               vBuffer[3] <
    01129               vBuffer[4] - vThreshold)
    01130       {
    01131         // is the pixel in the field ??
    01132         if(state != down && // was an "up" pixel above?
    01133            (colorClassBuffer[0] == green
    01134            || colorClassBuffer[1] == green
    01135            || colorClassBuffer[2] == green)) // is there green in one of the pixels below ?
    01136         {
    01137           colorClass c = colorClassBuffer[6];
    01138           if(!noLines || c == skyblue || c == yellow)
    01139           {
    01140             Vector2<int> coords = getCoords(p3),
    01141                          pointOnField; //position on field, relative to robot
    01142
    01143             if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
    01144             {
    01145               Vector2<int> upCoords = getCoords(up);
    01146               int height = (coords - upCoords).abs();
    01147               int distance = (int) sqrt(sqr(cameraMatrix.translation.z) + sqr(pointOnField.abs())),
    01148                 lineHeight = (int) Geometry::getSizeByDistance(image.cameraInfo, 25, distance);
    01149
    01150               //The bright region is assumed to be on the ground. If it is small enough, it is a field line.
    01151               if(height < 3 * lineHeight && state == greenAndUP &&
    01152                  (colorClassBuffer[5] == white ||
    01153                   colorClassBuffer[4] == white))
    01154               {
    01155                 double edgeAngleImage, edgeAngleField;
    01156                 Vector2<int> pointOnField2;
    01157                 if(pixelBufferFromUp.getNumberOfEntries() > 0 &&
    01158                    calcEdgeAngle(edgeAngleImage, edgeAngleField, coords, pointOnField, scanAngle, pixelBuffer) &&
    01159                    Geometry::calculatePointOnField(upCoords.x, upCoords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField2))
    01160                 {
    01161                   double upAngleImage, upAngleField;
    01162                   if(calcEdgeAngle(upAngleImage, upAngleField, upCoords, pointOnField2, scanAngle, pixelBufferFromUp, true))
    01163                   {
    01164                     // only add the closest field line. This makes filtering easier.
    01165                     linesPercept.points[LinesPercept::field].numberOfPoints = numberOfPoints[LinesPercept::field];
    01166                     linesPercept.add(LinesPercept::field, pointOnField2, upCoords, upAngleField, upAngleImage);
    01167                     linesPercept.add(LinesPercept::field, pointOnField, coords, edgeAngleField, edgeAngleImage);
    01168                   }
    01169                 }
    01170               }
    01171               else if(height >= 3 && !borderFound)
    01172               {
    01173                 switch(c)
    01174                 {
    01175                   case skyblue:
    01176                     if(vertical)
    01177                     {
    01178                       borderFound = true;
    01179                       if(getPlayer().getTeamColor() == Player::red && !linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints)
    01180                         goalAtBorder = pointOnField.abs() < closestBottom;
    01181                       closestBottom = 40000;
    01182                       double edgeAngleField, edgeAngleImage;
    01183                       if(calcEdgeAngle(edgeAngleImage, edgeAngleField, coords, pointOnField, scanAngle, pixelBuffer, false, false, 2))
    01184                         linesPercept.add(LinesPercept::skyblueGoal, pointOnField, coords, edgeAngleField, edgeAngleImage);
    01185                       typeOfLinesPercept = LinesPercept::skyblueGoal;
    01186                       pixelsSinceGoal = 0;
    01187
    01188                       lastRed = lastBlue = 0;
    01189                       redCount = blueCount = 0;
    01190                     }
    01191                     break;
    01192                   case yellow:
    01193                     if(vertical)
    01194                     {
    01195                       borderFound = true;
    01196                       if(getPlayer().getTeamColor() == Player::blue && !linesPercept.points[LinesPercept::yellowGoal].numberOfPoints)
    01197                         goalAtBorder = pointOnField.abs() < closestBottom;
    01198                       closestBottom = 40000;
    01199                       double edgeAngleField, edgeAngleImage;
    01200                       if(calcEdgeAngle(edgeAngleImage, edgeAngleField, coords, pointOnField, scanAngle, pixelBuffer, false, false))
    01201                         linesPercept.add(LinesPercept::yellowGoal, pointOnField, coords, edgeAngleField, edgeAngleImage);
    01202                       typeOfLinesPercept = LinesPercept::yellowGoal;
    01203                       pixelsSinceGoal = 0;
    01204
    01205                       lastRed = lastBlue = 0;
    01206                       redCount = blueCount = 0;
    01207                     }
    01208                     break;
    01209                   case white:
    01210                     if(height > lineHeight * 3 && (vertical || height > 30))
    01211                     {
    01212                       //borderFound = true;
    01213                       double edgeAngleField, edgeAngleImage;
    01214                       if(calcEdgeAngle(edgeAngleImage, edgeAngleField, coords, pointOnField, scanAngle, pixelBuffer))
    01215                       {
    01216                         //linesPercept.add(LinesPercept::border,pointOnField, edgeAngle);
    01217                         borderCandidate.found = true;
    01218                         borderCandidate.pointOnField = pointOnField;
    01219                         borderCandidate.pointInImage = coords;
    01220                         borderCandidate.angleOnField = edgeAngleField;
    01221                         borderCandidate.angleInImage = edgeAngleImage;
    01222                       }
    01223                       //typeOfLinesPercept = LinesPercept::border;
    01224                       lastRed = lastBlue = 0;
    01225                       redCount = blueCount = 0;
    01226                     }
    01227                     break;
    01228                 }
    01229               }
    01230             }
    01231           }
    01232           state = down;
    01233         }
    01234       }
    01235
    01236       colorClass c3 = colorClassBuffer[3];
    01237       ++noOrangeCount;
    01238       if (c3 == orange)
    01239       {
    01240         if(noOrangeCount > 1)
    01241         {
    01242           if (orangeCount > longestBallRun)
    01243           {
    01244             longestBallRun = orangeCount;
    01245             ballCandidate = (getCoords(firstOrange) + getCoords(lastOrange)) / 2;
    01246           }
    01247 DEBUG_RESPONSE_NOT("gt05-ip:disable-multiple-ball-candidates",
    01248           if (orangeCount>2)
    01249           {
    01250             ballClustering.addLine(getCoords(firstOrange), getCoords(lastOrange), orangeCount);
    01251           }
    01252 );
    01253           orangeCount = 1;
    01254           firstOrange = p3;
    01255           lastOrange = p3;
    01256         }
    01257         else
    01258         {
    01259           ++orangeCount;
    01260           if(orangeCount > 2) // ignore robot if ball is below, distance would be wrong
    01261           {
    01262             lastRed = lastBlue = 0;
    01263             redCount = blueCount = 0;
    01264           }
    01265           lastOrange = p3;
    01266         }
    01267         firstGreen = 0;
    01268         noOrangeCount = 0;
    01269       }
    01270       if(vertical)
    01271       {
    01272         // in scanColumns, recognize player and ball points
    01273         ++noGreenCount;
    01274         ++noPinkCount;
    01275         ++noYellowCount;
    01276         ++noSkyblueCount;
    01277         switch(c3)
    01278         {
    01279           case blue: // count blue, remember last
    01280             if(blueCount == 3)
    01281               firstBlue = pixelBuffer[6];
    01282             ++blueCount;
    01283             lastBlue = p3;
    01284             firstGreen = 0;
    01285             break;
    01286           case gray: // drop green above gray (i.e. robot legs)
    01287             if(firstGreen && noGreenCount < 8)
    01288               firstGreen = 0;
    01289             break;
    01290           case green: // find first green below a robot
    01291             if(!firstGreen)
    01292             {
    01293               firstGreen = p3;
    01294             }
    01295             if(noGreenCount > 6)
    01296               greenCount = 1;
    01297             else
    01298               ++greenCount;
    01299             noGreenCount = 0;
    01300             break;
    01301           case red:
    01302             if(orangeCount < 6 || noOrangeCount > 4 || redCount > orangeCount)
    01303             { // count red, remember last
    01304               if(redCount == 3)
    01305                 firstRed = pixelBuffer[6];
    01306               ++redCount;
    01307               lastRed = p3;
    01308               firstGreen = 0;
    01309               break;
    01310             }
    01311             // no break, red below orange is interpreted as orange
    01312           case pink:
    01313             if(noPinkCount > 6)
    01314             {
    01315               pinkCount = 1;
    01316               firstPink = p3;
    01317             }
    01318             else
    01319             {
    01320               ++pinkCount;
    01321             }
    01322             lastPink = p3;
    01323             noPinkCount = 0;
    01324             break;
    01325           case yellow:
    01326             if(noYellowCount > 6)
    01327               yellowCount = 1;
    01328             else
    01329               ++yellowCount;
    01330             noYellowCount = 0;
    01331             break;
    01332           case skyblue:
    01333             if(noSkyblueCount > 6)
    01334               skyblueCount = 1;
    01335             else
    01336               ++skyblueCount;
    01337             noSkyblueCount = 0;
    01338             break;
    01339           default:
    01340             break;
    01341
    01342         }//switch(c3)
    01343
    01344         // obstacles state machine
    01345         if(count > 90 && cameraMatrix.isValid)
    01346         {
    01347           DEBUG_IMAGE_SET_PIXEL_BLACK(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
    01348           colorClass color = colorClassBuffer[0];
    01349           if(groundState == noGroundFound)
    01350           {
    01351             DEBUG_IMAGE_SET_PIXEL_YELLOW(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
    01352             if(color == green || color == orange)
    01353             {
    01354               if(numberOfPixelsSinceFirstOccurrenceOfGroundColor == 0)
    01355               {
    01356                 firstGround = getCoords(pixelBuffer[0]);
    01357               }
    01358               numberOfPixelsSinceFirstOccurrenceOfGroundColor++;
    01359               if(numberOfPixelsSinceFirstOccurrenceOfGroundColor > 3 || color == orange)
    01360               {
    01361                 Vector2<int> coords = getCoords(pixelBuffer[0]);
    01362                 int lineHeight = Geometry::calculateLineSize(coords, cameraMatrix, image.cameraInfo);
    01363                 if(i < lineHeight * 4) beginOfGroundIsAtTheTopOfTheImage = true;
    01364                 groundState = foundBeginOfGround;
    01365                 pixelsBetweenGoalAndObstacle = pixelsSinceGoal;
    01366                 numberOfPixelsSinceFirstOccurrenceOfNonGroundColor = 0;
    01367               }
    01368             }
    01369             else if (color != noColor)
    01370             {
    01371               numberOfPixelsSinceFirstOccurrenceOfGroundColor = 0;
    01372               if(color == white) numberOfWhiteObstaclePixels++;
    01373               else numberOfNotWhiteObstaclePixels++;
    01374             }
    01375           }
    01376           else if(groundState == foundBeginOfGround)
    01377           {
    01378             DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
    01379             if(color != green && color != orange && color != noColor)
    01380             {
    01381               numberOfPixelsSinceFirstOccurrenceOfNonGroundColor++;
    01382               if(numberOfPixelsSinceFirstOccurrenceOfNonGroundColor > 3)
    01383               {
    01384                 groundState = foundEndOfGround;
    01385                 numberOfPixelsSinceLastGround = 0;
    01386                 numberOfWhitePixelsSinceLastGround = 0;
    01387                 numberOfPixelsSinceFirstOccurrenceOfGroundColor = 0;
    01388               }
    01389             }
    01390             else if (color != noColor)
    01391             {
    01392               numberOfGroundPixels++;
    01393               numberOfPixelsSinceFirstOccurrenceOfNonGroundColor = 0;
    01394             }
    01395           }
    01396           else if(groundState == foundEndOfGround)
    01397           {
    01398             numberOfPixelsSinceLastGround++;
    01399             if(color == white) numberOfWhitePixelsSinceLastGround++;
    01400             DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
    01401             if(color == green || color == orange)
    01402             {
    01403               numberOfPixelsSinceFirstOccurrenceOfGroundColor++;
    01404               if(numberOfPixelsSinceFirstOccurrenceOfGroundColor > 3 || color == orange)
    01405               {
    01406                 numberOfPixelsSinceFirstOccurrenceOfNonGroundColor = 0;
    01407                 groundState = foundBeginOfGround;
    01408                 Vector2<int> coords = getCoords(pixelBuffer[0]),
    01409                              pointOnField; //position on field, relative to robot
    01410                 int lineHeight = Geometry::calculateLineSize(coords, cameraMatrix, image.cameraInfo);
    01411                 // * 4 wenn 80% weiß (schräge linie). sonst * 1.5 (keine linie)
    01412                 if(numberOfPixelsSinceLastGround > lineHeight * (numberOfPixelsSinceLastGround * 4 < numberOfWhitePixelsSinceLastGround * 5 ? 4 : 1.5))
    01413                 {
    01414                   DEBUG_IMAGE_SET_PIXEL_PINK(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
    01415                   firstGround = getCoords(pixelBuffer[0]);
    01416                   numberOfGroundPixels = 0;
    01417                   beginOfGroundIsAtTheTopOfTheImage = false;
    01418                 }
    01419               }
    01420             }
    01421             else if (color != noColor)
    01422             {
    01423               numberOfPixelsSinceFirstOccurrenceOfGroundColor = 0;
    01424               if(color == white)
    01425                 numberOfWhiteObstaclePixels++;
    01426               else
    01427                 numberOfNotWhiteObstaclePixels++;
    01428             }
    01429           }
    01430         } // if(count > 100)
    01431
    01432       }//if(vertical)
    01433
    01434       PLOT(p3,ColorClasses::colorClassToDrawingsColor(c3));
    01435
    01436       // Bresenham
    01437       if(isVertical)
    01438       {
    01439         actual.y += step.y;
    01440         sum += size.x;
    01441         if(sum >= size.y)
    01442         {
    01443           sum -= size.y;
    01444           actual.x += step.x;
    01445         }
    01446       }
    01447       else
    01448       {
    01449         actual.x += step.x;
    01450         sum += size.y;
    01451         if(sum >= size.x)
    01452         {
    01453           sum -= size.x;
    01454           actual.y += step.y;
    01455         }
    01456       }
    01457     }
    01458
    01459     //if(!borderFound && borderCandidate.found)
    01460     //{
    01461     //  linesPercept.add(LinesPercept::border,borderCandidate.pointOnField, borderCandidate.pointInImage, borderCandidate.angleOnField, borderCandidate.angleInImage);
    01462     //}
    01463
    01464     if (orangeCount > longestBallRun)
    01465     {
    01466       longestBallRun = orangeCount;
    01467       ballCandidate = (getCoords(firstOrange) + getCoords(lastOrange)) / 2;
    01468     }
    01469 DEBUG_RESPONSE_NOT("gt05-ip:disable-multiple-ball-candidates",
    01470     if (orangeCount>2)
    01471     {
    01472       ballClustering.addLine(getCoords(firstOrange), getCoords(lastOrange), orangeCount);
    01473     }
    01474 );
    01475
    01476     const unsigned char* pLast = pixelBuffer[3];
    01477     PLOT(pLast,Drawings::numberOfColors);
    01478
    01479     if(vertical)
    01480     { // line scanning finished, analyze results (only in scanColumns)
    01481       int goal = getPlayer().getTeamColor() == Player::red ? LinesPercept::skyblueGoal
    01482                                                            : LinesPercept::yellowGoal;
    01483       if(linesPercept.points[goal].numberOfPoints == numberOfPoints[goal]) // no goal point found in this column
    01484       {
    01485         Vector2<int> coords = getCoords(pLast),
    01486                      pointOnField;
    01487         if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
    01488         {
    01489           int dist = pointOnField.abs();
    01490           if(dist < closestBottom)
    01491             closestBottom = dist;
    01492         }
    01493       }
    01494
    01495       bool redFound = false,
    01496            blueFound = false;
    01497
    01498       // red robot point found?
    01499       if(redCount > blueCount && (firstGreen && redCount > 2 || redCount > 20))
    01500       { // drop border and goal points
    01501         for(i = 1; i < LinesPercept::numberOfLineTypes; ++i)
    01502           linesPercept.points[i].numberOfPoints = numberOfPoints[i];
    01503         if(redCount > 20)
    01504         { // if robot is close, use upper boundary of tricot to calculate distance
    01505           Vector2<int> coords = getCoords(firstRed),
    01506                        pointOnField; //position on field, relative to robot
    01507           if(Geometry::calculatePointOnField(coords.x, coords.y, cmTricot, prevCmTricot, image.cameraInfo, pointOnField) &&
    01508              pointOnField.abs() < 500)
    01509           {
    01510             DOT(imageProcessor_general, coords.x, coords.y, Drawings::red, Drawings::white);
    01511             linesPercept.add(LinesPercept::redRobot,pointOnField,coords);
    01512             typeOfLinesPercept = LinesPercept::redRobot;
    01513             redFound = true;
    01514           }
    01515         }
    01516         if(!redFound)
    01517         { // otherwise, use foot point
    01518           Vector2<int> coords = getCoords(firstGreen),
    01519                        pointOnField; //position on field, relative to robot
    01520           if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
    01521           {
    01522             Vector2<int> redCoords = getCoords(lastRed);
    01523             if((redCoords - coords).abs() < Geometry::getSizeByDistance(image.cameraInfo, 100, pointOnField.abs()))
    01524             {
    01525               linesPercept.add(LinesPercept::redRobot,pointOnField,coords);
    01526               typeOfLinesPercept = LinesPercept::redRobot;
    01527               redFound = true;
    01528             }
    01529           }
    01530         }
    01531       }
    01532       // blue robot point found?
    01533       else if(blueCount > redCount && (firstGreen && blueCount > 2 || blueCount > 10))
    01534       { // drop border and goal points
    01535         for(i = 1; i < LinesPercept::numberOfLineTypes; ++i)
    01536           linesPercept.points[i].numberOfPoints = numberOfPoints[i];
    01537         if(blueCount > 10)
    01538         { // if robot is close, use upper boundary of tricot to calculate distance
    01539           Vector2<int> coords = getCoords(firstBlue),
    01540                        pointOnField; //position on field, relative to robot
    01541           if(Geometry::calculatePointOnField(coords.x, coords.y, cmTricot, prevCmTricot, image.cameraInfo, pointOnField) &&
    01542              pointOnField.abs() < 500)
    01543           {
    01544             DOT(imageProcessor_general, coords.x, coords.y, Drawings::pink, Drawings::white);
    01545             linesPercept.add(LinesPercept::blueRobot,pointOnField,coords);
    01546             typeOfLinesPercept = LinesPercept::blueRobot;
    01547             blueFound = true;
    01548           }
    01549         }
    01550         if(!blueFound)
    01551         { // otherwise, use foot point
    01552           Vector2<int> coords = getCoords(firstGreen),
    01553                        pointOnField; //position on field, relative to robot
    01554           if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
    01555           {
    01556             Vector2<int> blueCoords = getCoords(lastBlue);
    01557             if((blueCoords - coords).abs() < Geometry::getSizeByDistance(image.cameraInfo, 100, pointOnField.abs()))
    01558             {
    01559               linesPercept.add(LinesPercept::blueRobot,pointOnField,coords);
    01560               typeOfLinesPercept = LinesPercept::blueRobot;
    01561               blueFound = true;
    01562             }
    01563           }
    01564         }
    01565       }
    01566       // cluster the robot points found
    01567       clusterRobots(pLast, redFound, blueFound);
    01568
    01569       // obstacles found?
    01570       bool addObstaclesPoint = false;
    01571       ObstaclesPercept::Segment s;
    01572       if(count > 90 && cameraMatrix.isValid)
    01573       {
    01574         Vector2<int> firstGroundOnField;
    01575         bool firstGroundOnFieldIsOnField =
    01576           Geometry::calculatePointOnField(firstGround.x, firstGround.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, firstGroundOnField);
    01577
    01578         Vector2<int> imageBottom = getCoords(pixelBuffer[0]);
    01579         Vector2<int> imageBottomOnField;
    01580         bool imageBottomIsOnField =
    01581           Geometry::calculatePointOnField(imageBottom.x, imageBottom.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, imageBottomOnField);
    01582
    01583         if(groundState == foundBeginOfGround)
    01584         {
    01585           if(firstGroundOnFieldIsOnField)
    01586           {
    01587             addObstaclesPoint = true;
    01588             s.nearPointOnField = Vector2<double>(imageBottomOnField.x, imageBottomOnField.y);
    01589             s.farPointOnField = Vector2<double>(firstGroundOnField.x, firstGroundOnField.y);
    01590             s.nearPointInImage = Vector2<double>(imageBottom.x, imageBottom.y);
    01591             s.farPointInImage = Vector2<double>(firstGround.x, firstGround.y);
    01592             s.farPointIsOnImageBorder = beginOfGroundIsAtTheTopOfTheImage;
    01593           }
    01594         }
    01595         else if(groundState == foundEndOfGround)
    01596         {
    01597           int lineHeight = Geometry::calculateLineSize(imageBottom, cameraMatrix, image.cameraInfo);
    01598           if(numberOfPixelsSinceLastGround < lineHeight * 4)
    01599           {
    01600             if(firstGroundOnFieldIsOnField)
    01601             {
    01602               addObstaclesPoint = true;
    01603               s.nearPointOnField = Vector2<double>(imageBottomOnField.x, imageBottomOnField.y);
    01604               s.farPointOnField = Vector2<double>(firstGroundOnField.x, firstGroundOnField.y);
    01605               s.nearPointInImage = Vector2<double>(imageBottom.x, imageBottom.y);
    01606               s.farPointInImage = Vector2<double>(firstGround.x, firstGround.y);
    01607               s.farPointIsOnImageBorder = beginOfGroundIsAtTheTopOfTheImage;
    01608             }
    01609           }
    01610           else if(imageBottomIsOnField)
    01611           {
    01612             addObstaclesPoint = true;
    01613             s.nearPointOnField = s.farPointOnField = Vector2<double>(imageBottomOnField.x, imageBottomOnField.y);
    01614             s.nearPointInImage = s.farPointInImage = Vector2<double>(imageBottom.x, imageBottom.y);
    01615           }
    01616         }
    01617         else if(groundState == noGroundFound)
    01618         {
    01619           if(imageBottomIsOnField &&
    01620             // the carpet often is not green under the robot
    01621             imageBottomOnField.abs() > 150)
    01622           {
    01623             addObstaclesPoint = true;
    01624             s.nearPointOnField = s.farPointOnField = Vector2<double>(imageBottomOnField.x, imageBottomOnField.y);
    01625             s.nearPointInImage = s.farPointInImage = Vector2<double>(imageBottom.x, imageBottom.y);
    01626           }
    01627         }
    01628       }// if(count > 90 && cameraMatrix.isValid)
    01629       if(addObstaclesPoint)
    01630       {
    01631         if(
    01632           angleBetweenDirectionOfViewAndGround > -80.0 &&
    01633           !(s.farPointOnField.x == 0 && s.farPointOnField.y == 0)
    01634           )
    01635
    01636         {
    01637           switch(typeOfLinesPercept)
    01638           {
    01639           case LinesPercept::skyblueGoal:
    01640             if(pixelsBetweenGoalAndObstacle < 15)
    01641               s.obstacleType = ObstaclesPercept::goal;
    01642             break;
    01643           case LinesPercept::yellowGoal:
    01644             if(pixelsBetweenGoalAndObstacle < 15)
    01645               s.obstacleType = ObstaclesPercept::goal;
    01646             break;
    01647           case LinesPercept::blueRobot:
    01648             if(getPlayer().getTeamColor() == Player::blue)
    01649               s.obstacleType = ObstaclesPercept::teammate;
    01650             else
    01651               s.obstacleType = ObstaclesPercept::opponent;
    01652             break;
    01653           case LinesPercept::redRobot:
    01654             if(getPlayer().getTeamColor() == Player::red)
    01655               s.obstacleType = ObstaclesPercept::teammate;
    01656             else
    01657               s.obstacleType = ObstaclesPercept::opponent;
    01658             break;
    01659           //case LinesPercept::border:
    01660           //  s.obstacleType = ObstaclesPercept::border;
    01661           }
    01662
    01663           obstaclesPercept.add(s);
    01664         }
    01665       }
    01666     }//if(vertical)
    01667   }
    01668 }
    01669
    01670 void VLCImageProcessor::clusterRobots(const unsigned char* bottomPoint,
    01671                                          bool redFound, bool blueFound)
    01672 {
    01673   Vector2<int> coords = getCoords(bottomPoint),
    01674                pointOnField;
    01675   int noRedCount2 = noRedCount;
    01676
    01677   if(redFound)
    01678   {
    01679     lastRed =
    01680       linesPercept.points[LinesPercept::redRobot].pointsOnField[linesPercept.points[LinesPercept::redRobot].numberOfPoints - 1];
    01681     if(noRedCount > 2)
    01682       firstRed = closestRed = lastRed;
    01683     else if(closestRed.abs() > lastRed.abs())
    01684       closestRed = lastRed;
    01685     noRedCount = 0;
    01686   }
    01687   // count number of columns without robot points, but ignore columns that are
    01688   // not long enough to contain the robot. Drop single robot points of the other
    01689   // robot color.
    01690   else if(noRedCount > 2 || !bottomPoint ||
    01691           (Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField) &&
    01692           closestRed.abs() > pointOnField.abs()))
    01693   {
    01694     if(++noRedCount == 3 && (firstRed != lastRed || noBlueCount > 4))
    01695     {
    01696       SinglePlayerPercept percept;
    01697       percept.directionOnField = (atan2((double)firstRed.y,(double)firstRed.x) + atan2((double)lastRed.y,(double)lastRed.x)) / 2;
    01698       double distance = closestRed.abs();
    01699       percept.offsetOnField.x = cos(percept.directionOnField) * distance;
    01700       percept.offsetOnField.y = sin(percept.directionOnField) * distance;
    01701       percept.positionInImage = coords;
    01702       percept.validity = 1;
    01703       playersPercept.addRedPlayer(percept);
    01704     }
    01705   }
    01706
    01707   if(blueFound)
    01708   {
    01709     lastBlue =
    01710       linesPercept.points[LinesPercept::blueRobot].pointsOnField[linesPercept.points[LinesPercept::blueRobot].numberOfPoints - 1];
    01711     if(noBlueCount > 2)
    01712       firstBlue = closestBlue = lastBlue;
    01713     else if(closestBlue.abs() > lastBlue.abs())
    01714       closestBlue = lastBlue;
    01715     noBlueCount = 0;
    01716   }
    01717   // count number of columns without robot points, but ignore columns that are
    01718   // not long enough to contain the robot. Drop single robot points of the other
    01719   // robot color.
    01720   else if(noBlueCount > 2 || !bottomPoint ||
    01721           (Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField) &&
    01722           closestBlue.abs() > pointOnField.abs()))
    01723   {
    01724     if(++noBlueCount == 3 && (firstBlue != lastBlue || noRedCount2 > 4))
    01725     {
    01726       SinglePlayerPercept percept;
    01727       percept.directionOnField = (atan2((double)firstBlue.y,(double)firstBlue.x) + atan2((double)lastBlue.y,(double)lastBlue.x)) / 2;
    01728       double distance = closestBlue.abs();
    01729       percept.offsetOnField.x = int(cos(percept.directionOnField) * distance);
    01730       percept.offsetOnField.y = int(sin(percept.directionOnField) * distance);
    01731       percept.positionInImage = coords;
    01732       percept.validity = 1;
    01733       playersPercept.addBluePlayer(percept);
    01734     }
    01735   }
    01736 }
    01737
    01738 void VLCImageProcessor::filterPercepts()
    01739 {
    01740   // Close robots produce far ghost robots. Remove them.
    01741   int i;
    01742   for(i = 0; i < playersPercept.numberOfRedPlayers + playersPercept.numberOfBluePlayers; ++i)
    01743   {
    01744     SinglePlayerPercept& pi = i < playersPercept.numberOfRedPlayers
    01745                       ? playersPercept.redPlayers[i]
    01746                       : playersPercept.bluePlayers[i - playersPercept.numberOfRedPlayers];
    01747     double iDist = pi.offsetOnField.abs(),
    01748            iAngle = atan2(pi.offsetOnField.y, pi.offsetOnField.x),
    01749            ignoreAngle = atan2(150, iDist);
    01750     for(int j = 0; j < playersPercept.numberOfRedPlayers + playersPercept.numberOfBluePlayers; ++j)
    01751       if(i != j)
    01752       {
    01753         SinglePlayerPercept& pj = j < playersPercept.numberOfRedPlayers
    01754                             ? playersPercept.redPlayers[j]
    01755                             : playersPercept.bluePlayers[j - playersPercept.numberOfRedPlayers];
    01756         if(iDist < pj.offsetOnField.abs() &&
    01757            fabs(atan2(pj.offsetOnField.y, pj.offsetOnField.x) - iAngle) < ignoreAngle)
    01758         {
    01759           if(j < playersPercept.numberOfRedPlayers)
    01760             pj = playersPercept.redPlayers[--playersPercept.numberOfRedPlayers];
    01761           else
    01762             pj = playersPercept.bluePlayers[--playersPercept.numberOfBluePlayers];
    01763           if(i > j)
    01764           {
    01765             i = j; // check pi again
    01766             break;
    01767           }
    01768           else
    01769             --j; // check pj again
    01770         }
    01771       }
    01772   }
    01773
    01774   // If there are too few line points of a certain type, remove them all, they may be misreadings
    01775   for(i = 0; i < LinesPercept::numberOfLineTypes; ++i)
    01776     if(linesPercept.points[i].numberOfPoints < 3)
    01777       linesPercept.points[i].numberOfPoints = 0;
    01778
    01779   // Remove outliers in the border
    01780   //filterLinesPercept(linesPercept, LinesPercept::border, image);
    01781   /*
    01782   // First run: find them
    01783   int prev = 0;
    01784   for(i = 1; i < linesPercept.numberOfPoints[LinesPercept::border] - 1; ++i)
    01785   {
    01786     LinesPercept::LinePoint& pPrev = linesPercept.points[LinesPercept::border][prev],
    01787                            & p = linesPercept.points[LinesPercept::border][i],
    01788                            & pNext = linesPercept.points[LinesPercept::border][i + 1];
    01789
    01790     // Field wall is convex. So, no point can be closer than its two neighbors
    01791     int dist = p.abs();
    01792     if(pPrev.abs() > dist && pNext.abs() > dist)
    01793       p.angle = LinesPercept::UNDEF;
    01794     else
    01795       ++prev;
    01796   }
    01797
    01798   // second run: remove marked points
    01799   prev = 0;
    01800   for(i = 1; i < linesPercept.numberOfPoints[LinesPercept::border] - 1; ++i)
    01801     if(linesPercept.points[LinesPercept::border][i].angle != LinesPercept::UNDEF)
    01802       linesPercept.points[LinesPercept::border][prev++] = linesPercept.points[LinesPercept::border][i];
    01803   linesPercept.numberOfPoints[LinesPercept::border] = prev;
    01804   */
    01805
    01806   // Remove outliers in the field lines
    01807   filterLinesPercept(linesPercept, LinesPercept::field, image);
    01808
    01809 #ifdef PERCEPT_FILTER
    01810
    01811   char numberOfFreePartsOfGoals = 0;
    01812
    01813   if (obstaclesPercept.angleToFreePartOfGoalWasDetermined[0]) numberOfFreePartsOfGoals++;
    01814   if (obstaclesPercept.angleToFreePartOfGoalWasDetermined[1]) numberOfFreePartsOfGoals++;
    01815
    01816   // there can only be one goal in a frame
    01817   if (landmarksPercept.numberOfGoals == 2)
    01818   {
    01819     landmarksPercept.numberOfGoals = 0;
    01820     obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
    01821     obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
    01822     //OUTPUT(idText, text, "2 goals in one frame");
    01823   }
    01824
    01825   if (numberOfFreePartsOfGoals > landmarksPercept.numberOfGoals)
    01826   {
    01827     obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
    01828     obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
    01829   }
    01830
    01831   // there is a maximum of two flags in a frame
    01832   if (landmarksPercept.numberOfFlags > 2)
    01833   {
    01834     landmarksPercept.numberOfFlags = 0;
    01835     //OUTPUT(idText, text, "3+ flags in one frame");
    01836   }
    01837
    01838   if (landmarksPercept.numberOfFlags > 0)
    01839   {
    01840     // find invalid combinations of goal and flag
    01841     if (landmarksPercept.numberOfGoals == 1)
    01842     {
    01843       if (landmarksPercept.goals[0].color == skyblue)
    01844       {
    01845         for (unsigned char i = 0;i<landmarksPercept.numberOfFlags;i++)
    01846           if ((landmarksPercept.flags[i].type == Flag::pinkAboveYellow) ||
    01847             (landmarksPercept.flags[i].type == Flag::yellowAbovePink))
    01848           {
    01849             //OUTPUT(idText, text, "blue goal and a yellow flag in one frame");
    01850             landmarksPercept.numberOfGoals = 0;
    01851             landmarksPercept.numberOfFlags = 0;
    01852             obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
    01853             obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
    01854           }
    01855       }
    01856       else
    01857       {
    01858         for (unsigned char i = 0;i<landmarksPercept.numberOfFlags;i++)
    01859           if ((landmarksPercept.flags[i].type == Flag::pinkAboveSkyblue) ||
    01860             (landmarksPercept.flags[i].type == Flag::skyblueAbovePink))
    01861           {
    01862             //OUTPUT(idText, text, "yellow goal and a blue flag in one frame");
    01863             landmarksPercept.numberOfGoals = 0;
    01864             landmarksPercept.numberOfFlags = 0;
    01865             obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
    01866             obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
    01867           }
    01868       }
    01869     }
    01870
    01871     // two mutually exclusive flags
    01872     if (landmarksPercept.numberOfFlags == 2)
    01873     {
    01874       switch(landmarksPercept.flags[0].type)
    01875       {
    01876       case Flag::pinkAboveSkyblue:
    01877         switch(landmarksPercept.flags[1].type)
    01878         {
    01879         case Flag::yellowAbovePink:
    01880           //OUTPUT(idText, text, "pinkAboveSkyblue and yellowAbovePink Flag in one frame");
    01881           landmarksPercept.numberOfFlags = 0;
    01882           break;
    01883         case Flag::skyblueAbovePink:
    01884           if (landmarksPercept.flags[0].angle > landmarksPercept.flags[1].angle)
    01885           {
    01886             //OUTPUT(idText, text, "pinkAboveSkyblue and skyblueAbovePink Flag in wrong order");
    01887             landmarksPercept.numberOfFlags = 0;
    01888           }
    01889           break;
    01890         case Flag::pinkAboveYellow:
    01891           if (landmarksPercept.flags[0].angle < landmarksPercept.flags[1].angle)
    01892           {
    01893             //OUTPUT(idText, text, "pinkAboveSkyblue and pinkAboveYellow Flag in wrong order");
    01894             landmarksPercept.numberOfFlags = 0;
    01895           }
    01896           break;
    01897         }
    01898         break;
    01899       case Flag::skyblueAbovePink:
    01900         switch(landmarksPercept.flags[1].type)
    01901         {
    01902         case Flag::pinkAboveYellow:
    01903           //OUTPUT(idText, text, "skyblueAbovePink and pinkAboveYellow Flag in one frame");
    01904           landmarksPercept.numberOfFlags = 0;
    01905         case Flag::yellowAbovePink:
    01906           if (landmarksPercept.flags[0].angle > landmarksPercept.flags[1].angle)
    01907           {
    01908             //OUTPUT(idText, text, "skyblueAbovePink and yellowAbovePink Flag in wrong order");
    01909             landmarksPercept.numberOfFlags = 0;
    01910           }
    01911           break;
    01912         case Flag::pinkAboveSkyblue:
    01913           if (landmarksPercept.flags[0].angle < landmarksPercept.flags[1].angle)
    01914           {
    01915             //OUTPUT(idText, text, "skyblueAbovePink and pinkAboveSkyblue Flag in wrong order");
    01916             landmarksPercept.numberOfFlags = 0;
    01917           }
    01918           break;
    01919         }
    01920         break;
    01921       case Flag::yellowAbovePink:
    01922         switch(landmarksPercept.flags[1].type)
    01923         {
    01924         case Flag::pinkAboveSkyblue:
    01925           //OUTPUT(idText, text, "yellowAbovePink and pinkAboveSkyblue Flag in one frame");
    01926           landmarksPercept.numberOfFlags = 0;
    01927           break;
    01928         case Flag::pinkAboveYellow:
    01929           if (landmarksPercept.flags[0].angle > landmarksPercept.flags[1].angle)
    01930           {
    01931             //OUTPUT(idText, text, "yellowAbovePink and pinkAboveYellow Flag in wrong order");
    01932             landmarksPercept.numberOfFlags = 0;
    01933           }
    01934           break;
    01935         case Flag::skyblueAbovePink:
    01936           if (landmarksPercept.flags[0].angle < landmarksPercept.flags[1].angle)
    01937           {
    01938             //OUTPUT(idText, text, "yellowAbovePink and skyblueAbovePink Flag in wrong order");
    01939             landmarksPercept.numberOfFlags = 0;
    01940           }
    01941           break;
    01942         }
    01943         break;
    01944       case Flag::pinkAboveYellow:
    01945         switch(landmarksPercept.flags[1].type)
    01946         {
    01947         case Flag::skyblueAbovePink:
    01948           //OUTPUT(idText, text, "pinkAboveYellow and skyblueAbovePink Flag in one frame");
    01949           landmarksPercept.numberOfFlags = 0;
    01950           break;
    01951         case Flag::pinkAboveSkyblue:
    01952           if (landmarksPercept.flags[0].angle > landmarksPercept.flags[1].angle)
    01953           {
    01954             //OUTPUT(idText, text, "pinkAboveYellow and pinkAboveSkyblue Flag in wrong order");
    01955             landmarksPercept.numberOfFlags = 0;
    01956           }
    01957           break;
    01958         case Flag::yellowAbovePink:
    01959           if (landmarksPercept.flags[0].angle < landmarksPercept.flags[1].angle)
    01960           {
    01961             //OUTPUT(idText, text, "pinkAboveYellow and yellowAbovePink Flag in wrong order");
    01962             landmarksPercept.numberOfFlags = 0;
    01963           }
    01964           break;
    01965         }
    01966         break;
    01967       }
    01968     }
    01969   }
    01970
    01971   // filters impossible relations between flags and goals
    01972   if (landmarksPercept.numberOfFlags != 0)
    01973     if (landmarksPercept.numberOfGoals == 1)
    01974     {
    01975       for (int i = 0;i<landmarksPercept.numberOfFlags;i++)
    01976         if (landmarksPercept.goals[0].color == skyblue)
    01977         {
    01978           // the skyblueAbovePink Flag is on the left side of the blue goal
    01979           if (landmarksPercept.flags[i].type == Flag::skyblueAbovePink)
    01980           {
    01981             if (landmarksPercept.flags[i].angle < (landmarksPercept.goals[0].angle + minAngleBetweenFlagAndGoal))
    01982             {
    01983               //              OUTPUT(idText, text, "skyblueAbovePink flag and blue goal in wrong order");
    01984               landmarksPercept.numberOfFlags = 0;
    01985               landmarksPercept.numberOfGoals = 0;
    01986               obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
    01987               obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
    01988               return;
    01989             }
    01990           }
    01991           else
    01992           {
    01993             // the pinkAboveSkyBlue Flag is on the right side of the blue goal
    01994             if ((landmarksPercept.flags[i].angle + minAngleBetweenFlagAndGoal) > landmarksPercept.goals[0].angle)
    01995             {
    01996               //              OUTPUT(idText, text, "pinkAboveSkyBlue flag and blue goal in wrong order");
    01997               landmarksPercept.numberOfFlags = 0;
    01998               landmarksPercept.numberOfGoals = 0;
    01999               obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
    02000               obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
    02001               return;
    02002             }
    02003           }
    02004         }
    02005         else // yellow goal
    02006         {
    02007           // the yellowAbovePink Flag is on the right side of the yellow goal
    02008           if (landmarksPercept.flags[i].type == Flag::yellowAbovePink)
    02009           {
    02010             if ((landmarksPercept.flags[i].angle + minAngleBetweenFlagAndGoal) > landmarksPercept.goals[0].angle)
    02011             {
    02012               //              OUTPUT(idText, text, "yellowAbovePink flag and yellow goal in wrong order");
    02013               landmarksPercept.numberOfFlags = 0;
    02014               landmarksPercept.numberOfGoals = 0;
    02015               obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
    02016               obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
    02017               return;
    02018             }
    02019           }
    02020           else
    02021           {
    02022             // the pinkAboveYellow Flag is on the left side of the yellow goal
    02023             if (landmarksPercept.flags[i].angle < (landmarksPercept.goals[0].angle + minAngleBetweenFlagAndGoal))
    02024             {
    02025               //              OUTPUT(idText, text, "pinkAboveYellow flag and yellow goal in wrong order");
    02026               landmarksPercept.numberOfFlags = 0;
    02027               landmarksPercept.numberOfGoals = 0;
    02028               obstaclesPercept.angleToFreePartOfGoalWasDetermined[0] = false;
    02029               obstaclesPercept.angleToFreePartOfGoalWasDetermined[1] = false;
    02030               return;
    02031             }
    02032           }
    02033         }
    02034     }
    02035 #endif
    02036   /*
    02037   // First run: find them
    02038   prev = 0;
    02039   for(i = 2; i <= linesPercept.numberOfPoints[LinesPercept::field]; i += 2)
    02040   {
    02041     LinesPercept::LinePoint& pPrev = linesPercept.points[LinesPercept::field][i - 2],
    02042                            & p = linesPercept.points[LinesPercept::field][i];
    02043
    02044     // end of line?
    02045     double angle = fabs(((Vector2<int>&) pPrev).angle() - ((Vector2<int>&) p).angle());
    02046     int dist1 = pPrev.abs(),
    02047         dist2 = p.abs();
    02048     if(i == linesPercept.numberOfPoints[LinesPercept::field] ||
    02049        angle > 0.08 ||
    02050        abs(dist1 - dist2) > 2 * fabs(tan(angle / 2) * (dist1 + dist2)))
    02051     {
    02052       if(i - prev < 6)
    02053         while(prev < i)
    02054           linesPercept.points[LinesPercept::field][prev++].angle = LinesPercept::UNDEF;
    02055       prev = i;
    02056     }
    02057   }
    02058
    02059   // second run: remove marked points
    02060   prev = 0;
    02061   for(i = 0; i < linesPercept.numberOfPoints[LinesPercept::field]; ++i)
    02062     if(linesPercept.points[LinesPercept::field][i].angle != LinesPercept::UNDEF)
    02063       linesPercept.points[LinesPercept::field][prev++] = linesPercept.points[LinesPercept::field][i];
    02064   linesPercept.numberOfPoints[LinesPercept::field] = prev;
    02065   */
    02066 }
    02067
    02068 bool VLCImageProcessor::calcEdgeAngle(
    02069   double& angleInImage,
    02070   double& angleOnField,
    02071   const Vector2<int>& pointInImage,
    02072   const Vector2<int>& pointOnField,
    02073   double scanAngle,
    02074   const RingBuffer<const unsigned char*,7>& pixelBuffer,
    02075   const bool againstScanline,
    02076   const bool borderOrLine,
    02077   int channel)
    02078 {
    02079   // find maximum gradient around point on the scan line
    02080   int indices[] = {3, 4, 2, 5, 1};
    02081   Vector2<double> maxGrad;
    02082   double maxLength = -1;
    02083   for(unsigned int i = 0; i < sizeof(indices) / sizeof(indices[0]); ++i)
    02084   {
    02085     Vector2<int> point = getCoords(pixelBuffer[indices[i]]);
    02086     Vector2<double> grad = Vector2<double>(double(image.image[point.y][channel][point.x]) - image.image[point.y + 1][channel][point.x - 1] ,
    02087       double(image.image[point.y + 1][channel][point.x]) - image.image[point.y][channel][point.x - 1]);
    02088     double length = grad.abs();
    02089
    02090     if(length > maxLength)
    02091     {
    02092       Vector2<double> gradRotated = (rotation2x2 * Vector2<double>(grad.x, -grad.y)).normalize();
    02093       gradRotated.y *= -1;
    02094       // filter gradient which are in wrong direction (belong to other side of line)
    02095       double angle = gradRotated.angle();
    02096       bool similarDirection = (angle > scanAngle && angle < scanAngle + pi) || (angle < scanAngle - pi && angle > scanAngle - pi2);
    02097       if(againstScanline != similarDirection)
    02098       {
    02099         maxGrad = grad;
    02100         maxLength = length;
    02101       }
    02102     }
    02103   }
    02104
    02105   if(maxLength > 0)
    02106   {
    02107     Vector2<double> gradRotated = (rotation2x2 * Vector2<double>(maxGrad.x, -maxGrad.y)).normalize();
    02108     gradRotated.y *= -1;
    02109
    02110     // check whether edge direction differs too much from scan direction
    02111     double diff = fabs(scanAngle - gradRotated.angle());
    02112     if(diff > pi)
    02113       diff = pi2 - diff;
    02114     if(diff < pi / 6 || diff > 5 * pi / 6)
    02115     {
    02116       angleInImage = angleOnField = LinesPercept::UNDEF;
    02117       return false;
    02118     }
    02119     Vector2<int> endPoint(int(pointInImage.x + gradRotated.x * 10), int(pointInImage.y + gradRotated.y * 10));
    02120     ARROW(imageProcessor_edges,
    02121       pointInImage.x,
    02122       pointInImage.y,
    02123       endPoint.x,
    02124       endPoint.y,
    02125       1, 1, (againstScanline ? Drawings::red : Drawings::pink)
    02126     );
    02127
    02128     if (borderOrLine)
    02129     {
    02130       Vector2<int> pointImage = pointInImage;
    02131       Vector2<int> pointField = pointOnField;
    02132 //      circleFinder.addCandidate(pointField,pointImage);
    02133       Vector2<double> normalToLine = gradRotated;
    02134       normalToLine = normalToLine.rotateLeft();
    02135       Vector2<int> pointOnLine = pointInImage;
    02136       lineFinder.considerLinePoint(pointOnLine, normalToLine);
    02137     }
    02138
    02139     Vector2<int> point;
    02140     if(Geometry::calculatePointOnField(endPoint.x, endPoint.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, point))
    02141     {
    02142       angleInImage = normalize((endPoint - pointInImage).angle() - pi_2);
    02143       angleOnField = normalize((point - pointOnField).angle() + pi_2);
    02144       return true;
    02145     }
    02146   }
    02147   angleInImage = angleOnField = LinesPercept::UNDEF;
    02148   return false;
    02149 }
    02150
    02151 void VLCImageProcessor::plot(const unsigned char* p,Drawings::Color color)
    02152 {
    02153   if(lineColor == Drawings::numberOfColors)
    02154   {
    02155     last = p;
    02156     lineColor = color;
    02157   }
    02158   else if(color != lineColor)
    02159   {
    02160     Vector2<int> p1 = getCoords(last),
    02161                  p2 = getCoords(p);
    02162     LINE(imageProcessor_general,p1.x,p1.y,p2.x,p2.y,0,Drawings::ps_solid,lineColor);
    02163     last = p;
    02164     lineColor = color;
    02165   }
    02166 }
    02167
    02168 bool VLCImageProcessor::handleMessage(InMessage& message)
    02169 {
    02170   switch(message.getMessageID())
    02171   {
    02172 /*
    02173     case idLinesImageProcessorParameters:
    02174     {
    02175       GenericDebugData d;
    02176       message.bin >> d;
    02177       yThreshold = (int)d.data[1];
    02178       vThreshold = (int)d.data[2];
    02179       execute();
    02180       return true;
    02181     }
    02182 */
    02183     case idColorTable64: beaconDetector.analyzeColorTable();
    02184       return true;
    02185   }
    02186   return false;
    02187 }
    02188
    02189 void VLCImageProcessor::filterLinesPercept(LinesPercept& percept, int type, const Image& image)
    02190 {
    02191   const int maxNumberOfEdgePoints = 200;
    02192   double maxAngle = 0.25; // 8.6°
    02193   double maxDist = 0.187; // 80°
    02194
    02195   int i, j;
    02196
    02197   // for all lines, calculate how many other lines are similar/near in sense of this special norm
    02198   bool similar[maxNumberOfEdgePoints][maxNumberOfEdgePoints];
    02199   for(i = 0; i < percept.points[type].numberOfPoints; i++)
    02200   {
    02201     Vector2<double> dir1 = Vector2<double>(cos(percept.points[type].pointsOnField[i].angle),sin(percept.points[type].pointsOnField[i].angle));
    02202
    02203     //int lineHeightI = Geometry::calculateLineSize(percept.points[type][i], cameraMatrix, image.cameraInfo);
    02204     for(j = i + 1; j < percept.points[type].numberOfPoints; j++)
    02205     {
    02206       // similar/near if distance is small and normal is in the same direction
    02207       bool sim = false;
    02208       // test projection first - to remove unneeded calculations of "expensive" distance
    02209       double angleDiff = fabs(normalize(percept.points[type].pointsOnField[i].angle - percept.points[type].pointsOnField[j].angle));
    02210       if(angleDiff < maxAngle)
    02211       {
    02212         // distance point to line:
    02213         // http://mo.mathematik.uni-stuttgart.de/kurse/kurs8/seite44.html
    02214         // NOTE: this is because in fact, the line.direction is here the *normal* to the real line
    02215         Vector2<double> v1 = Vector2<double>((double)(percept.points[type].pointsOnField[j] - percept.points[type].pointsOnField[i]).x,(double)(percept.points[type].pointsOnField[j] - percept.points[type].pointsOnField[i]).y).normalize();
    02216         double distance1 = fabs(v1 * dir1);
    02217         if(distance1 < maxDist)
    02218         {
    02219           Vector2<double> dir2 = Vector2<double>(cos(percept.points[type].pointsOnField[j].angle),sin(percept.points[type].pointsOnField[j].angle));
    02220           double distance2 = fabs(-v1 * dir2);
    02221           sim = (distance2 < maxDist);
    02222         }
    02223       }
    02224       similar[i][j] = sim;
    02225       similar[j][i] = sim;
    02226       // draw similarity-lines
    02227       /*Vector2<int> p1,p2;
    02228       Geometry::calculatePointInImage(percept.points[type][i], cameraMatrix, image.cameraInfo, p1);
    02229       Geometry::calculatePointInImage(percept.points[type][j], cameraMatrix, image.cameraInfo, p2);
    02230       if(sim) LINE(imageProcessor_edges,
    02231         p1.x,
    02232         p1.y,
    02233         p2.x,
    02234         p2.y,
    02235         0, 0, 1
    02236       );*/
    02237     }
    02238   }
    02239
    02240   // remove points with low number of similar points
    02241   int nextFree = 0;
    02242
    02243   // for all points, calculate how many other are similar
    02244   for(i = 0; i < percept.points[type].numberOfPoints; i++)
    02245   {
    02246     int countSim = 0;
    02247     //if(edgePoints[i].belongsToLineNo == -1) // only if point is not yet matched to an edge
    02248     for (j = 0; j < percept.points[type].numberOfPoints; j++)
    02249     {
    02250       //if(edgePoints[j].belongsToLineNo == -1) // only if point is not yet matched to an edge
    02251       if(similar[i][j])
    02252       {
    02253         countSim++;
    02254       }
    02255     }
    02256     if(countSim >= 2) // keep percept
    02257     {
    02258       percept.points[type].pointsInImage[nextFree] = percept.points[type].pointsInImage[i];
    02259       percept.points[type].pointsOnField[nextFree++] = percept.points[type].pointsOnField[i];
    02260     }
    02261   }
    02262   percept.points[type].numberOfPoints = nextFree;
    02263 }