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

# Modules/ImageProcessor/SlamImageProcessor/SlamImageProcessor.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file SlamImageProcessor.cpp
    00003 *
    00004 * Implementation of class SlamImageProcessor
    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 */
    00009
    00010 #include "SlamImageProcessor.h"
    00011 #include "Tools/Debugging/Debugging.h"
    00012 #include "Tools/Debugging/GenericDebugData.h"
    00013 #include "Tools/FieldDimensions.h"
    00014 #include "Tools/Math/Geometry.h"
    00015 #include "SlamImageProcessorTools.h"
    00016 #include "Representations/Perception/JPEGImage.h"
    00017
    00018 #include "Representations/Perception/SlamPercept.h"
    00019
    00020 #define slamPercept (*(SlamPercept *)specialPercept.slamData)
    00021
    00022 #define MULTIPLE_BALL_CANDIDATES
    00023 //~ #define EDGES
    00024 #define PLOT(p,c) COMPLEX_DRAWING(imageProcessor_general,plot(p,c))
    00025
    00026 const int Y = 0,
    00027           U = cameraResolutionWidth_ERS7, /**< Relative offset of U component. */
    00028           V = 2 * cameraResolutionWidth_ERS7; /**< Relative offset of V component. */
    00029
    00030 SlamImageProcessor::SlamImageProcessor(const ImageProcessorInterfaces& interfaces)
    00031 : ImageProcessor(interfaces),
    00032 beaconDetector(image, cameraMatrix, imageInfo.previousCameraMatrix, imageInfo, colorTable, colorCorrector, robotPose, landmarksPercept, specialPercept),
    00033 goalRecognizer(image, cameraMatrix, imageInfo.previousCameraMatrix, colorTable, colorCorrector, obstaclesPercept, landmarksPercept),
    00034 ballSpecialist(colorCorrector),
    00035 ballClustering(),
    00036 lineFinder(colorCorrector, colorTable),
    00037 borderFinder(colorCorrector, colorTable)
    00038 //,circleFinder(cameraMatrix, image)
    00039 {
    00040   yThreshold = 15;
    00041   vThreshold = 8;
    00042   beaconDetector.analyzeColorTable();
    00043   double c = cos(-3*pi/4);
    00044   double s = sin(-3*pi/4);
    00045   rotation2x2 = Matrix2x2<double>(
    00046     Vector2<double>(c,-s),
    00047     Vector2<double>(s,c)
    00048   );
    00049   prevCameraMatrixUninitialized = true;
    00050
    00051   count = 0;
    00052
    00053   filterWeight = 0.975;
    00054
    00055   for (int i = 0; i < NUM_PINK; i++)
    00056   {
    00057     pinkCaracteristic[i].count = 0;
    00058     pinkCaracteristic[i].pixel = 0;
    00059   };
    00060 }
    00061
    00062 void SlamImageProcessor::execute()
    00063 {
    00064
    00065   INIT_DEBUG_IMAGE(imageProcessorPlayers, image);
    00066
    00067   NDECLARE_DEBUGDRAWING ("SlamIMP:maxGreenImage", "drawingOnImage", "shows maximal green point");
    00068   NDECLARE_DEBUGDRAWING ("SlamIMP:horizon", "drawingOnImage", "shows horizon");
    00069
    00070   NDECLARE_DEBUGDRAWING ("SlamIMP:maxGreenField", "drawingOnField", "shows maximal green point");
    00071   NDECLARE_DEBUGDRAWING ("SlamIMP:borderPoints", "drawingOnImage", "shows the border points");
    00072
    00073   NRECTANGLE ("SlamIMP:maxGreenImage", 0, 0, 10, 10, 1, Drawings::ps_solid, Drawings::red);
    00074   NRECTANGLE ("SlamIMP:maxGreenField", -100, -100, 100, 100, 50, Drawings::ps_solid, Drawings::red);
    00075
    00076
    00077   xFactor = yFactor = image.cameraInfo.focalLength;
    00078
    00079   numberOfScannedPixels = 0;
    00080
    00081   // reset everything
    00082   landmarksPercept.reset(image.frameNumber);
    00083   linesPercept.reset(image.frameNumber);
    00084   ballPercept.reset(image.frameNumber);
    00085   playersPercept.reset(image.frameNumber);
    00086   obstaclesPercept.reset(image.frameNumber);
    00087 //  circleFinder.reset();
    00088   lineFinder.reset();
    00089   borderFinder.reset();
    00090
    00091   slamPercept.greenLocation.impLengthOfMaxGreen = 0.0;
    00092   slamPercept.greenLocation.impAngleToMaxGreen = 0;
    00093   slamPercept.greenLocation.impAngleToMaxGreenValid = false;
    00094
    00095   averageY = 0;
    00096   averageU = 0;
    00097   averageV = 0;
    00098
    00099   averageCount = 0;
    00100
    00101   slamPercept.imageCharacteristic.numPink = 0;
    00102   slamPercept.imageCharacteristic.angle = cameraMatrix.rotation.getZAngle ();
    00103
    00104 #ifdef MULTIPLE_BALL_CANDIDATES
    00105     ballClustering.reset();
    00106 #endif
    00107
    00108 #ifdef EDGES
    00109   edgesPercept.reset(image.frameNumber);
    00110   edgeSpecialist.reset();
    00111 #endif
    00112
    00113   // variations of the camera matrix for different object heights
    00114   cmTricot = cameraMatrix;
    00115   cmTricot.translation.z -= 100; // upper tricot border
    00116
    00117   if (prevCameraMatrixUninitialized && cameraMatrix.frameNumber != 0)
    00118   {
    00119     imageInfo.previousCameraMatrix = cameraMatrix;
    00120     prevCmTricot = cmTricot;
    00121     prevCameraMatrixUninitialized = false;
    00122   }
    00123
    00124   longestBallRun = 0;
    00125
    00126 //-----------------------------------------------------
    00127   Vector3<double> vectorInDirectionOfViewCamera(1,0,0);
    00128   Vector3<double> vectorInDirectionOfViewWorld;
    00129   vectorInDirectionOfViewWorld = cameraMatrix.rotation * vectorInDirectionOfViewCamera;
    00130
    00131   const int visualizationScale = 20;
    00132   Vector3<double> vectorInDirectionOfViewWorldOld;
    00133   vectorInDirectionOfViewWorldOld = imageInfo.previousCameraMatrix.rotation * vectorInDirectionOfViewCamera;
    00134   Vector3<double> cameraMotionVectorWorld = vectorInDirectionOfViewWorld - vectorInDirectionOfViewWorldOld;
    00135   long frameNumber = cameraMatrix.frameNumber,
    00136     prevFrameNumber = imageInfo.previousCameraMatrix.frameNumber;
    00137   const RobotDimensions& r = getRobotConfiguration().getRobotDimensions();
    00138   double timeDiff = (frameNumber - prevFrameNumber) * r.motionCycleTime; // in seconds
    00139   Vector2<double> currentOnGround(vectorInDirectionOfViewWorld.x, vectorInDirectionOfViewWorld.y),
    00140       oldOnGround(vectorInDirectionOfViewWorldOld.x, vectorInDirectionOfViewWorldOld.y);
    00141   currentOnGround.normalize();
    00142   oldOnGround.normalize();
    00143   Vector3<double> cameraSpeedVectorWorld(0, 0, 0);
    00144   double panningVelocity = 0;
    00145   if (timeDiff > 0) //should never happen otherwise, but better safe than sorry
    00146   {
    00147     cameraSpeedVectorWorld = cameraMotionVectorWorld/timeDiff;
    00148     double cosAng = currentOnGround*oldOnGround;
    00149     if (cosAng > 1.0)
    00150       cosAng = 1.0;
    00151     else
    00152     if (cosAng <-1.0)
    00153       cosAng = -1.0;
    00154     panningVelocity = normalize(acos(cosAng))/timeDiff;
    00155   }
    00156   Vector3<double> cameraSpeedVectorCamera = cameraMatrix.invert().rotation * cameraSpeedVectorWorld;
    00157   Vector2<double> cameraSpeedVectorImg(cameraSpeedVectorCamera.y,
    00158         cameraSpeedVectorCamera.z);
    00159   cameraSpeedVectorImg /= cameraSpeedVectorCamera.x + // perspective projection,
    00160     image.cameraInfo.focalLength; // focal length is added to translate the vector away from the lens in case the distance
    00161                                     // is too small and hence the visualization would be greatly magnified
    00162   cameraSpeedVectorImg *= image.cameraInfo.focalLength*visualizationScale;
    00163   Drawings::Color arrowColor = Drawings::blue;
    00164   if (fabs(panningVelocity) > pi/6)
    00165     arrowColor = Drawings::red;
    00166   NARROW("SlamIMP:horizon",
    00167     image.cameraInfo.opticalCenter.x, image.cameraInfo.opticalCenter.y,
    00168     int(image.cameraInfo.opticalCenter.x-cameraSpeedVectorImg.x), int(image.cameraInfo.opticalCenter.y-cameraSpeedVectorImg.y),
    00169     3, Drawings::ps_solid, arrowColor);
    00170 //-----------------------------------------------------
    00171   angleBetweenDirectionOfViewAndGround =
    00172     toDegrees(
    00173     atan2(
    00174     vectorInDirectionOfViewWorld.z,
    00175     sqrt(sqr(vectorInDirectionOfViewWorld.x) + sqr(vectorInDirectionOfViewWorld.y))
    00176     )
    00177     );
    00178
    00179   // Compute additional information about the image
    00180   imageInfo.maxImageCoordinates.x = image.cameraInfo.resolutionWidth - 1;
    00181   imageInfo.maxImageCoordinates.y = image.cameraInfo.resolutionHeight - 1;
    00182   imageInfo.horizon = Geometry::calculateHorizon(cameraMatrix, image.cameraInfo);
    00183   imageInfo.horizonInImage = Geometry::getIntersectionPointsOfLineAndRectangle(
    00184   Vector2<int>(0,0), imageInfo.maxImageCoordinates, imageInfo.horizon,
    00185           imageInfo.horizonStart, imageInfo.horizonEnd);
    00186   imageInfo.vertLine = imageInfo.horizon;
    00187   imageInfo.vertLine.direction.x = -imageInfo.vertLine.direction.y;
    00188   imageInfo.vertLine.direction.y = imageInfo.horizon.direction.x;
    00189   imageInfo.vertLine.normalizeDirection();
    00190
    00191   // Scan through image
    00192   scanColumns();
    00193   scanRows();
    00194
    00195   averageY /= averageCount;
    00196   averageU /= averageCount;
    00197   averageV /= averageCount;
    00198
    00199   slamPercept.imageCharacteristic.averageY = averageY;
    00200   slamPercept.imageCharacteristic.averageU = averageU;
    00201   slamPercept.imageCharacteristic.averageV = averageV;
    00202
    00203   slamPercept.imageCharacteristic.filteredAverageY *= filterWeight;
    00204   slamPercept.imageCharacteristic.filteredAverageY += (1.0 - filterWeight) * averageY;
    00205   slamPercept.imageCharacteristic.filteredAverageU *= filterWeight;
    00206   slamPercept.imageCharacteristic.filteredAverageU += (1.0 - filterWeight) * averageU;
    00207   slamPercept.imageCharacteristic.filteredAverageV *= filterWeight;
    00208   slamPercept.imageCharacteristic.filteredAverageV += (1.0 - filterWeight) * averageV;
    00209
    00210   MODIFY ("slamIMP:filteredY", slamPercept.imageCharacteristic.filteredAverageY);
    00211   MODIFY ("slamIMP:filteredU", slamPercept.imageCharacteristic.filteredAverageU);
    00212   MODIFY ("slamIMP:filteredV", slamPercept.imageCharacteristic.filteredAverageV);
    00213
    00214   MODIFY ("slamIMP:filterWeight", filterWeight);
    00215
    00216   if (slamPercept.imageCharacteristic.averageY >= 70)
    00217     slamPercept.viewAngle = 0;
    00218   else
    00219     slamPercept.viewAngle = 180;
    00220
    00221   slamPercept.viewAngleValid = true;
    00222
    00223   if (slamPercept.greenLocation.impAngleToMaxGreenValid)
    00224   {
    00225     NCIRCLE ("SlamIMP:maxGreenImage", slamPercept.greenLocation.maxGreenPointInImage.x, slamPercept.greenLocation.maxGreenPointInImage.y, 10, 3, Drawings::ps_solid, Drawings::green);
    00226
    00227     NCIRCLE ("SlamIMP:maxGreenField", slamPercept.greenLocation.maxGreenPointOnField.x, slamPercept.greenLocation.maxGreenPointOnField.y, 10, 3, Drawings::ps_solid, Drawings::blue);
    00228   };
    00229
    00230 //  circleFinder.determineCirclePoint();
    00231   Vector2<double> circleMidPoint;
    00232   lineFinder.execute(linesPercept, cameraMatrix, image, imageInfo, robotPose, false /*circleFinder.getCircle(circleMidPoint)*/, Vector2<double>(0,0)/*circleMidPoint*/);
    00233   borderFinder.execute(linesPercept, cameraMatrix, image, imageInfo, robotPose);
    00234
    00235
    00236
    00237   DEBUG_DRAWING_FINISHED(circlePoints_image);
    00238   DEBUG_DRAWING_FINISHED(circlePoints_Field);
    00239
    00240
    00241 #ifdef EDGES
    00242   // get line-point percepts from specialist
    00243   //edgeSpecialist.getEdgePointPercepts(linesPercept, cameraMatrix, imageInfo.previousCameraMatrix, image);
    00244   // get edge percepts from specialist
    00245   edgeSpecialist.getEdgesPercept(edgesPercept, cameraMatrix, imageInfo.previousCameraMatrix, image);
    00246 #endif
    00247
    00248   // clears the multiple ball percept list
    00249   ballSpecialist.resetMultiplePerceptsList(ballPercept);
    00250
    00251
    00252   // Analyze results
    00253 #ifdef MULTIPLE_BALL_CANDIDATES
    00254   if (longestBallRun > 2)
    00255   {
    00256     int numberOfCandidates = ballClustering.getSize();
    00257     for (int i=0; i<numberOfCandidates; i++)
    00258     {
    00259       Vector2<double> temp = ballClustering.getMidpoint(i);
    00260       ballCandidate.x = (int)temp.x;
    00261         ballCandidate.y = (int)temp.y;
    00262         DOT(imageProcessor_ball3, ballCandidate.x, ballCandidate.y, Drawings::black, Drawings::orange);
    00263       ballSpecialist.searchBall(image, colorTable, cameraMatrix, imageInfo.previousCameraMatrix,
    00264                                         ballCandidate.x, ballCandidate.y, ballPercept);
    00265
    00266       //CRASH_CHECK(i);
    00267     }
    00268     DEBUG_DRAWING_FINISHED(imageProcessor_multipleBallPercepts);
    00269     DEBUG_DRAWING_FINISHED(multipleBallPerceptsField);
    00270   }
    00271 #else
    00272   if (longestBallRun > 2)
    00273   {
    00274     ballSpecialist.searchBall(image, colorTable, cameraMatrix, imageInfo.previousCameraMatrix,
    00275                                     ballCandidate.x, ballCandidate.y, ballPercept);
    00276   }
    00277 #endif
    00278
    00279   ballSpecialist.forwardPercept(ballPercept);
    00280
    00281   // is not needed any more, because fieldLine-percepts now come from recognized lines
    00282   // -- reverted --
    00283   filterPercepts();
    00284
    00285   landmarksPercept.cameraOffset = cameraMatrix.translation;
    00286   if(imageInfo.horizonInImage)
    00287   {
    00288     beaconDetector.execute();
    00289   }
    00290
    00291   slamPercept.imageCharacteristic.numPink = 0;
    00292
    00293   if (slamPercept.slamState == SlamPercept::LearnNewFlags)
    00294   goalRecognizer.execute();
    00295
    00296   // Drawing stuff
    00297   if(imageInfo.horizonInImage)
    00298   {
    00299     NLINE("SlamIMP:horizon",
    00300      imageInfo.horizonStart.x, imageInfo.horizonStart.y, imageInfo.horizonEnd.x,
    00301      imageInfo.horizonEnd.y, 1, Drawings::ps_solid, Drawings::white );
    00302   }
    00303   DEBUG_DRAWING_FINISHED(imageProcessor_horizon);
    00304   DEBUG_DRAWING_FINISHED(imageProcessor_scanLines);
    00305   DEBUG_DRAWING_FINISHED(imageProcessor_general);
    00306   DEBUG_DRAWING_FINISHED(imageProcessor_edges);
    00307
    00308   DEBUG_DRAWING_FINISHED(ballLocatorField);
    00309   DEBUG_DRAWING_FINISHED(percepts_robot);
    00310
    00311   SEND_DEBUG_IMAGE(imageProcessorPlayers);
    00312   GENERATE_DEBUG_IMAGE(imageProcessorGeneral,
    00313     Image correctedImage(image);
    00314     colorCorrector.correct(correctedImage);
    00315     INIT_DEBUG_IMAGE(imageProcessorGeneral, correctedImage);
    00316   )
    00317   SEND_DEBUG_IMAGE(imageProcessorGeneral);
    00318
    00319   GENERATE_DEBUG_IMAGE(segmentedImage1,
    00320     Image correctedImage(image);
    00321     colorCorrector.correct(correctedImage);
    00322     colorTable.generateColorClassImage(correctedImage, segmentedImage1ColorClassImage);
    00323   )
    00324   SEND_DEBUG_COLOR_CLASS_IMAGE(segmentedImage1);
    00325
    00326   GENERATE_DEBUG_IMAGE(imageProcessorGradients,
    00327     SUSANEdgeDetectionLite edgeDetectionU(SlamBeaconDetector::edgeThresholdU);
    00328     SUSANEdgeDetectionLite edgeDetectionV(SlamBeaconDetector::edgeThresholdV);
    00329     Image edgeDetectionImage;
    00330     bool edgeY;
    00331     bool edgeU;
    00332     bool edgeV;
    00333     edgeDetectionImage.cameraInfo = image.cameraInfo;
    00334     for (int y=1; y<edgeDetectionImage.cameraInfo.resolutionHeight-1; y++)
    00335     {
    00336       for (int x=1; x<edgeDetectionImage.cameraInfo.resolutionWidth-1; x++)
    00337       {
    00338         edgeY = edgeDetectionU.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentA);
    00339         edgeU = edgeDetectionU.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentB);
    00340         edgeV = edgeDetectionV.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentC);
    00341         if (edgeU&&edgeV)
    00342         {
    00343           edgeDetectionImage.image[y][0][x] = 210;
    00344           edgeDetectionImage.image[y][1][x] = 127;
    00345           edgeDetectionImage.image[y][2][x] = 127;
    00346         }
    00347         else if (edgeU)
    00348         {
    00349           edgeDetectionImage.image[y][0][x] = 130;
    00350           edgeDetectionImage.image[y][1][x] = 170;
    00351           edgeDetectionImage.image[y][2][x] = 80;
    00352         }
    00353         else if (edgeV)
    00354         {
    00355           edgeDetectionImage.image[y][0][x] = 130;
    00356           edgeDetectionImage.image[y][1][x] = 80;
    00357           edgeDetectionImage.image[y][2][x] = 170;
    00358         }
    00359         else if (edgeY)
    00360         {
    00361           edgeDetectionImage.image[y][0][x] = 255;
    00362           edgeDetectionImage.image[y][1][x] = 127;
    00363           edgeDetectionImage.image[y][2][x] = 127;
    00364         }
    00365         else
    00366         {
    00367           edgeDetectionImage.image[y][0][x] = image.image[y][0][x]/2;
    00368           edgeDetectionImage.image[y][1][x] = image.image[y][1][x];
    00369           edgeDetectionImage.image[y][2][x] = image.image[y][2][x];
    00370         }
    00371         edgeDetectionImage.image[y][3][x] = 128;
    00372         edgeDetectionImage.image[y][4][x] = 128;
    00373         edgeDetectionImage.image[y][5][x] = 128;
    00374       }
    00375     }
    00376     INIT_DEBUG_IMAGE(imageProcessorGradients, edgeDetectionImage);
    00377   )
    00378   SEND_DEBUG_IMAGE(imageProcessorGradients);
    00379
    00380   GENERATE_DEBUG_IMAGE(imageProcessorBall,
    00381     Image correctedImage(image);
    00382     colorCorrector.correct(correctedImage);
    00383     for (int iPBix=0; iPBix<correctedImage.cameraInfo.resolutionWidth; iPBix++)
    00384     {
    00385       for (int iPBiy=0; iPBiy<correctedImage.cameraInfo.resolutionHeight; iPBiy++)
    00386       {
    00387         correctedImage.image[iPBiy][0][iPBix]=ballSpecialist.getSimilarityToOrange(correctedImage.image[iPBiy][0][iPBix],correctedImage.image[iPBiy][1][iPBix],correctedImage.image[iPBiy][2][iPBix]);
    00388         if (correctedImage.image[iPBiy][0][iPBix]==0)
    00389         {
    00390           correctedImage.image[iPBiy][1][iPBix]=127;
    00391           correctedImage.image[iPBiy][2][iPBix]=127;
    00392         }
    00393         else
    00394         {
    00395           if (correctedImage.image[iPBiy][0][iPBix]>30)
    00396             correctedImage.image[iPBiy][1][iPBix]=0;
    00397           else
    00398             correctedImage.image[iPBiy][1][iPBix]=255;
    00399           if (correctedImage.image[iPBiy][0][iPBix]>60)
    00400             correctedImage.image[iPBiy][2][iPBix]=0;
    00401           else
    00402             correctedImage.image[iPBiy][2][iPBix]=255;
    00403         }
    00404       }
    00405     }
    00406     INIT_DEBUG_IMAGE(imageProcessorBall, correctedImage);
    00407   )
    00408   SEND_DEBUG_IMAGE(imageProcessorBall);
    00409   DEBUG_DRAWING_FINISHED(imageProcessor_ball4);
    00410
    00411   //for(i = 0; i < LinesPercept::numberOfLineTypes; ++i)
    00412   //    numberOfPoints[i] = linesPercept.numberOfPoints[i];
    00413   // output number of found percepts
    00414   /*OUTPUT(idText,text,"SlamImageProcessor: percept-counting " << endl
    00415     << "\tflags: " << landmarksPercept.numberOfFlags << endl
    00416     << "\tgoals: " << landmarksPercept.numberOfGoals << endl
    00417     << "\tlines: " << linesPercept.numberOfPoints[1] << endl
    00418     << "\tballs: " << ballPercept.ballWasSeen << endl
    00419     << "\tblue:  " << playersPercept.numberOfBluePlayers << endl
    00420     << "\tred:   " << playersPercept.numberOfRedPlayers << endl
    00421     << "\tobst:  " << obstaclesPercept.numberOfPoints
    00422   );*/
    00423
    00424   imageInfo.previousCameraMatrix = cameraMatrix;
    00425   prevCmTricot = cmTricot;
    00426   /*OUTPUT(idText, text, "---");
    00427   for(int ii = 0; ii < linesPercept.numberOfPoints[1]; ++ii)
    00428     OUTPUT(idText, text, int(linesPercept.points[1][ii].angle * 180 / pi));*/
    00429
    00430 //////// New Debug Images
    00431   N_GENERATE_DEBUG_IMAGE(colorCorrectedAsJpeg,
    00432     colorCorrectedAsJpegImage = image;
    00433     colorCorrector.correct(colorCorrectedAsJpegImage);
    00434   );
    00435   N_SEND_DEBUG_IMAGE_AS_JPEG(colorCorrectedAsJpeg)
    00436
    00437   N_GENERATE_DEBUG_IMAGE(colorCorrected,
    00438     colorCorrectedImage = image;
    00439     colorCorrector.correct(colorCorrectedImage);
    00440   );
    00441   N_SEND_DEBUG_IMAGE(colorCorrected);
    00442
    00443   N_GENERATE_DEBUG_IMAGE(segmented,
    00444     Image colorCorrectedImage(image);
    00445     colorCorrector.correct(colorCorrectedImage);
    00446     colorTable.generateColorClassImage(colorCorrectedImage, segmentedImage);
    00447   );
    00448   N_SEND_DEBUG_COLOR_CLASS_IMAGE(segmented)
    00449
    00450   N_GENERATE_DEBUG_IMAGE(edgeDetection,
    00451     SUSANEdgeDetectionLite edgeDetectionU(SlamBeaconDetector::edgeThresholdU);
    00452     SUSANEdgeDetectionLite edgeDetectionV(SlamBeaconDetector::edgeThresholdV);
    00453     bool edgeY;
    00454     bool edgeU;
    00455     bool edgeV;
    00456     edgeDetectionImage.cameraInfo = image.cameraInfo;
    00457     for (int y=1; y<edgeDetectionImage.cameraInfo.resolutionHeight-1; y++)
    00458     {
    00459       for (int x=1; x<edgeDetectionImage.cameraInfo.resolutionWidth-1; x++)
    00460       {
    00461         edgeY = edgeDetectionU.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentA);
    00462         edgeU = edgeDetectionU.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentB);
    00463         edgeV = edgeDetectionV.isEdgePoint(image, x, y, SUSANEdgeDetectionLite::componentC);
    00464         if (edgeU&&edgeV)
    00465         {
    00466           edgeDetectionImage.image[y][0][x] = 210;
    00467           edgeDetectionImage.image[y][1][x] = 127;
    00468           edgeDetectionImage.image[y][2][x] = 127;
    00469         }
    00470         else if (edgeU)
    00471         {
    00472           edgeDetectionImage.image[y][0][x] = 130;
    00473           edgeDetectionImage.image[y][1][x] = 170;
    00474           edgeDetectionImage.image[y][2][x] = 80;
    00475         }
    00476         else if (edgeV)
    00477         {
    00478           edgeDetectionImage.image[y][0][x] = 130;
    00479           edgeDetectionImage.image[y][1][x] = 80;
    00480           edgeDetectionImage.image[y][2][x] = 170;
    00481         }
    00482         else if (edgeY)
    00483         {
    00484           edgeDetectionImage.image[y][0][x] = 255;
    00485           edgeDetectionImage.image[y][1][x] = 127;
    00486           edgeDetectionImage.image[y][2][x] = 127;
    00487         }
    00488         else
    00489         {
    00490           edgeDetectionImage.image[y][0][x] = image.image[y][0][x]/2;
    00491           edgeDetectionImage.image[y][1][x] = image.image[y][1][x];
    00492           edgeDetectionImage.image[y][2][x] = image.image[y][2][x];
    00493         }
    00494         edgeDetectionImage.image[y][3][x] = 128;
    00495         edgeDetectionImage.image[y][4][x] = 128;
    00496         edgeDetectionImage.image[y][5][x] = 128;
    00497       }
    00498     }
    00499   );
    00500   N_SEND_DEBUG_IMAGE_AS_JPEG(edgeDetection);
    00501
    00502   N_GENERATE_DEBUG_IMAGE(ball,
    00503     ballImage = image;
    00504     colorCorrector.correct(ballImage);
    00505     for (int iPBix=0; iPBix<ballImage.cameraInfo.resolutionWidth; iPBix++)
    00506     {
    00507       for (int iPBiy=0; iPBiy<ballImage.cameraInfo.resolutionHeight; iPBiy++)
    00508       {
    00509         ballImage.image[iPBiy][0][iPBix]=ballSpecialist.getSimilarityToOrange(ballImage.image[iPBiy][0][iPBix],ballImage.image[iPBiy][1][iPBix],ballImage.image[iPBiy][2][iPBix]);
    00510         if (ballImage.image[iPBiy][0][iPBix]==0)
    00511         {
    00512           ballImage.image[iPBiy][1][iPBix]=127;
    00513           ballImage.image[iPBiy][2][iPBix]=127;
    00514         }
    00515         else
    00516         {
    00517           if (ballImage.image[iPBiy][0][iPBix]>30)
    00518             ballImage.image[iPBiy][1][iPBix]=0;
    00519           else
    00520             ballImage.image[iPBiy][1][iPBix]=255;
    00521           if (ballImage.image[iPBiy][0][iPBix]>60)
    00522             ballImage.image[iPBiy][2][iPBix]=0;
    00523           else
    00524             ballImage.image[iPBiy][2][iPBix]=255;
    00525         }
    00526       }
    00527     }
    00528   );
    00529   N_SEND_DEBUG_IMAGE_AS_JPEG(ball);
    00530   N_SEND_DEBUG_GRAY_SCALE_IMAGE(scanLines);
    00531 }
    00532
    00533 void SlamImageProcessor::scanColumns()
    00534 {
    00535   const int scanLineSpacing(4);
    00536   const double verticalScanLineOffset(15.0);
    00537                //verticalScanLineLength(50.0);
    00538   Geometry::Line scanLine;
    00539   Vector2<double> intersection;
    00540   Vector2<int> topPoint,
    00541                bottomPoint,
    00542                topBorderPoint,
    00543                bottomBorderPoint;
    00544   double bottomBorderDist, topBorderDist;
    00545   int i;
    00546   bool noLines = false,
    00547        skipLine;
    00548
    00549   // Reset horizontal counters
    00550   noRedCount = noBlueCount = 100;
    00551   closestBottom = closestBottom = 40000;
    00552   goalAtBorder = goalAtBorder = false;
    00553
    00554
    00555   // scan lines are perpendicular to horizon
    00556   scanLine.direction = imageInfo.vertLine.direction;
    00557
    00558   // calculate and scan lines
    00559   for(int dir = -1; dir <= 1; dir += 2)
    00560   {
    00561     for(i = 0; true; i++)
    00562     {
    00563       if (dir == 1 && i == 0) i = 1;
    00564       scanLine.base.x = image.cameraInfo.resolutionWidth / 2 + imageInfo.horizon.direction.x * scanLineSpacing * dir * i;
    00565       scanLine.base.y = image.cameraInfo.resolutionHeight / 2 + imageInfo.horizon.direction.y * scanLineSpacing * dir * i;
    00566
    00567       //Does the scan line intersect with the image?
    00568       if(!Geometry::getIntersectionPointsOfLineAndRectangle(
    00569                                           Vector2<int>(0,0),
    00570                                           imageInfo.maxImageCoordinates,
    00571                                           scanLine, topBorderPoint, bottomBorderPoint))
    00572       {
    00573         break;
    00574       }
    00575       Geometry::getIntersectionOfLines(imageInfo.horizon, scanLine, intersection);
    00576
    00577       if (fabs(scanLine.direction.y) > 0.001)
    00578       {
    00579         topBorderDist = (topBorderPoint.y - intersection.y) / scanLine.direction.y;
    00580         bottomBorderDist = (bottomBorderPoint.y - intersection.y) / scanLine.direction.y;
    00581       } else {
    00582         topBorderDist = (topBorderPoint.x - intersection.x) / scanLine.direction.x;
    00583         bottomBorderDist = (bottomBorderPoint.x - intersection.x) / scanLine.direction.x;
    00584       }
    00585
    00586       skipLine = false;
    00587
    00588       // upper boundary for scan lines
    00589       if (topBorderDist > -10)
    00590         topPoint = topBorderPoint;
    00591       else if (bottomBorderDist < -10)
    00592         skipLine = true;
    00593       else
    00594       {
    00595         topPoint.x = (int)(intersection.x - scanLine.direction.x * verticalScanLineOffset);
    00596         topPoint.y = (int)(intersection.y - scanLine.direction.y * verticalScanLineOffset);
    00597         if (topPoint.x < 0) topPoint.x = 0;
    00598         if (topPoint.x >= image.cameraInfo.resolutionWidth) topPoint.x = image.cameraInfo.resolutionWidth - 1;
    00599         if (topPoint.y < 0) topPoint.y = 0;
    00600         if (topPoint.y >= image.cameraInfo.resolutionHeight) topPoint.y = image.cameraInfo.resolutionHeight - 1;
    00601       }
    00602
    00603       // lower boundaries for scan lines
    00604       switch (i & 3)
    00605       {
    00606         case 1:
    00607         case 3:
    00608           if (bottomBorderDist < 30)
    00609             bottomPoint = bottomBorderPoint;
    00610           else if (topBorderDist > 30)
    00611             skipLine = true;
    00612           else
    00613           {
    00614             bottomPoint.x = (int)(intersection.x + scanLine.direction.x * 40);
    00615             bottomPoint.y = (int)(intersection.y + scanLine.direction.y * 40);
    00616             if (bottomPoint.x < 0) bottomPoint.x = 0;
    00617             if (bottomPoint.x >= image.cameraInfo.resolutionWidth) bottomPoint.x = image.cameraInfo.resolutionWidth - 1;
    00618             if (bottomPoint.y < 0) bottomPoint.y = 0;
    00619             if (bottomPoint.y >= image.cameraInfo.resolutionHeight) bottomPoint.y = image.cameraInfo.resolutionHeight - 1;
    00620           }
    00621           noLines = true;
    00622           break;
    00623         case 2:
    00624           if (bottomBorderDist < 60)
    00625             bottomPoint = bottomBorderPoint;
    00626           else if (topBorderDist > 60)
    00627             skipLine = true;
    00628           else
    00629           {
    00630             bottomPoint.x = (int)(intersection.x + scanLine.direction.x * 60);
    00631             bottomPoint.y = (int)(intersection.y + scanLine.direction.y * 60);
    00632             if (bottomPoint.x < 0) bottomPoint.x = 0;
    00633             if (bottomPoint.x >= image.cameraInfo.resolutionWidth) bottomPoint.x = image.cameraInfo.resolutionWidth - 1;
    00634             if (bottomPoint.y < 0) bottomPoint.y = 0;
    00635             if (bottomPoint.y >= image.cameraInfo.resolutionHeight) bottomPoint.y = image.cameraInfo.resolutionHeight - 1;
    00636           }
    00637           noLines = true;
    00638           break;
    00639         case 0:
    00640         default:
    00641           bottomPoint = bottomBorderPoint;
    00642           noLines = false;
    00643           break;
    00644       }
    00645       if (!skipLine)
    00646         scan(topPoint, bottomPoint, true, noLines);
    00647     }
    00648   }
    00649   // finish clustering
    00650   for(i = 0; i < 3; ++i)
    00651   {
    00652     clusterRobots(0,false,false);
    00653   }
    00654 }
    00655
    00656 void SlamImageProcessor::scanRows()
    00657 {
    00658   Geometry::Line scanLine(imageInfo.horizon);
    00659   double horizontalScanLineSpacing(20.0);
    00660   Vector2<double> scanOffset(imageInfo.vertLine.direction * horizontalScanLineSpacing);
    00661   scanLine.base += (scanOffset*2.0);
    00662   Vector2<int> startPoint,
    00663                  endPoint;
    00664   //Test if the horizon is inside the image
    00665   if(!Geometry::getIntersectionPointsOfLineAndRectangle(Vector2<int>(0,0),
    00666                                                         imageInfo.maxImageCoordinates,
    00667                                                         scanLine, startPoint, endPoint))
    00668   {
    00669     scanLine.base.x = (double)imageInfo.maxImageCoordinates.x / 2.0;
    00670     scanLine.base.y = 1;
    00671   }
    00672   while(true)
    00673   {
    00674     if(Geometry::getIntersectionPointsOfLineAndRectangle(
    00675                                         Vector2<int>(0,0),
    00676                                         imageInfo.maxImageCoordinates,
    00677                                         scanLine, startPoint, endPoint))
    00678     {
    00679       LINE(imageProcessor_general,startPoint.x,startPoint.y,endPoint.x,endPoint.y,
    00680        1,Drawings::ps_solid, Drawings::green);
    00681       if(rand() > RAND_MAX / 2)
    00682         scan(startPoint, endPoint, false, false);
    00683       else
    00684         scan(startPoint, endPoint, false, false);
    00685       scanLine.base += scanOffset;
    00686     }
    00687     else
    00688     {
    00689       return;
    00690     }
    00691   }
    00692 }
    00693
    00694 void SlamImageProcessor::scan(const Vector2<int>& start, const Vector2<int>& end,
    00695                                 bool vertical, bool noLines)
    00696 {
    00697 #ifdef EDGES
    00698   edgeSpecialist.resetLine();
    00699 #endif
    00700
    00701   LinesPercept::LineType typeOfLinesPercept = LinesPercept::field;
    00702   int pixelsSinceGoal = 0;
    00703   int pixelsBetweenGoalAndObstacle = 0;
    00704
    00705   Vector2<int> actual = start,
    00706                diff = end - start,
    00707                step(sgn(diff.x),sgn(diff.y)),
    00708                size(abs(diff.x),abs(diff.y));
    00709   bool isVertical = abs(diff.y) > abs(diff.x);
    00710   int count,
    00711       sum;
    00712   double scanAngle = diff.angle();
    00713
    00714   LINE(imageProcessor_scanLines,
    00715     start.x, start.y, end.x, end.y,
    00716     1, Drawings::ps_solid, Drawings::gray );
    00717
    00718   // init Bresenham
    00719   if(isVertical)
    00720   {
    00721     count = size.y;
    00722     sum = size.y / 2;
    00723   }
    00724   else
    00725   {
    00726     count = size.x;
    00727     sum = size.x / 2;
    00728   }
    00729
    00730   if(count > 7)
    00731   {
    00732     int numberOfPoints[LinesPercept::numberOfLineTypes],
    00733         i;
    00734     for(i = 0; i < LinesPercept::numberOfLineTypes; ++i)
    00735       numberOfPoints[i] = linesPercept.points[i].numberOfPoints;
    00736     RingBuffer<const unsigned char*,7> pixelBuffer;
    00737     RingBuffer<const unsigned char*,7> pixelBufferFromUp;
    00738     RingBuffer<unsigned char, 7> yBuffer;
    00739     RingBuffer<unsigned char, 7> vBuffer;
    00740     RingBuffer<colorClass, 7> colorClassBuffer;
    00741
    00742     /*
    00743     * Slam
    00744     */
    00745     bool startOfGreenFound = false;
    00746     Vector2<int> startOfGreen;
    00747
    00748     //the image is scanned vertically
    00749     for(i = 0; i < 6; ++i) // fill buffer
    00750     {
    00751       unsigned char y,u,v;
    00752       pixelBuffer.add(&image.image[actual.y][0][actual.x]);
    00753       if (actual.x < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y < 0 || actual.y >= image.cameraInfo.resolutionHeight)
    00754         y = u = v = 0;
    00755       else
    00756       {
    00757         y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    00758         u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    00759         v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    00760       }
    00761       yBuffer.add(y);
    00762       vBuffer.add(v);
    00763       colorClass color = COLOR_CLASS(y, u, v);
    00764       colorClassBuffer.add(color);
    00765
    00766     if
    00767       (
    00768         (actual.x > imageInfo.maxImageCoordinates.x / 2 - 10)
    00769         && (actual.x < imageInfo.maxImageCoordinates.x / 2 + 10)
    00770       )
    00771     {
    00772     averageY += y;
    00773     averageU += u;
    00774     averageV += v;
    00775
    00776     averageCount++;
    00777     };
    00778 #ifdef EDGES
    00779       edgeSpecialist.checkPoint(actual, color, cameraMatrix, imageInfo.previousCameraMatrix, image);
    00780 #endif
    00781
    00782       // Bresenham
    00783       if(isVertical)
    00784       {
    00785         actual.y += step.y;
    00786         sum += size.x;
    00787         if(sum >= size.y)
    00788         {
    00789           sum -= size.y;
    00790           actual.x += step.x;
    00791         }
    00792       }
    00793       else
    00794       {
    00795         actual.x += step.x;
    00796         sum += size.y;
    00797         if(sum >= size.x)
    00798         {
    00799           sum -= size.x;
    00800           actual.y += step.y;
    00801         }
    00802       }
    00803     }
    00804     lineColor = Drawings::numberOfColors;
    00805
    00806     int redCount = 0,
    00807         blueCount = 0,
    00808         greenCount = 0,
    00809         noGreenCount = 100,
    00810         pinkCount = 0,
    00811         noPinkCount = 100,
    00812         yellowCount = 0,
    00813         noYellowCount = 100,
    00814         skyblueCount = 0,
    00815         noSkyblueCount = 100,
    00816         orangeCount = 0,
    00817         noOrangeCount = 100;
    00818     const unsigned char* firstRed = 0,
    00819                        * lastRed = 0,
    00820                        * firstBlue = 0,
    00821                        * lastBlue = 0,
    00822                        * firstGreen = 0,
    00823                        * firstOrange = 0,
    00824                        * lastOrange = 0,
    00825                        * firstPink = 0,
    00826                        * lastPink = 0,
    00827                        * pFirst = pixelBuffer[2],
    00828                        * up = pFirst;
    00829     enum{justUP, greenAndUP, down} state = justUP;
    00830     bool borderFound = false; // there can be only one border point per scan line
    00831
    00832     // obstacles
    00833     enum{noGroundFound, foundBeginOfGround, foundEndOfGround} groundState = noGroundFound;
    00834     Vector2<int> firstGround(0,0);
    00835     int numberOfPixelsSinceFirstOccurrenceOfGroundColor = 0;
    00836     int numberOfPixelsSinceFirstOccurrenceOfNonGroundColor = 0;
    00837     int numberOfPixelsSinceLastGround = 0;
    00838     int numberOfWhitePixelsSinceLastGround = 0;
    00839     int numberOfGroundPixels = 0;
    00840     int numberOfWhiteObstaclePixels = 0;
    00841     int numberOfNotWhiteObstaclePixels = 0;
    00842     bool beginOfGroundIsAtTheTopOfTheImage = false;
    00843
    00844     struct BorderCandidate
    00845     {
    00846       bool found;
    00847       Vector2<int> pointOnField;
    00848       Vector2<int> pointInImage;
    00849       double angleOnField;
    00850       double angleInImage;
    00851     };
    00852     BorderCandidate borderCandidate;
    00853     borderCandidate.found = false;
    00854
    00855     for(; i <= count; ++i)
    00856     {
    00857       pixelsSinceGoal++;
    00858       numberOfScannedPixels++;
    00859       unsigned char y,u,v;
    00860       pixelBuffer.add(&image.image[actual.y][0][actual.x]);
    00861       if (actual.x < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y < 0 || actual.y >= image.cameraInfo.resolutionHeight)
    00862         y = u = v = 0;
    00863       else
    00864       {
    00865         y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    00866         u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    00867         v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    00868       }
    00869       yBuffer.add(y);
    00870       vBuffer.add(v);
    00871       colorClass color = COLOR_CLASS(y, u, v);
    00872       colorClassBuffer.add(color);
    00873
    00874 #ifdef EDGES
    00875       edgeSpecialist.checkPoint(actual, color, cameraMatrix, imageInfo.previousCameraMatrix, image);
    00876 #endif
    00877
    00878       // line point state machine
    00879       const unsigned char* p3 = pixelBuffer[3];
    00880       //UP: y-component increases ////////////////////////////////////////
    00881       if(yBuffer[3] > yBuffer[4] + yThreshold)
    00882       {
    00883         up = p3;
    00884         // is there green in one of the pixels above?
    00885         if(   colorClassBuffer[6] == green
    00886           ||  colorClassBuffer[5] == green
    00887           ||  colorClassBuffer[4] == green)
    00888         {
    00889           Vector2<int> coords = getCoords(p3),
    00890                        pointOnField; //position on field, relative to robot
    00891           pixelBufferFromUp.init();
    00892           if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
    00893             for(int k = pixelBuffer.getNumberOfEntries() - 1; k >= 0; --k)
    00894               pixelBufferFromUp.add(pixelBuffer.getEntry(k));
    00895           state = greenAndUP;
    00896         }
    00897         else
    00898           state = justUP;
    00899       }
    00900
    00901       //DOWN: y-component decreases //////////////////////////////////////
    00902       else if(yBuffer[3] <
    00903               yBuffer[4] - yThreshold ||
    00904               vBuffer[3] <
    00905               vBuffer[4] - vThreshold)
    00906       {
    00907         // is the pixel in the field ??
    00908         if(state != down && // was an "up" pixel above?
    00909            (colorClassBuffer[0] == green
    00910            || colorClassBuffer[1] == green
    00911            || colorClassBuffer[2] == green)) // is there green in one of the pixels below ?
    00912         {
    00913           colorClass c = colorClassBuffer[6];
    00914           if(!noLines || c == skyblue || c == yellow)
    00915           {
    00916             Vector2<int> coords = getCoords(p3),
    00917                          pointOnField; //position on field, relative to robot
    00918
    00919             if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
    00920             {
    00921               Vector2<int> upCoords = getCoords(up);
    00922               int height = (coords - upCoords).abs();
    00923               int distance = (int) sqrt(sqr(cameraMatrix.translation.z) + sqr(pointOnField.abs())),
    00924                 lineHeight = (int) Geometry::getSizeByDistance(image.cameraInfo, 25, distance);
    00925
    00926               //The bright region is assumed to be on the ground. If it is small enough, it is a field line.
    00927               if(height < 3 * lineHeight && state == greenAndUP &&
    00928                  (colorClassBuffer[5] == white ||
    00929                   colorClassBuffer[4] == white))
    00930               {
    00931                 double edgeAngleImage, edgeAngleField;
    00932                 Vector2<int> pointOnField2;
    00933                 if(pixelBufferFromUp.getNumberOfEntries() > 0 &&
    00934                    calcEdgeAngle(edgeAngleImage, edgeAngleField, coords, pointOnField, scanAngle, pixelBuffer) &&
    00935                    Geometry::calculatePointOnField(upCoords.x, upCoords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField2))
    00936                 {
    00937                   double upAngleImage, upAngleField;
    00938                   if(calcEdgeAngle(upAngleImage, upAngleField, upCoords, pointOnField2, scanAngle, pixelBufferFromUp, true))
    00939                   {
    00940                     // only add the closest field line. This makes filtering easier.
    00941                     linesPercept.points[LinesPercept::field].numberOfPoints = numberOfPoints[LinesPercept::field];
    00942                     linesPercept.add(LinesPercept::field, pointOnField2, upCoords, upAngleField, upAngleImage);
    00943                     linesPercept.add(LinesPercept::field, pointOnField, coords, edgeAngleField, edgeAngleImage);
    00944                   }
    00945                 }
    00946               }
    00947               else if(height >= 3 && !borderFound)
    00948               {
    00949                 switch(c)
    00950                 {
    00951                   case skyblue:
    00952                     if(vertical)
    00953                     {
    00954                       borderFound = true;
    00955                       if(getPlayer().getTeamColor() == Player::red && !linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints)
    00956                         goalAtBorder = pointOnField.abs() < closestBottom;
    00957                       closestBottom = 40000;
    00958                       double edgeAngleField, edgeAngleImage;
    00959                       if(calcEdgeAngle(edgeAngleImage, edgeAngleField, coords, pointOnField, scanAngle, pixelBuffer, false, false, 2))
    00960                         linesPercept.add(LinesPercept::skyblueGoal, pointOnField, coords, edgeAngleField, edgeAngleImage);
    00961                       typeOfLinesPercept = LinesPercept::skyblueGoal;
    00962                       pixelsSinceGoal = 0;
    00963
    00964                       lastRed = lastBlue = 0;
    00965                       redCount = blueCount = 0;
    00966                     }
    00967                     break;
    00968                   case yellow:
    00969                     if(vertical)
    00970                     {
    00971                       borderFound = true;
    00972                       if(getPlayer().getTeamColor() == Player::blue && !linesPercept.points[LinesPercept::yellowGoal].numberOfPoints)
    00973                         goalAtBorder = pointOnField.abs() < closestBottom;
    00974                       closestBottom = 40000;
    00975                       double edgeAngleField, edgeAngleImage;
    00976                       if(calcEdgeAngle(edgeAngleImage, edgeAngleField, coords, pointOnField, scanAngle, pixelBuffer, false, false))
    00977                         linesPercept.add(LinesPercept::yellowGoal, pointOnField, coords, edgeAngleField, edgeAngleImage);
    00978                       typeOfLinesPercept = LinesPercept::yellowGoal;
    00979                       pixelsSinceGoal = 0;
    00980
    00981                       lastRed = lastBlue = 0;
    00982                       redCount = blueCount = 0;
    00983                     }
    00984                     break;
    00985                   case white:
    00986                     if(height > lineHeight * 3 && (vertical || height > 30))
    00987                     {
    00988                       //borderFound = true;
    00989                       double edgeAngleField, edgeAngleImage;
    00990                       if(calcEdgeAngle(edgeAngleImage, edgeAngleField, coords, pointOnField, scanAngle, pixelBuffer))
    00991                       {
    00992                         //linesPercept.add(LinesPercept::border,pointOnField, edgeAngle);
    00993                         borderCandidate.found = true;
    00994                         borderCandidate.pointOnField = pointOnField;
    00995                         borderCandidate.pointInImage = coords;
    00996                         borderCandidate.angleOnField = edgeAngleField;
    00997                         borderCandidate.angleInImage = edgeAngleImage;
    00998                       }
    00999                       //typeOfLinesPercept = LinesPercept::border;
    01000                       lastRed = lastBlue = 0;
    01001                       redCount = blueCount = 0;
    01002                     }
    01003                     break;
    01004                 }
    01005               }
    01006             }
    01007           }
    01008           state = down;
    01009         }
    01010       }
    01011
    01012       colorClass c3 = colorClassBuffer[3];
    01013
    01014       if(c3 == gray && colorClassBuffer[4] == gray && colorClassBuffer[5] && colorClassBuffer[6] == gray)
    01015         {
    01016           if(colorClassBuffer[2] == green && colorClassBuffer[1] == green && colorClassBuffer[0] == green)
    01017           {
    01018             NDOT("SlamIMP:borderPoints",actual.x,actual.y,Drawings::blue,Drawings::blue)
    01019
    01020             // find maximum gradient around point on the scan line
    01021             int indices[] = {3, 4, 2, 5, 1};
    01022             Vector2<double> maxGrad;
    01023             double maxLength = -1;
    01024             int channel = 0;
    01025             bool againstScanline = false;
    01026             for(unsigned int i = 0; i < sizeof(indices) / sizeof(indices[0]); ++i)
    01027             {
    01028               Vector2<int> point = getCoords(pixelBuffer[indices[i]]);
    01029               Vector2<double> grad = Vector2<double>(double(image.image[point.y][channel][point.x]) - image.image[point.y + 1][channel][point.x - 1] ,
    01030               double(image.image[point.y + 1][channel][point.x]) - image.image[point.y][channel][point.x - 1]);
    01031               double length = grad.abs();
    01032
    01033               if(length > maxLength)
    01034               {
    01035                 Vector2<double> gradRotated = (rotation2x2 * Vector2<double>(grad.x, -grad.y)).normalize();
    01036                 gradRotated.y *= -1;
    01037                 // filter gradient which are in wrong direction (belong to other side of line)
    01038                 double angle = gradRotated.angle();
    01039                 bool similarDirection = (angle > scanAngle && angle < scanAngle + pi) || (angle < scanAngle - pi && angle > scanAngle - pi2);
    01040                 if(againstScanline != similarDirection)
    01041                 {
    01042                  maxGrad = grad;
    01043                  maxLength = length;
    01044                 }
    01045               }
    01046              }//for(unsigned int i = 0; i < sizeof(indices) / sizeof(indices[0]); ++i)
    01047             if(maxLength > 0)
    01048             {
    01049               Vector2<double> gradRotated = (rotation2x2 * Vector2<double>(maxGrad.x, -maxGrad.y)).normalize();
    01050               gradRotated.y *= -1;
    01051               Vector2<int> endPoint(int(actual.x + gradRotated.x * 10), int(actual.y + gradRotated.y * 10));
    01052               borderFinder.considerLinePoint(actual,maxGrad);
    01053               NARROW("SlamIMP:borderPoints",actual.x,actual.y,endPoint.x,endPoint.y,1,1,Drawings::red);
    01054             }
    01055           }//if(colorClassBuffer[4] == green && colorClassBuffer[5] == green && colorClassBuffer[6] == green)
    01056         }//if(c3 == gray && colorClassBuffer[2] == gray && colorClassBuffer[1] && colorClassBuffer[0] == gray)
    01057
    01058
    01059       ++noOrangeCount;
    01060       if (c3 == orange)
    01061       {
    01062         if(noOrangeCount > 1)
    01063         {
    01064           if (orangeCount > longestBallRun)
    01065           {
    01066             longestBallRun = orangeCount;
    01067             ballCandidate = (getCoords(firstOrange) + getCoords(lastOrange)) / 2;
    01068           }
    01069 #ifdef MULTIPLE_BALL_CANDIDATES
    01070           if (orangeCount>2)
    01071           {
    01072             ballClustering.addLine(getCoords(firstOrange), getCoords(lastOrange), orangeCount);
    01073           }
    01074 #endif
    01075           orangeCount = 1;
    01076           firstOrange = p3;
    01077           lastOrange = p3;
    01078         }
    01079         else
    01080         {
    01081           ++orangeCount;
    01082           if(orangeCount > 2) // ignore robot if ball is below, distance would be wrong
    01083           {
    01084             lastRed = lastBlue = 0;
    01085             redCount = blueCount = 0;
    01086           }
    01087           lastOrange = p3;
    01088         }
    01089         firstGreen = 0;
    01090         noOrangeCount = 0;
    01091       }
    01092       if(vertical)
    01093       {
    01094         // in scanColumns, recognize player and ball points
    01095         ++noGreenCount;
    01096         ++noPinkCount;
    01097         ++noYellowCount;
    01098         ++noSkyblueCount;
    01099         switch(c3)
    01100         {
    01101           case blue: // count blue, remember last
    01102             if(blueCount == 3)
    01103               firstBlue = pixelBuffer[6];
    01104             ++blueCount;
    01105             lastBlue = p3;
    01106             firstGreen = 0;
    01107             break;
    01108           case gray: // drop green above gray (i.e. robot legs)
    01109             if(firstGreen && noGreenCount < 8)
    01110               firstGreen = 0;
    01111             break;
    01112           case green: // find first green below a robot
    01113             if(!firstGreen)
    01114             {
    01115               firstGreen = p3;
    01116             }
    01117             if(noGreenCount > 6)
    01118               greenCount = 1;
    01119             else
    01120               ++greenCount;
    01121             noGreenCount = 0;
    01122
    01123             if ((greenCount > 5) && (!startOfGreenFound))
    01124             {
    01125               startOfGreenFound = true;
    01126
    01127               startOfGreen = getCoords (firstGreen);
    01128             };
    01129
    01130             break;
    01131           case red:
    01132             if(orangeCount < 6 || noOrangeCount > 4 || redCount > orangeCount)
    01133             { // count red, remember last
    01134               if(redCount == 3)
    01135                 firstRed = pixelBuffer[6];
    01136               ++redCount;
    01137               lastRed = p3;
    01138               firstGreen = 0;
    01139               break;
    01140             }
    01141             // no break, red below orange is interpreted as orange
    01142           case pink:
    01143             if(noPinkCount > 6)
    01144             {
    01145               pinkCount = 1;
    01146               firstPink = p3;
    01147             }
    01148             else
    01149             {
    01150               ++pinkCount;
    01151
    01152 //        if (pinkCount > 10)
    01153         if (pinkCount > 4)
    01154         {
    01155           slamPercept.imageCharacteristic.numPink = 1;
    01156 /*          slamPercept.viewAngle = 0 - cameraMatrix.rotation.getZAngle ();
    01157           slamPercept.viewAngleValid = true;*/
    01158         }
    01159             }
    01160             lastPink = p3;
    01161             noPinkCount = 0;
    01162             break;
    01163           case yellow:
    01164             if(noYellowCount > 6)
    01165               yellowCount = 1;
    01166             else
    01167       {
    01168               ++yellowCount;
    01169
    01170 /*        if (yellowCount > 10)
    01171         {
    01172           specialPercept.viewAngle = -pi - cameraMatrix.rotation.getZAngle ();
    01173           specialPercept.viewAngleValid = true;
    01174         }*/
    01175       }
    01176             noYellowCount = 0;
    01177             break;
    01178           case skyblue:
    01179             if(noSkyblueCount > 6)
    01180               skyblueCount = 1;
    01181             else
    01182               ++skyblueCount;
    01183             noSkyblueCount = 0;
    01184             break;
    01185           default:
    01186             break;
    01187
    01188         }//switch(c3)
    01189
    01190         // obstacles state machine
    01191         if(count > 90 && cameraMatrix.isValid)
    01192         {
    01193           DEBUG_IMAGE_SET_PIXEL_BLACK(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
    01194           colorClass color = colorClassBuffer[0];
    01195           if(groundState == noGroundFound)
    01196           {
    01197             DEBUG_IMAGE_SET_PIXEL_YELLOW(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
    01198             if(color == green || color == orange)
    01199             {
    01200               if(numberOfPixelsSinceFirstOccurrenceOfGroundColor == 0)
    01201               {
    01202                 firstGround = getCoords(pixelBuffer[0]);
    01203               }
    01204               numberOfPixelsSinceFirstOccurrenceOfGroundColor++;
    01205               if(numberOfPixelsSinceFirstOccurrenceOfGroundColor > 3 || color == orange)
    01206               {
    01207                 Vector2<int> coords = getCoords(pixelBuffer[0]);
    01208                 int lineHeight = Geometry::calculateLineSize(coords, cameraMatrix, image.cameraInfo);
    01209                 if(i < lineHeight * 4) beginOfGroundIsAtTheTopOfTheImage = true;
    01210                 groundState = foundBeginOfGround;
    01211                 pixelsBetweenGoalAndObstacle = pixelsSinceGoal;
    01212                 numberOfPixelsSinceFirstOccurrenceOfNonGroundColor = 0;
    01213               }
    01214             }
    01215             else if (color != noColor)
    01216             {
    01217               numberOfPixelsSinceFirstOccurrenceOfGroundColor = 0;
    01218               if(color == white) numberOfWhiteObstaclePixels++;
    01219               else numberOfNotWhiteObstaclePixels++;
    01220             }
    01221           }
    01222           else if(groundState == foundBeginOfGround)
    01223           {
    01224             DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
    01225             if(color != green && color != orange && color != noColor)
    01226             {
    01227               numberOfPixelsSinceFirstOccurrenceOfNonGroundColor++;
    01228               if(numberOfPixelsSinceFirstOccurrenceOfNonGroundColor > 3)
    01229               {
    01230                 groundState = foundEndOfGround;
    01231                 numberOfPixelsSinceLastGround = 0;
    01232                 numberOfWhitePixelsSinceLastGround = 0;
    01233                 numberOfPixelsSinceFirstOccurrenceOfGroundColor = 0;
    01234               }
    01235             }
    01236             else if (color != noColor)
    01237             {
    01238               numberOfGroundPixels++;
    01239               numberOfPixelsSinceFirstOccurrenceOfNonGroundColor = 0;
    01240             }
    01241           }
    01242           else if(groundState == foundEndOfGround)
    01243           {
    01244             numberOfPixelsSinceLastGround++;
    01245             if(color == white) numberOfWhitePixelsSinceLastGround++;
    01246             DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
    01247             if(color == green || color == orange)
    01248             {
    01249               numberOfPixelsSinceFirstOccurrenceOfGroundColor++;
    01250               if(numberOfPixelsSinceFirstOccurrenceOfGroundColor > 3 || color == orange)
    01251               {
    01252                 numberOfPixelsSinceFirstOccurrenceOfNonGroundColor = 0;
    01253                 groundState = foundBeginOfGround;
    01254                 Vector2<int> coords = getCoords(pixelBuffer[0]),
    01255                              pointOnField; //position on field, relative to robot
    01256                 int lineHeight = Geometry::calculateLineSize(coords, cameraMatrix, image.cameraInfo);
    01257                 // * 4 wenn 80% weiß (schräge linie). sonst * 1.5 (keine linie)
    01258                 if(numberOfPixelsSinceLastGround > lineHeight * (numberOfPixelsSinceLastGround * 4 < numberOfWhitePixelsSinceLastGround * 5 ? 4 : 1.5))
    01259                 {
    01260                   DEBUG_IMAGE_SET_PIXEL_PINK(imageProcessorPlayers, getCoords(pixelBuffer[0]).x, getCoords(pixelBuffer[0]).y);
    01261                   firstGround = getCoords(pixelBuffer[0]);
    01262                   numberOfGroundPixels = 0;
    01263                   beginOfGroundIsAtTheTopOfTheImage = false;
    01264                 }
    01265               }
    01266             }
    01267             else if (color != noColor)
    01268             {
    01269               numberOfPixelsSinceFirstOccurrenceOfGroundColor = 0;
    01270               if(color == white)
    01271                 numberOfWhiteObstaclePixels++;
    01272               else
    01273                 numberOfNotWhiteObstaclePixels++;
    01274             }
    01275           }
    01276         } // if(count > 100)
    01277
    01278       }//if(vertical)
    01279
    01280       PLOT(p3,ColorClasses::colorClassToDrawingsColor(c3));
    01281
    01282       // Bresenham
    01283       if(isVertical)
    01284       {
    01285         actual.y += step.y;
    01286         sum += size.x;
    01287         if(sum >= size.y)
    01288         {
    01289           sum -= size.y;
    01290           actual.x += step.x;
    01291         }
    01292       }
    01293       else
    01294       {
    01295         actual.x += step.x;
    01296         sum += size.y;
    01297         if(sum >= size.x)
    01298         {
    01299           sum -= size.x;
    01300           actual.y += step.y;
    01301         }
    01302       }
    01303     }
    01304
    01305     //if(!borderFound && borderCandidate.found)
    01306     //{
    01307     //  linesPercept.add(LinesPercept::border,borderCandidate.pointOnField, borderCandidate.pointInImage, borderCandidate.angleOnField, borderCandidate.angleInImage);
    01308     //}
    01309
    01310     if (orangeCount > longestBallRun)
    01311     {
    01312       longestBallRun = orangeCount;
    01313       ballCandidate = (getCoords(firstOrange) + getCoords(lastOrange)) / 2;
    01314     }
    01315 #ifdef MULTIPLE_BALL_CANDIDATES
    01316     if (orangeCount>2)
    01317     {
    01318       ballClustering.addLine(getCoords(firstOrange), getCoords(lastOrange), orangeCount);
    01319     }
    01320 #endif
    01321
    01322     const unsigned char* pLast = pixelBuffer[3];
    01323     PLOT(pLast,Drawings::numberOfColors);
    01324
    01325     if(vertical)
    01326     { // line scanning finished, analyze results (only in scanColumns)
    01327       int goal = getPlayer().getTeamColor() == Player::red ? LinesPercept::skyblueGoal
    01328                                                            : LinesPercept::yellowGoal;
    01329       if(linesPercept.points[goal].numberOfPoints == numberOfPoints[goal]) // no goal point found in this column
    01330       {
    01331         Vector2<int> coords = getCoords(pLast),
    01332                      pointOnField;
    01333         if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
    01334         {
    01335           int dist = pointOnField.abs();
    01336           if(dist < closestBottom)
    01337             closestBottom = dist;
    01338         }
    01339       }
    01340
    01341       bool redFound = false,
    01342            blueFound = false;
    01343
    01344       // red robot point found?
    01345       if(redCount > blueCount && (firstGreen && redCount > 2 || redCount > 20))
    01346       { // drop border and goal points
    01347         for(i = 1; i < LinesPercept::numberOfLineTypes; ++i)
    01348           linesPercept.points[i].numberOfPoints = numberOfPoints[i];
    01349         if(redCount > 20)
    01350         { // if robot is close, use upper boundary of tricot to calculate distance
    01351           Vector2<int> coords = getCoords(firstRed),
    01352                        pointOnField; //position on field, relative to robot
    01353           if(Geometry::calculatePointOnField(coords.x, coords.y, cmTricot, prevCmTricot, image.cameraInfo, pointOnField) &&
    01354              pointOnField.abs() < 500)
    01355           {
    01356             DOT(imageProcessor_general, coords.x, coords.y, Drawings::red, Drawings::white);
    01357             linesPercept.add(LinesPercept::redRobot,pointOnField,coords);
    01358             typeOfLinesPercept = LinesPercept::redRobot;
    01359             redFound = true;
    01360           }
    01361         }
    01362         if(!redFound)
    01363         { // otherwise, use foot point
    01364           Vector2<int> coords = getCoords(firstGreen),
    01365                        pointOnField; //position on field, relative to robot
    01366           if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
    01367           {
    01368             Vector2<int> redCoords = getCoords(lastRed);
    01369             if((redCoords - coords).abs() < Geometry::getSizeByDistance(image.cameraInfo, 100, pointOnField.abs()))
    01370             {
    01371               linesPercept.add(LinesPercept::redRobot,pointOnField,coords);
    01372               typeOfLinesPercept = LinesPercept::redRobot;
    01373               redFound = true;
    01374             }
    01375           }
    01376         }
    01377       }
    01378       // blue robot point found?
    01379       else if(blueCount > redCount && (firstGreen && blueCount > 2 || blueCount > 10))
    01380       { // drop border and goal points
    01381         for(i = 1; i < LinesPercept::numberOfLineTypes; ++i)
    01382           linesPercept.points[i].numberOfPoints = numberOfPoints[i];
    01383         if(blueCount > 10)
    01384         { // if robot is close, use upper boundary of tricot to calculate distance
    01385           Vector2<int> coords = getCoords(firstBlue),
    01386                        pointOnField; //position on field, relative to robot
    01387           if(Geometry::calculatePointOnField(coords.x, coords.y, cmTricot, prevCmTricot, image.cameraInfo, pointOnField) &&
    01388              pointOnField.abs() < 500)
    01389           {
    01390             DOT(imageProcessor_general, coords.x, coords.y, Drawings::pink, Drawings::white);
    01391             linesPercept.add(LinesPercept::blueRobot,pointOnField,coords);
    01392             typeOfLinesPercept = LinesPercept::blueRobot;
    01393             blueFound = true;
    01394           }
    01395         }
    01396         if(!blueFound)
    01397         { // otherwise, use foot point
    01398           Vector2<int> coords = getCoords(firstGreen),
    01399                        pointOnField; //position on field, relative to robot
    01400           if(Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField))
    01401           {
    01402             Vector2<int> blueCoords = getCoords(lastBlue);
    01403             if((blueCoords - coords).abs() < Geometry::getSizeByDistance(image.cameraInfo, 100, pointOnField.abs()))
    01404             {
    01405               linesPercept.add(LinesPercept::blueRobot,pointOnField,coords);
    01406               typeOfLinesPercept = LinesPercept::blueRobot;
    01407               blueFound = true;
    01408             }
    01409           }
    01410         }
    01411       }
    01412       // cluster the robot points found
    01413       clusterRobots(pLast, redFound, blueFound);
    01414
    01415       // obstacles found?
    01416       bool addObstaclesPoint = false;
    01417       ObstaclesPercept::Segment s;
    01418       if(count > 90 && cameraMatrix.isValid)
    01419       {
    01420         Vector2<int> firstGroundOnField;
    01421         bool firstGroundOnFieldIsOnField =
    01422           Geometry::calculatePointOnField(firstGround.x, firstGround.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, firstGroundOnField);
    01423
    01424         Vector2<int> imageBottom = getCoords(pixelBuffer[0]);
    01425         Vector2<int> imageBottomOnField;
    01426         bool imageBottomIsOnField =
    01427           Geometry::calculatePointOnField(imageBottom.x, imageBottom.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, imageBottomOnField);
    01428
    01429         /*
    01430         * Slam
    01431         */
    01432
    01433         if (firstGroundOnFieldIsOnField)
    01434         {
    01435           double distance = firstGroundOnField.abs ();
    01436
    01437           if (distance > slamPercept.greenLocation.lengthOfMaxGreen)
    01438           {
    01439             slamPercept.greenLocation.maxGreenPointInImage = firstGround;
    01440             slamPercept.greenLocation.maxGreenPointOnField.x = (int) firstGroundOnField.x;
    01441             slamPercept.greenLocation.maxGreenPointOnField.y = (int) firstGroundOnField.y;
    01442
    01443             slamPercept.greenLocation.impLengthOfMaxGreen = distance;
    01444             slamPercept.greenLocation.impAngleToMaxGreen = firstGroundOnField.angle ();
    01445             slamPercept.greenLocation.impAngleToMaxGreenValid = true;
    01446           };
    01447         };
    01448
    01449         /*
    01450         * /Slam
    01451         */
    01452
    01453         if(groundState == foundBeginOfGround)
    01454         {
    01455           if(firstGroundOnFieldIsOnField)
    01456           {
    01457             addObstaclesPoint = true;
    01458             s.nearPointOnField = Vector2<double>(imageBottomOnField.x, imageBottomOnField.y);
    01459             s.farPointOnField = Vector2<double>(firstGroundOnField.x, firstGroundOnField.y);
    01460             s.nearPointInImage = Vector2<double>(imageBottom.x, imageBottom.y);
    01461             s.farPointInImage = Vector2<double>(firstGround.x, firstGround.y);
    01462             s.farPointIsOnImageBorder = beginOfGroundIsAtTheTopOfTheImage;
    01463           }
    01464         }
    01465         else if(groundState == foundEndOfGround)
    01466         {
    01467           int lineHeight = Geometry::calculateLineSize(imageBottom, cameraMatrix, image.cameraInfo);
    01468           if(numberOfPixelsSinceLastGround < lineHeight * 4)
    01469           {
    01470             if(firstGroundOnFieldIsOnField)
    01471             {
    01472               addObstaclesPoint = true;
    01473               s.nearPointOnField = Vector2<double>(imageBottomOnField.x, imageBottomOnField.y);
    01474               s.farPointOnField = Vector2<double>(firstGroundOnField.x, firstGroundOnField.y);
    01475               s.nearPointInImage = Vector2<double>(imageBottom.x, imageBottom.y);
    01476               s.farPointInImage = Vector2<double>(firstGround.x, firstGround.y);
    01477               s.farPointIsOnImageBorder = beginOfGroundIsAtTheTopOfTheImage;
    01478             }
    01479           }
    01480           else if(imageBottomIsOnField)
    01481           {
    01482             addObstaclesPoint = true;
    01483             s.nearPointOnField = s.farPointOnField = Vector2<double>(imageBottomOnField.x, imageBottomOnField.y);
    01484             s.nearPointInImage = s.farPointInImage = Vector2<double>(imageBottom.x, imageBottom.y);
    01485           }
    01486         }
    01487         else if(groundState == noGroundFound)
    01488         {
    01489           if(imageBottomIsOnField &&
    01490             // the carpet often is not green under the robot
    01491             imageBottomOnField.abs() > 150)
    01492           {
    01493             addObstaclesPoint = true;
    01494             s.nearPointOnField = s.farPointOnField = Vector2<double>(imageBottomOnField.x, imageBottomOnField.y);
    01495             s.nearPointInImage = s.farPointInImage = Vector2<double>(imageBottom.x, imageBottom.y);
    01496           }
    01497         }
    01498       }// if(count > 90 && cameraMatrix.isValid)
    01499       if(addObstaclesPoint)
    01500       {
    01501         if(
    01502           angleBetweenDirectionOfViewAndGround > -80.0 &&
    01503           !(s.farPointOnField.x == 0 && s.farPointOnField.y == 0)
    01504           )
    01505
    01506         {
    01507           switch(typeOfLinesPercept)
    01508           {
    01509           case LinesPercept::skyblueGoal:
    01510             if(pixelsBetweenGoalAndObstacle < 15)
    01511               s.obstacleType = ObstaclesPercept::goal;
    01512             break;
    01513           case LinesPercept::yellowGoal:
    01514             if(pixelsBetweenGoalAndObstacle < 15)
    01515               s.obstacleType = ObstaclesPercept::goal;
    01516             break;
    01517           case LinesPercept::blueRobot:
    01518             if(getPlayer().getTeamColor() == Player::blue)
    01519               s.obstacleType = ObstaclesPercept::teammate;
    01520             else
    01521               s.obstacleType = ObstaclesPercept::opponent;
    01522             break;
    01523           case LinesPercept::redRobot:
    01524             if(getPlayer().getTeamColor() == Player::red)
    01525               s.obstacleType = ObstaclesPercept::teammate;
    01526             else
    01527               s.obstacleType = ObstaclesPercept::opponent;
    01528             break;
    01529           //case LinesPercept::border:
    01530           //  s.obstacleType = ObstaclesPercept::border;
    01531           }
    01532
    01533           obstaclesPercept.add(s);
    01534         }
    01535       }
    01536     }//if(vertical)
    01537
    01538 /*
    01539     if (startOfGreenFound)
    01540     {
    01541       Vector2<double> pointOnField;
    01542
    01543       Geometry::calculatePointOnField (startOfGreen.x, startOfGreen.y, cameraMatrix, image.cameraInfo, pointOnField);
    01544
    01545       double distance = pointOnField.abs ();
    01546
    01547       if (distance > slamPercept.greenLocation.lengthOfMaxGreen)
    01548       {
    01549         slamPercept.greenLocation.maxGreenPointInImage = startOfGreen;
    01550         slamPercept.greenLocation.maxGreenPointOnField.x = (int) pointOnField.x;
    01551         slamPercept.greenLocation.maxGreenPointOnField.y = (int) pointOnField.y;
    01552
    01553         slamPercept.greenLocation.impLengthOfMaxGreen = distance;
    01554         slamPercept.greenLocation.impAngleToMaxGreen = pointOnField.angle ();
    01555         slamPercept.greenLocation.impAngleToMaxGreenValid = true;
    01556       };
    01557     };*/
    01558   }
    01559 }
    01560
    01561 void SlamImageProcessor::clusterRobots(const unsigned char* bottomPoint,
    01562                                          bool redFound, bool blueFound)
    01563 {
    01564   Vector2<int> coords = getCoords(bottomPoint),
    01565                pointOnField;
    01566   int noRedCount2 = noRedCount;
    01567
    01568   if(redFound)
    01569   {
    01570     lastRed =
    01571       linesPercept.points[LinesPercept::redRobot].pointsOnField[linesPercept.points[LinesPercept::redRobot].numberOfPoints - 1];
    01572     if(noRedCount > 2)
    01573       firstRed = closestRed = lastRed;
    01574     else if(closestRed.abs() > lastRed.abs())
    01575       closestRed = lastRed;
    01576     noRedCount = 0;
    01577   }
    01578   // count number of columns without robot points, but ignore columns that are
    01579   // not long enough to contain the robot. Drop single robot points of the other
    01580   // robot color.
    01581   else if(noRedCount > 2 || !bottomPoint ||
    01582           (Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField) &&
    01583           closestRed.abs() > pointOnField.abs()))
    01584   {
    01585     if(++noRedCount == 3 && (firstRed != lastRed || noBlueCount > 4))
    01586     {
    01587       SinglePlayerPercept percept;
    01588       percept.directionOnField = (atan2((double)firstRed.y,(double)firstRed.x) + atan2((double)lastRed.y,(double)lastRed.x)) / 2;
    01589       double distance = closestRed.abs();
    01590       percept.offsetOnField.x = cos(percept.directionOnField) * distance;
    01591       percept.offsetOnField.y = sin(percept.directionOnField) * distance;
    01592       percept.positionInImage = coords;
    01593       percept.validity = 1;
    01594       playersPercept.addRedPlayer(percept);
    01595     }
    01596   }
    01597
    01598   if(blueFound)
    01599   {
    01600     lastBlue =
    01601       linesPercept.points[LinesPercept::blueRobot].pointsOnField[linesPercept.points[LinesPercept::blueRobot].numberOfPoints - 1];
    01602     if(noBlueCount > 2)
    01603       firstBlue = closestBlue = lastBlue;
    01604     else if(closestBlue.abs() > lastBlue.abs())
    01605       closestBlue = lastBlue;
    01606     noBlueCount = 0;
    01607   }
    01608   // count number of columns without robot points, but ignore columns that are
    01609   // not long enough to contain the robot. Drop single robot points of the other
    01610   // robot color.
    01611   else if(noBlueCount > 2 || !bottomPoint ||
    01612           (Geometry::calculatePointOnField(coords.x, coords.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, pointOnField) &&
    01613           closestBlue.abs() > pointOnField.abs()))
    01614   {
    01615     if(++noBlueCount == 3 && (firstBlue != lastBlue || noRedCount2 > 4))
    01616     {
    01617       SinglePlayerPercept percept;
    01618       percept.directionOnField = (atan2((double)firstBlue.y,(double)firstBlue.x) + atan2((double)lastBlue.y,(double)lastBlue.x)) / 2;
    01619       double distance = closestBlue.abs();
    01620       percept.offsetOnField.x = int(cos(percept.directionOnField) * distance);
    01621       percept.offsetOnField.y = int(sin(percept.directionOnField) * distance);
    01622       percept.positionInImage = coords;
    01623       percept.validity = 1;
    01624       playersPercept.addBluePlayer(percept);
    01625     }
    01626   }
    01627 }
    01628
    01629 void SlamImageProcessor::filterPercepts()
    01630 {
    01631   // Close robots produce far ghost robots. Remove them.
    01632   int i;
    01633   for(i = 0; i < playersPercept.numberOfRedPlayers + playersPercept.numberOfBluePlayers; ++i)
    01634   {
    01635     SinglePlayerPercept& pi = i < playersPercept.numberOfRedPlayers
    01636                       ? playersPercept.redPlayers[i]
    01637                       : playersPercept.bluePlayers[i - playersPercept.numberOfRedPlayers];
    01638     double iDist = pi.offsetOnField.abs(),
    01639            iAngle = atan2(pi.offsetOnField.y, pi.offsetOnField.x),
    01640            ignoreAngle = atan2(150, iDist);
    01641     for(int j = 0; j < playersPercept.numberOfRedPlayers + playersPercept.numberOfBluePlayers; ++j)
    01642       if(i != j)
    01643       {
    01644         SinglePlayerPercept& pj = j < playersPercept.numberOfRedPlayers
    01645                             ? playersPercept.redPlayers[j]
    01646                             : playersPercept.bluePlayers[j - playersPercept.numberOfRedPlayers];
    01647         if(iDist < pj.offsetOnField.abs() &&
    01648            fabs(atan2(pj.offsetOnField.y, pj.offsetOnField.x) - iAngle) < ignoreAngle)
    01649         {
    01650           if(j < playersPercept.numberOfRedPlayers)
    01651             pj = playersPercept.redPlayers[--playersPercept.numberOfRedPlayers];
    01652           else
    01653             pj = playersPercept.bluePlayers[--playersPercept.numberOfBluePlayers];
    01654           if(i > j)
    01655           {
    01656             i = j; // check pi again
    01657             break;
    01658           }
    01659           else
    01660             --j; // check pj again
    01661         }
    01662       }
    01663   }
    01664
    01665   // If there are too few line points of a certain type, remove them all, they may be misreadings
    01666   for(i = 0; i < LinesPercept::numberOfLineTypes; ++i)
    01667     if(linesPercept.points[i].numberOfPoints < 3)
    01668       linesPercept.points[i].numberOfPoints = 0;
    01669
    01670   // Remove outliers in the border
    01671   //filterLinesPercept(linesPercept, LinesPercept::border, cameraMatrix, imageInfo.previousCameraMatrix, image);
    01672   /*
    01673   // First run: find them
    01674   int prev = 0;
    01675   for(i = 1; i < linesPercept.numberOfPoints[LinesPercept::border] - 1; ++i)
    01676   {
    01677     LinesPercept::LinePoint& pPrev = linesPercept.points[LinesPercept::border][prev],
    01678                            & p = linesPercept.points[LinesPercept::border][i],
    01679                            & pNext = linesPercept.points[LinesPercept::border][i + 1];
    01680
    01681     // Field wall is convex. So, no point can be closer than its two neighbors
    01682     int dist = p.abs();
    01683     if(pPrev.abs() > dist && pNext.abs() > dist)
    01684       p.angle = LinesPercept::UNDEF;
    01685     else
    01686       ++prev;
    01687   }
    01688
    01689   // second run: remove marked points
    01690   prev = 0;
    01691   for(i = 1; i < linesPercept.numberOfPoints[LinesPercept::border] - 1; ++i)
    01692     if(linesPercept.points[LinesPercept::border][i].angle != LinesPercept::UNDEF)
    01693       linesPercept.points[LinesPercept::border][prev++] = linesPercept.points[LinesPercept::border][i];
    01694   linesPercept.numberOfPoints[LinesPercept::border] = prev;
    01695   */
    01696
    01697   // Remove outliers in the field lines
    01698   filterLinesPercept(linesPercept, LinesPercept::field, cameraMatrix, imageInfo.previousCameraMatrix, image);
    01699   /*
    01700   // First run: find them
    01701   prev = 0;
    01702   for(i = 2; i <= linesPercept.numberOfPoints[LinesPercept::field]; i += 2)
    01703   {
    01704     LinesPercept::LinePoint& pPrev = linesPercept.points[LinesPercept::field][i - 2],
    01705                            & p = linesPercept.points[LinesPercept::field][i];
    01706
    01707     // end of line?
    01708     double angle = fabs(((Vector2<int>&) pPrev).angle() - ((Vector2<int>&) p).angle());
    01709     int dist1 = pPrev.abs(),
    01710         dist2 = p.abs();
    01711     if(i == linesPercept.numberOfPoints[LinesPercept::field] ||
    01712        angle > 0.08 ||
    01713        abs(dist1 - dist2) > 2 * fabs(tan(angle / 2) * (dist1 + dist2)))
    01714     {
    01715       if(i - prev < 6)
    01716         while(prev < i)
    01717           linesPercept.points[LinesPercept::field][prev++].angle = LinesPercept::UNDEF;
    01718       prev = i;
    01719     }
    01720   }
    01721
    01722   // second run: remove marked points
    01723   prev = 0;
    01724   for(i = 0; i < linesPercept.numberOfPoints[LinesPercept::field]; ++i)
    01725     if(linesPercept.points[LinesPercept::field][i].angle != LinesPercept::UNDEF)
    01726       linesPercept.points[LinesPercept::field][prev++] = linesPercept.points[LinesPercept::field][i];
    01727   linesPercept.numberOfPoints[LinesPercept::field] = prev;
    01728   */
    01729 }
    01730
    01731 bool SlamImageProcessor::calcEdgeAngle(
    01732   double& angleInImage,
    01733   double& angleOnField,
    01734   const Vector2<int>& pointInImage,
    01735   const Vector2<int>& pointOnField,
    01736   double scanAngle,
    01737   const RingBuffer<const unsigned char*,7>& pixelBuffer,
    01738   const bool againstScanline,
    01739   const bool borderOrLine,
    01740   int channel)
    01741 {
    01742   // find maximum gradient around point on the scan line
    01743   int indices[] = {3, 4, 2, 5, 1};
    01744   Vector2<double> maxGrad;
    01745   double maxLength = -1;
    01746   for(unsigned int i = 0; i < sizeof(indices) / sizeof(indices[0]); ++i)
    01747   {
    01748     Vector2<int> point = getCoords(pixelBuffer[indices[i]]);
    01749     Vector2<double> grad = Vector2<double>(double(image.image[point.y][channel][point.x]) - image.image[point.y + 1][channel][point.x - 1] ,
    01750       double(image.image[point.y + 1][channel][point.x]) - image.image[point.y][channel][point.x - 1]);
    01751     double length = grad.abs();
    01752
    01753     if(length > maxLength)
    01754     {
    01755       Vector2<double> gradRotated = (rotation2x2 * Vector2<double>(grad.x, -grad.y)).normalize();
    01756       gradRotated.y *= -1;
    01757       // filter gradient which are in wrong direction (belong to other side of line)
    01758       double angle = gradRotated.angle();
    01759       bool similarDirection = (angle > scanAngle && angle < scanAngle + pi) || (angle < scanAngle - pi && angle > scanAngle - pi2);
    01760       if(againstScanline != similarDirection)
    01761       {
    01762         maxGrad = grad;
    01763         maxLength = length;
    01764       }
    01765     }
    01766   }
    01767
    01768   if(maxLength > 0)
    01769   {
    01770     Vector2<double> gradRotated = (rotation2x2 * Vector2<double>(maxGrad.x, -maxGrad.y)).normalize();
    01771     gradRotated.y *= -1;
    01772
    01773     // check whether edge direction differs too much from scan direction
    01774     double diff = fabs(scanAngle - gradRotated.angle());
    01775     if(diff > pi)
    01776       diff = pi2 - diff;
    01777     if(diff < pi / 6 || diff > 5 * pi / 6)
    01778     {
    01779       angleInImage = angleOnField = LinesPercept::UNDEF;
    01780       return false;
    01781     }
    01782     Vector2<int> endPoint(int(pointInImage.x + gradRotated.x * 10), int(pointInImage.y + gradRotated.y * 10));
    01783     ARROW(imageProcessor_edges,
    01784       pointInImage.x,
    01785       pointInImage.y,
    01786       endPoint.x,
    01787       endPoint.y,
    01788       1, 1, (againstScanline ? Drawings::red : Drawings::pink)
    01789     );
    01790
    01791     if (borderOrLine)
    01792     {
    01793       Vector2<int> pointImage = pointInImage;
    01794       Vector2<int> pointField = pointOnField;
    01795 //      circleFinder.addCandidate(pointField,pointImage);
    01796       Vector2<double> normalToLine = gradRotated;
    01797       normalToLine = normalToLine.rotateLeft();
    01798       Vector2<int> pointOnLine = pointInImage;
    01799       lineFinder.considerLinePoint(pointOnLine, normalToLine);
    01800     }
    01801
    01802     Vector2<int> point;
    01803     if(Geometry::calculatePointOnField(endPoint.x, endPoint.y, cameraMatrix, imageInfo.previousCameraMatrix, image.cameraInfo, point))
    01804     {
    01805       angleInImage = normalize((endPoint - pointInImage).angle() - pi_2);
    01806       angleOnField = normalize((point - pointOnField).angle() + pi_2);
    01807       return true;
    01808     }
    01809   }
    01810   angleInImage = angleOnField = LinesPercept::UNDEF;
    01811   return false;
    01812 }
    01813
    01814 void SlamImageProcessor::plot(const unsigned char* p,Drawings::Color color)
    01815 {
    01816   if(lineColor == Drawings::numberOfColors)
    01817   {
    01818     last = p;
    01819     lineColor = color;
    01820   }
    01821   else if(color != lineColor)
    01822   {
    01823     Vector2<int> p1 = getCoords(last),
    01824                  p2 = getCoords(p);
    01825     LINE(imageProcessor_general,p1.x,p1.y,p2.x,p2.y,0,Drawings::ps_solid,lineColor);
    01826     last = p;
    01827     lineColor = color;
    01828   }
    01829 }
    01830
    01831 bool SlamImageProcessor::handleMessage(InMessage& message)
    01832 {
    01833   switch(message.getMessageID())
    01834   {
    01835 /*
    01836     case idLinesImageProcessorParameters:
    01837     {
    01838       GenericDebugData d;
    01839       message.bin >> d;
    01840       yThreshold = (int)d.data[1];
    01841       vThreshold = (int)d.data[2];
    01842       execute();
    01843       return true;
    01844     }
    01845 */
    01846     case idColorTable64: beaconDetector.analyzeColorTable();
    01847       return true;
    01848   }
    01849   return false;
    01850 }
    01851
    01852 void SlamImageProcessor::filterLinesPercept(LinesPercept& percept, int type, const CameraMatrix& cameraMatrix, const CameraMatrix& prevCameraMatrix, const Image& image)
    01853 {
    01854   const int maxNumberOfEdgePoints = 200;
    01855   double maxAngle = 0.25; // 8.6°
    01856   double maxDist = 0.187; // 80°
    01857
    01858   int i, j;
    01859
    01860   // for all lines, calculate how many other lines are similar/near in sense of this special norm
    01861   bool similar[maxNumberOfEdgePoints][maxNumberOfEdgePoints];
    01862   for(i = 0; i < percept.points[type].numberOfPoints; i++)
    01863   {
    01864     Vector2<double> dir1 = Vector2<double>(cos(percept.points[type].pointsOnField[i].angle),sin(percept.points[type].pointsOnField[i].angle));
    01865
    01866     //int lineHeightI = Geometry::calculateLineSize(percept.points[type][i], cameraMatrix, image.cameraInfo);
    01867     for(j = i + 1; j < percept.points[type].numberOfPoints; j++)
    01868     {
    01869       // similar/near if distance is small and normal is in the same direction
    01870       bool sim = false;
    01871       // test projection first - to remove unneeded calculations of "expensive" distance
    01872       double angleDiff = fabs(normalize(percept.points[type].pointsOnField[i].angle - percept.points[type].pointsOnField[j].angle));
    01873       if(angleDiff < maxAngle)
    01874       {
    01875         // distance point to line:
    01876         // http://mo.mathematik.uni-stuttgart.de/kurse/kurs8/seite44.html
    01877         // NOTE: this is because in fact, the line.direction is here the *normal* to the real line
    01878         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();
    01879         double distance1 = fabs(v1 * dir1);
    01880         if(distance1 < maxDist)
    01881         {
    01882           Vector2<double> dir2 = Vector2<double>(cos(percept.points[type].pointsOnField[j].angle),sin(percept.points[type].pointsOnField[j].angle));
    01883           double distance2 = fabs(-v1 * dir2);
    01884           sim = (distance2 < maxDist);
    01885         }
    01886       }
    01887       similar[i][j] = sim;
    01888       similar[j][i] = sim;
    01889       // draw similarity-lines
    01890       /*Vector2<int> p1,p2;
    01891       Geometry::calculatePointInImage(percept.points[type][i], cameraMatrix, image.cameraInfo, p1);
    01892       Geometry::calculatePointInImage(percept.points[type][j], cameraMatrix, image.cameraInfo, p2);
    01893       if(sim) LINE(imageProcessor_edges,
    01894         p1.x,
    01895         p1.y,
    01896         p2.x,
    01897         p2.y,
    01898         0, 0, 1
    01899       );*/
    01900     }
    01901   }
    01902
    01903   // remove points with low number of similar points
    01904   int nextFree = 0;
    01905
    01906   // for all points, calculate how many other are similar
    01907   for(i = 0; i < percept.points[type].numberOfPoints; i++)
    01908   {
    01909     int countSim = 0;
    01910     //if(edgePoints[i].belongsToLineNo == -1) // only if point is not yet matched to an edge
    01911     for (j = 0; j < percept.points[type].numberOfPoints; j++)
    01912     {
    01913       //if(edgePoints[j].belongsToLineNo == -1) // only if point is not yet matched to an edge
    01914       if(similar[i][j])
    01915       {
    01916         countSim++;
    01917       }
    01918     }
    01919     if(countSim >= 2) // keep percept
    01920     {
    01921       percept.points[type].pointsInImage[nextFree] = percept.points[type].pointsInImage[i];
    01922       percept.points[type].pointsOnField[nextFree++] = percept.points[type].pointsOnField[i];
    01923     }
    01924   }
    01925   percept.points[type].numberOfPoints = nextFree;
    01926 }