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

# Modules/ImageProcessor/GT2005ImageProcessor/GT2005ImageProcessor.cpp

Go to the documentation of this file.

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