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

# Modules/ImageProcessor/VLCImageProcessor/VLCGoalRecognizer.cpp

Go to the documentation of this file.

    00001 /**
    00002  * @file VLCGoalRecognizer.cpp
    00003  *
    00004  * Implementation of class VLCGoalRecognizer
    00005  *
    00006  * @author <a href="mailto:oberlies@sim.tu-darmstadt.de">Tobias Oberlies</a>
    00007  */
    00008
    00009 //#include "Tools/Player.h"
    00010 //#include "Tools/FieldDimensions.h"
    00011 //#include "Tools/RangeArray.h"
    00012 //#include "Tools/Debugging/View.h"
    00013 //#include "Representations/Perception/Image.h"
    00014 //#include "Representations/Perception/ColorTable.h"
    00015 //#include "Representations/Perception/ObstaclesPercept.h"
    00016 //#include "Representations/Perception/LandmarksPercept.h"
    00017 //#include "Representations/Perception/CameraMatrix.h"
    00018 //#include "Modules/ImageProcessor/ImageProcessorTools/ColorTableReferenceColor.h"
    00019 #include "Tools/ColorClasses.h"
    00020 #include "Tools/Math/Geometry.h"
    00021 #include "Tools/Debugging/DebugImages.h"
    00022 #include "Tools/Debugging/DebugDrawings.h"
    00023 #include "Platform/GTAssert.h"
    00024 #include "Representations/Perception/LandmarksPercept.h"
    00025 #include "VLCImageProcessorTools.h"
    00026 #include "VLCGoalRecognizer.h"
    00027
    00028
    00029
    00030 /** Parameters */
    00031 enum parameters
    00032 {
    00033   minGoalpostHeight = 7,
    00034   maxScanLengthToGoalpostEdge = 14
    00035 };
    00036
    00037
    00038
    00039 /**
    00040  * Macro to draw a rectangle that is given in horizon-aligned coordinates.
    00041  * @param horizoninfo The current ImageInfo instance.
    00042  * @param x1,y1,x2,y2 The coordinates of 2 opposite corners in horizon-aligned coordinates.
    00043  * @param penwidth The line width.
    00044  * @param penstyle The line style, e.g. ls_solid.
    00045  * @param pencolor The drawing colour.
    00046  */
    00047 #ifdef NDEBUG
    00048 #define HORIZON_ALIGNED_RECTANGLE(horizoninfo, x1, y1, x2, y2, penwidth, penstyle, pencolor) ;
    00049
    00050 #else
    00051 #define HORIZON_ALIGNED_RECTANGLE(horizoninfo, x1, y1, x2, y2, penwidth, penstyle, pencolor) \
    00052   { \
    00053     Vector2<int> _topleft = horizoninfo.toImageCoordinates(x1, y1); \
    00054     Vector2<int> _bottomleft = horizonInfo.toImageCoordinates(x1, y2); \
    00055     Vector2<int> _bottomright = horizoninfo.toImageCoordinates(x2, y2); \
    00056     Vector2<int> _topright = horizoninfo.toImageCoordinates(x2, y1); \
    00057         QUADRANGLE(imageProcessor_flagsAndGoals, _topleft.x, _topleft.y, _bottomleft.x, _bottomleft.y, _bottomright.x, _bottomright.y, _topright.x, _topright.y, penwidth, penstyle, pencolor); \
    00058   }
    00059
    00060 #endif
    00061
    00062
    00063 VLCGoalRecognizer::VLCGoalRecognizer(colorClass color, const ImageProcessorInterfaces& interfaces, const ColorCorrector& colorCorrector, const ImageInfo& horizonInfo)
    00064   : goalColor(color),
    00065     interfaces(interfaces),
    00066     colorCorrector(colorCorrector),
    00067     horizonInfo(horizonInfo)
    00068 {
    00069 }
    00070
    00071 void VLCGoalRecognizer::notifyAboutNewImage()
    00072 {
    00073   // Reset detection counter
    00074   detectionCounter = 0;
    00075
    00076   // Clear locked areas and goal hypothesises
    00077   lockAreaCount = 0;
    00078   hypothesisCount = 0;
    00079
    00080   // Copy current image into the debug image
    00081   if (yellow == goalColor)
    00082   {
    00083     INIT_DEBUG_IMAGE(imageProcessorGoal1, interfaces.image);
    00084   }
    00085   else
    00086   {
    00087     INIT_DEBUG_IMAGE(imageProcessorGoal2, interfaces.image);
    00088   }
    00089 }
    00090
    00091 bool VLCGoalRecognizer::inspectNeighbourhood(const Vector2<int>& point)
    00092 {
    00093   // Skip if on the image border
    00094   if (nearImageBorder(point, 0))
    00095     return false;
    00096
    00097   // Look at 9-connected neighbourhood and count the number of pixels in target colour
    00098   int targetCount = 0;
    00099   for (int i = -1; i<=1; ++i)
    00100     for (int j = -1; j<=1; ++j)
    00101       if (goalColor == CORRECTED_COLOR_CLASS(point.x+i, point.y+j, interfaces.image.image[point.y+j][0][point.x+i], interfaces.image.image[point.y+j][1][point.x+i], interfaces.image.image[point.y+j][2][point.x+i], (*bestColorTable), colorCorrector))
    00102       {
    00103         // This pixel has the goal colour
    00104         ++targetCount;
    00105       }
    00106
    00107   // Positive result if the majority of the pixels are in goal colour
    00108   return targetCount>4;
    00109 }
    00110
    00111 void VLCGoalRecognizer::notifyAboutFinish()
    00112 {
    00113   // Edge detector state machines
    00114   VLCGoalRecognizer::EdgeDetector detector[2] =
    00115   {
    00116     VLCGoalRecognizer::EdgeDetector(goalColor),
    00117     VLCGoalRecognizer::EdgeDetector(goalColor)
    00118   };
    00119
    00120   // Merge goal fragments
    00121   bool deletedHypothesis[maxHypothesises];
    00122   mergeFragments(deletedHypothesis);
    00123
    00124   // Interpret goal hypothesises
    00125   interpretResults(detector, deletedHypothesis);
    00126
    00127
    00128   if (yellow == goalColor)
    00129   {
    00130     // Send debug image
    00131     SEND_DEBUG_IMAGE(imageProcessorGoal1);
    00132   }
    00133   else
    00134   {
    00135     // This is the second GoalRecognizer, so send drawing stuff now
    00136     DEBUG_DRAWING_FINISHED(imageProcessor_flagsAndGoals);
    00137     SEND_DEBUG_IMAGE(imageProcessorGoal2);
    00138   }
    00139 }
    00140
    00141 void VLCGoalRecognizer::analyzeGoal(Vector2<int> focus)
    00142 {
    00143   // Edge detector state machines
    00144   VLCGoalRecognizer::EdgeDetector detector[2] =
    00145   {
    00146     VLCGoalRecognizer::EdgeDetector(goalColor),
    00147     VLCGoalRecognizer::EdgeDetector(goalColor)
    00148   };
    00149
    00150   // Flags storing on which image border scanning has been done, to prevent repeated scans
    00151   // on the image borders within one analysis step or in different steps. The array is
    00152   // indexed by the enum imageBorderType.
    00153   bool imageBorderScanned[4] = {false, false, false, false};
    00154
    00155   // Objects to store detection result
    00156   VLCGoalRecognizer::GoalHypothesis result;
    00157
    00158
    00159   //-- Analyze the left goalpost
    00160   analyzeGoalpost(detector, -horizonInfo.horizon.direction, true, focus, result.goalpost[0], imageBorderScanned);
    00161
    00162
    00163   //-- Scan along the cross bar
    00164   scanCrossBar(detector, focus, result, imageBorderScanned);
    00165
    00166   // Filter obvious false hypothesis
    00167   result.width = static_cast<int>(result.crossBarEndpointH.x-result.goalpost[0].edgePointH.x);
    00168   if (result.goalpost[0].height+result.width < 15 || result.width <= 5)
    00169   {
    00170     // Prevent this blob to be analysed again
    00171     lockArea(result.crossBarEndpointH.x + 10, result.goalpost[0].bottomPointH.y + 10, result.goalpost[0].onGreen);
    00172
    00173     HORIZON_ALIGNED_RECTANGLE(horizonInfo,
    00174       result.goalpost[0].edgePointH.x, min(result.goalpost[0].topPointH.y, result.crossBarEndpointH.y),
    00175             result.crossBarEndpointH.x, result.goalpost[0].bottomPointH.y,
    00176       1, Drawings::ps_solid, Drawings::red);
    00177
    00178     return;
    00179   }
    00180
    00181
    00182   //-- Analyze the right goalpost
    00183   analyzeGoalpost(detector, horizonInfo.horizon.direction, false, focus, result.goalpost[1], imageBorderScanned);
    00184
    00185   // Possibly recalculate the height of the left goalpost (necessary if scanCrossBar
    00186   // scanned along the image border downwards)
    00187   if (result.crossBarEndpointH.y < result.goalpost[1].topPointH.y)
    00188   {
    00189     result.goalpost[1].topPoint = result.crossBarEndpoint;
    00190     result.goalpost[1].topPointH = result.crossBarEndpointH;
    00191     result.goalpost[1].height = static_cast<int>(result.goalpost[1].bottomPointH.y - result.goalpost[1].topPointH.y);
    00192   }
    00193
    00194   // Calculate the width and height
    00195   result.width = static_cast<int>(result.goalpost[1].edgePointH.x - result.goalpost[0].edgePointH.x);
    00196   result.height = max(result.goalpost[0].height, result.goalpost[1].height);
    00197
    00198   // Goal hypothesis is on the green field?
    00199   result.onGreen = result.goalpost[0].onGreen || result.goalpost[1].onGreen;
    00200
    00201   //-- Interpretation
    00202   // Prevent that this goal is discovered again
    00203   lockArea(result.goalpost[1].edgePoint.x + 5, max(result.goalpost[0].bottomPointH.y, result.goalpost[1].bottomPointH.y) + 5, result.goalpost[0].onGreen);
    00204
    00205   // Add goal to hypothsises
    00206   if (hypothesisCount < maxHypothesises)
    00207     hypothesis[hypothesisCount++] = result;
    00208 }
    00209
    00210 void VLCGoalRecognizer::analyzeGoalpost(VLCGoalRecognizer::EdgeDetector* detector,
    00211                                            const Vector2<double>& directionOfEdge,
    00212                                            bool leftGoalpost,
    00213                                            Vector2<int>& focus,
    00214                                            VLCGoalRecognizer::Goalpost& result,
    00215                                            bool* imageBorderScanned)
    00216 {
    00217   // Container to hold the detected points on the edge
    00218   EdgePointList edgePoints;
    00219
    00220   // Scan along the image border, because the normal scanning would violate the borders
    00221   bool borderScan = false;
    00222
    00223   // While scanning towards the edge, a huge target-coloured area was discovered. This
    00224   // usually happens, if a goal was only partially detected in a preceding goal analysis
    00225   // and hence not entirely locked and detected again. As a result, the same goal might be
    00226   // scanned again, which is a waste of processing time. Therefore I cancel the detection
    00227   // in these cases
    00228   bool detectionCanceled = false;
    00229
    00230   // Bresenham scanlines along and perpendicular to the goalpost
    00231   // The vertical scanline is slightly rotated inwards (i.e. away from the edge) to account
    00232   // for motion skew or a non-perfect horizon.
    00233   const double verticalScanSlope = 0.1;
    00234   BresenhamLineScan horizontalScan(directionOfEdge);
    00235   BresenhamLineScan verticalScan(horizonInfo.vertLine.direction - directionOfEdge*verticalScanSlope);
    00236
    00237   // Offset between the two parallel scanlines (horizontal and vertical scans are usually
    00238   // done on two scanlines for more noise tolerance)
    00239   Vector2<int> horizontalScanOffset(static_cast<int>(floor(-horizonInfo.vertLine.direction.x*2+0.5)),
    00240                                     static_cast<int>(floor(-horizonInfo.vertLine.direction.y*2+0.5)));
    00241   Vector2<int> verticalScanOffset(static_cast<int>(floor(-directionOfEdge.x+0.5)),
    00242                                   static_cast<int>(floor(-directionOfEdge.y+0.5)));
    00243
    00244   //-- Scan towards the edge
    00245   detector[0].clear(focus, false);
    00246   detector[1].clear(focus, true);
    00247   switch (detectEdgeTwice(detector, focus, horizontalScan, horizontalScanOffset, maxScanLengthToGoalpostEdge, edgePoints))
    00248   {
    00249   case imageBorder:
    00250     // Check whether the focus is on the image border (might not be there, because
    00251     // the last point in the target class is returned)
    00252     borderScan = nearImageBorder(focus, 1);
    00253
    00254     // Otherwise assume that there is an edge
    00255     // Even if the focus is not on the image border, it is still possible that there is
    00256     // no edge. The edge detector would have needed to investigates some more pixels (not
    00257     // available due to the image border) to decide this. But since assuming a (weak)
    00258     // edge is not harmful if its wrong, so we do this.
    00259     break;
    00260
    00261   case none:
    00262     // Scanning towards the edge did find nothing but goal-coloured pixels. In this case
    00263     // we cancel the scanning, because it is likely, that we are running into an area,
    00264     // that has been scanned during a preceding goal analysis
    00265     detectionCanceled = true;
    00266   }
    00267
    00268   // Save this point for upwards scanning
    00269   Vector2<int> firstEdgePoint = focus;
    00270
    00271   // Variable to store if the downwards scan started with a scan on the image border
    00272   VLCGoalRecognizer::ImageBorderSide firstEdgeImageBorder = noBorder;
    00273
    00274
    00275   //-- Sense along the edge downwards
    00276   if (borderScan)
    00277   {
    00278     // Scan along the image border and store on which image border the scan has been done.
    00279     // The scanned side will be scanned again in the other direction at the beginning of
    00280     // the upwards sensing step
    00281     firstEdgeImageBorder =
    00282       scanOnImageBorder(detector, focus, horizonInfo.vertLine.direction, 3, imageBorderScanned);
    00283     borderScan = false;
    00284
    00285     if (firstEdgeImageBorder != noBorder)
    00286     {
    00287       // Store image border point
    00288       edgePoints.add(focus, imageBorder);
    00289
    00290       // Set slightly inwards for the following scans
    00291       focus += verticalScanOffset*2;
    00292       horizonInfo.clipToImageBoundary(focus);
    00293     }
    00294   }
    00295
    00296   for (int i = 0; !detectionCanceled && i<25; ++i)
    00297   {
    00298     ASSERT(i!=23);  // This loop should be not be infinite without the limit i<12
    00299
    00300     if (borderScan)
    00301     {
    00302       // Slide alon the image border to the bottom
    00303       if (noBorder == scanOnImageBorder(detector, focus, horizonInfo.vertLine.direction, 3, imageBorderScanned))
    00304       {
    00305         // The border scan failed without finding any pixels in the target class, so
    00306         // assume the bottom point has been outrun or scanning along the image border
    00307         // the focus has run into doesn't make sense
    00308         break;
    00309       }
    00310       borderScan = false;
    00311
    00312       // Store image border point
    00313       edgePoints.add(focus, imageBorder);
    00314
    00315       // Set slightly inwards
    00316       focus += verticalScanOffset*2;
    00317       horizonInfo.clipToImageBoundary(focus);
    00318     }
    00319     else
    00320     {
    00321       // Scan downwards for 20 pixels (at maximum)
    00322       detector[0].clear(focus, true);
    00323       detector[1].clear(focus, true);
    00324       int scanned;
    00325       if (scanAlongLine(detector, focus, verticalScan, verticalScanOffset, 20, scanned) == imageBorder)
    00326       {
    00327         // Assuming that we either hit the bottom border, or the image border and the
    00328         // edge converge, continue scanning on the edge (not true if there is only a
    00329         // very thin strip of the goal in the image, but don't worry too much about
    00330         // this case)
    00331         borderScan = true;
    00332
    00333         // Store image border point
    00334         edgePoints.add(focus, imageBorder);
    00335         continue;
    00336       }
    00337
    00338       // Only scan horizontal again if vertical scan made it at least some pixels
    00339       if (scanned>4)
    00340       {
    00341         // Scan towards the edge
    00342         detector[0].clear(focus, true);
    00343         detector[1].clear(focus, true);
    00344         switch(detectEdgeTwice(detector, focus, horizontalScan, horizontalScanOffset, maxScanLengthToGoalpostEdge, edgePoints))
    00345         {
    00346         case imageBorder:
    00347           // Check whether the focus is on the image border
    00348           borderScan = nearImageBorder(focus, 1);
    00349           break;
    00350
    00351         case none:
    00352           // There is no edge, so cancel goalpost detection
    00353           detectionCanceled = true;
    00354         }
    00355       }
    00356
    00357       // Leave the loop if vertical scan didn't get far
    00358       if (scanned<8)
    00359         break;
    00360     }
    00361   } // Downwards sensing loop
    00362
    00363   // Store bottom point
    00364   result.bottomPoint = focus;
    00365   result.bottomPointH = horizonInfo.toHorizonAligned(focus);
    00366   //int bottomPointIndex = edgePoints.size() - 1;
    00367
    00368   // Store reference colour of the edge detector used on the inner scanline. It is needed
    00369   // for the detection of the free part of the goal (is done in the analysis at the end)
    00370   detector[1].getReferenceColor(result.color);
    00371
    00372   //-- Sense along edge upwards
    00373   // This is only necessary on the left goalpost. The crossbar analysis ends at the top
    00374   // point of the right goalpost.
    00375   focus = firstEdgePoint;
    00376
    00377   if (leftGoalpost && !detectionCanceled)
    00378   {
    00379     verticalScan = BresenhamLineScan(-horizonInfo.vertLine.direction - directionOfEdge*verticalScanSlope);
    00380     horizontalScanOffset = -horizontalScanOffset;
    00381     borderScan = false;
    00382
    00383     if (firstEdgeImageBorder != noBorder)
    00384     {
    00385       // The scan towards the edge ran into the image border and as a consequence, the
    00386       // sensing towards the bottom began with a scan along the border. It might be
    00387       // necessary to begin with a scan along the image border again, but this would
    00388       // usually be prohibited (no image side may be scanned twice). So we need to
    00389       // explicitly allow the scan on the same side.
    00390       imageBorderScanned[firstEdgeImageBorder] = false;
    00391
    00392       // Now scan along the image border
    00393       if (noBorder != scanOnImageBorder(detector, focus, -horizonInfo.vertLine.direction, 3, imageBorderScanned))
    00394       {
    00395         // Store image border point
    00396         edgePoints.add(focus, imageBorder);
    00397
    00398         // Set slightly inwards for the following scans
    00399         focus += verticalScanOffset*2;
    00400         horizonInfo.clipToImageBoundary(focus);
    00401       }
    00402
    00403       // Disallow the scanned image border side again (the call to scanOnImageBorder
    00404       // might not have scanned along any side or along a different side, both would be
    00405       // okay)
    00406       imageBorderScanned[firstEdgeImageBorder] = true;
    00407     }
    00408
    00409     for (int i = 0; !detectionCanceled && i<12; ++i)
    00410     {
    00411       //ASSERT(i!=10);    // This loop should be not be infinite without the limit i<12
    00412
    00413       if (borderScan)
    00414       {
    00415         // Slide alon the image border to the top
    00416         if (noBorder == scanOnImageBorder(detector, focus, horizonInfo.vertLine.direction, 3, imageBorderScanned))
    00417         {
    00418           // The border scan failed without finding any pixels in the target class,
    00419           // so assume the bottom point has been outrun or that the focus ran into
    00420           // an image border not worth to be followed (with resepect to our current
    00421           // desired scanning direction)
    00422           break;
    00423         }
    00424         borderScan = false;
    00425
    00426         // Store image border point
    00427         edgePoints.add(focus, imageBorder);
    00428
    00429         // Set slightly inwards for the following scanning
    00430         focus += verticalScanOffset*2;
    00431         horizonInfo.clipToImageBoundary(focus);
    00432       }
    00433       else
    00434       {
    00435         // Scan upwards for 20 pixels (at maximum)
    00436         detector[0].clear(focus, true);
    00437         detector[1].clear(focus, true);
    00438         int scanned;
    00439         if (scanAlongLine(detector, focus, verticalScan, verticalScanOffset, 20, scanned) == imageBorder)
    00440         {
    00441           // Store image border point
    00442           edgePoints.add(focus, imageBorder);
    00443
    00444           // Scan along the image border
    00445           borderScan = true;
    00446           continue;
    00447         }
    00448
    00449         // Only scan horizontal again if vertical scan made it at least some pixels
    00450         if (scanned>8)
    00451         {
    00452           // Scan towards the edge
    00453           detector[0].clear(focus, true);
    00454           detector[1].clear(focus, true);
    00455           switch(detectEdgeTwice(detector, focus, horizontalScan, horizontalScanOffset, maxScanLengthToGoalpostEdge, edgePoints))
    00456           {
    00457           case imageBorder:
    00458             // Scan along the border
    00459             borderScan = true;
    00460             break;
    00461
    00462           case none:
    00463             // Assume there is actually no goalpost
    00464             detectionCanceled = true;
    00465           }
    00466         }
    00467         else
    00468           break;
    00469       }
    00470
    00471     } // Upwards sensing loop
    00472   }
    00473
    00474   // If the top point of the goalpost still has to be found (for the following cross bar
    00475   // analysis), but there was no goapost, just scan straight upwards
    00476   if (detectionCanceled && leftGoalpost)
    00477   {
    00478     verticalScan = BresenhamLineScan(-horizonInfo.vertLine.direction);
    00479
    00480     // Scan straight upwards (unlike before, where the scanline is slightly slanted away
    00481     // from the edge)
    00482     detector[0].clear(focus, true);
    00483     detector[1].clear(focus, true);
    00484
    00485     int dummy;
    00486     if (scanAlongLine(detector, focus, verticalScan, verticalScanOffset, 1024, dummy) == imageBorder)
    00487     {
    00488       // Trouble with the image border again
    00489       detector[0].clear(focus, false);
    00490       detector[1].clear(focus, false);
    00491       scanOnImageBorder(detector, focus, horizonInfo.vertLine.direction, 3, imageBorderScanned);
    00492     }
    00493   }
    00494
    00495
    00496   // Store top point
    00497   result.topPoint = focus;
    00498   result.topPointH = horizonInfo.toHorizonAligned(focus);
    00499
    00500   //-- Analysis
    00501   result.nonExistant = detectionCanceled;
    00502
    00503   if (detectionCanceled)
    00504   {
    00505     // Set default values
    00506     result.height = 0;
    00507     result.visibleHeight = 0;
    00508     result.edgePoint = result.topPoint;
    00509     result.edgePointH = result.topPointH;
    00510     result.onGreen = false;
    00511   }
    00512   else
    00513   {
    00514     // Calculate the height of the goalpost
    00515     result.height = static_cast<int>(result.bottomPointH.y - result.topPointH.y);
    00516     //OUTPUT(idText,text,result.height[index]);
    00517
    00518     // Check if there is green below the goalpost
    00519     if (result.height > minGoalpostHeight)
    00520       result.onGreen = detectGreenBelowGoalpost(result.bottomPoint, directionOfEdge, result.height);
    00521     else
    00522       result.onGreen = false;
    00523
    00524
    00525     // TODO replace by proper one
    00526     result.visibleHeight = edgePoints.getCount(edge)+edgePoints.getCount(deviationWithoutEdge)>0 ? 1 : 0;
    00527
    00528     if (result.visibleHeight > 0)
    00529     {
    00530       // Do additional scans
    00531       // Check straightness
    00532       // Store slope
    00533
    00534       // Average all edge point as the goalpost edge
    00535       for (int i = 0; i<edgePoints.size(); ++i)
    00536       {
    00537         switch (edgePoints.getType(i))
    00538         {
    00539         case edge:
    00540         case deviationWithoutEdge:
    00541           result.edgePoint += edgePoints[i];
    00542         }
    00543       }
    00544       result.edgePoint /= edgePoints.getCount(edge)+edgePoints.getCount(deviationWithoutEdge);
    00545       result.edgePointH = horizonInfo.invRotation * Vector2<double>(static_cast<double>(result.edgePoint.x),
    00546                                     static_cast<double>(result.edgePoint.y));
    00547
    00548       // Store the number of weak and strong edges
    00549       result.strongEdgeCount = edgePoints.getCount(edge);
    00550       result.weakEdgeCount = edgePoints.getCount(deviationWithoutEdge);
    00551     }
    00552     else // visibleHeight == 0
    00553     {
    00554       ASSERT(edgePoints.getCount(imageBorder)>0);
    00555
    00556       // Use the leftmost/rightmost
    00557       double extremeHX = leftGoalpost ? 1024.0f : -1024.0f;
    00558
    00559       for (int i = 0; i<edgePoints.size(); ++i)
    00560       {
    00561         if (edgePoints.getType(i) == imageBorder)
    00562         {
    00563           // Transform into the horizon-aligned coordinate system
    00564           double HX = horizonInfo.invRotation.c[0].x * edgePoints[i].x
    00565               + horizonInfo.invRotation.c[1].x * edgePoints[i].y;
    00566           if ((HX < extremeHX) == leftGoalpost)
    00567           {
    00568             // Use this point as extreme point.
    00569             // NB: with goalpostVisibleHeight==0 it is obvious that albeit the name,
    00570             // the value is not an edge point
    00571             result.edgePoint = edgePoints[i];
    00572             result.edgePointH.x = HX;
    00573             extremeHX = HX;
    00574           }
    00575         }
    00576       }
    00577
    00578       // Store edge point as well in horizon-aligned coordinates
    00579       result.edgePointH.y = horizonInfo.invRotation.c[0].y * result.edgePoint.x
    00580                 + horizonInfo.invRotation.c[1].y * result.edgePoint.y;
    00581     }
    00582   }
    00583
    00584
    00585   // Possible improvements
    00586   // - straightness check with added scans to the edge -> need to store more edge point so
    00587   //   that scanlines with weak edges aren't scanned again & no scans towards the image
    00588   //   boundary
    00589 }
    00590
    00591 VLCGoalRecognizer::EdgeType VLCGoalRecognizer::detectEdgeTwice(VLCGoalRecognizer::EdgeDetector* detector,
    00592                                                                      Vector2<int>& focus,
    00593                                                                      BresenhamLineScan& scanLine,
    00594                                                                      const Vector2<int>& scanLineOffset,
    00595                                                                      int maxScanLength,
    00596                                                                      EdgePointList& edgePoints)
    00597 {
    00598   // Start point for the first scanline (scanning from focus is done second)
    00599   Vector2<int> focus0 = focus + scanLineOffset;
    00600
    00601   int distance[2];
    00602   bool onImageBorder[2];// = {false, false};
    00603
    00604   // Scan towards the edge
    00605   VLCGoalRecognizer::EdgeType type =
    00606     detectEdge(detector[0], focus0, scanLine, maxScanLength, distance[0]);
    00607   if (type == none)
    00608   {
    00609     // Signalize that there is an large goal-coloured area where the edge was expected
    00610     return none;
    00611   }
    00612   else
    00613   {
    00614     // Store the detected edge
    00615     edgePoints.add(focus0, type);
    00616     onImageBorder[0] = type == imageBorder;
    00617   }
    00618
    00619   // Scan towards the edge again
    00620   scanLine.init();
    00621   Vector2<int> focus1 = focus;
    00622   type = detectEdge(detector[1], focus1, scanLine, maxScanLength, distance[1]);
    00623   if (type == none)
    00624   {
    00625     // Signalize that there is an large goal-coloured area where the edge was expected
    00626     return none;
    00627   }
    00628   else
    00629   {
    00630     // Store the detected edge
    00631     edgePoints.add(focus1, type);
    00632     onImageBorder[1] = type == imageBorder;
    00633   }
    00634
    00635   //-- Use the point closer the start point as new focus
    00636   if (onImageBorder[0])
    00637   {
    00638     if (onImageBorder[1])
    00639     {
    00640       // Return the lastPointInClass nearer to the start point as new focus
    00641       if (distance[0]<distance[1])
    00642         focus = focus0;
    00643       else
    00644         focus = focus1;
    00645
    00646       // Both edges on the image border
    00647       return imageBorder;
    00648     }
    00649     else
    00650     {
    00651       // Return edge point in focus
    00652       focus = focus1;
    00653       return edge;
    00654     }
    00655   }
    00656   else
    00657   {
    00658     if (onImageBorder[1])
    00659     {
    00660       // Return edge point in focus
    00661       focus = focus0;
    00662       return edge;
    00663     }
    00664     else
    00665     {
    00666       // Use the edge point nearer to the start point as new focus
    00667       if (distance[0]<distance[1])
    00668         focus = focus0;
    00669       else
    00670         focus = focus1;
    00671
    00672       // Edge detected
    00673       return edge;
    00674     }
    00675   }
    00676 }
    00677
    00678 VLCGoalRecognizer::ImageBorderSide VLCGoalRecognizer::scanOnImageBorder(
    00679                                            VLCGoalRecognizer::EdgeDetector* detector,
    00680                                            Vector2<int>& focus,
    00681                                            const Vector2<double>& targetDirection,
    00682                                            int maxBorderDistance,
    00683                                            bool* scannedSides)
    00684 {
    00685   // Exclude the possibility, that the focus is considered near to more than two adjacent
    00686   // borders
    00687   ASSERT(maxBorderDistance < horizonInfo.maxImageCoordinates.x/2 &&
    00688          maxBorderDistance < horizonInfo.maxImageCoordinates.y/2);
    00689
    00690
    00691   // Check to which borders the focus is close
    00692   // If the focus is close to two borders, i.e. in a corner of the image, the following
    00693   // code also computes two vectors pointing away from that corner. These can be used to
    00694   // decide, whether a border scan makes any sense.
    00695   bool nearBorder[4] = {false, false, false, false};
    00696   int nearCount = 0;
    00697   bool scannedBefore[2];
    00698   Vector2<double> scanDirection[2];
    00699   double awayFromEdgeFactor = 1.0;
    00700
    00701   if (focus.y <= maxBorderDistance)
    00702   {
    00703     // Top
    00704     nearBorder[topBorder] = true;
    00705     scannedBefore[nearCount] = scannedSides[topBorder];
    00706     scanDirection[nearCount++].x = awayFromEdgeFactor;
    00707     awayFromEdgeFactor *= -1.0;
    00708
    00709   }
    00710   if (focus.x <= maxBorderDistance)
    00711   {
    00712     // Left
    00713     nearBorder[leftBorder] = true;
    00714     scannedBefore[nearCount] = scannedSides[leftBorder];
    00715     scanDirection[nearCount++].y = -awayFromEdgeFactor;
    00716     awayFromEdgeFactor *= -1.0;
    00717   }
    00718   if (horizonInfo.maxImageCoordinates.y-focus.y <= maxBorderDistance)
    00719   {
    00720     // Bottom
    00721     nearBorder[bottomBorder] = true;
    00722     scannedBefore[nearCount] = scannedSides[bottomBorder];
    00723     scanDirection[nearCount++].x = -awayFromEdgeFactor;
    00724     awayFromEdgeFactor *= -1.0;
    00725   }
    00726   if (horizonInfo.maxImageCoordinates.x-focus.x <= maxBorderDistance)
    00727   {
    00728     // Right
    00729     nearBorder[rightBorder] = true;
    00730     scannedBefore[nearCount] = scannedSides[rightBorder];
    00731     scanDirection[nearCount++].y = awayFromEdgeFactor;
    00732     //awayFromEdgeFactor *= -1.0;
    00733   }
    00734
    00735   // If the focus is near the top right corner, the vectors are pointing towards the edge,
    00736   // so reverse them
    00737   if (nearBorder[topBorder] && nearBorder[rightBorder])
    00738   {
    00739     scanDirection[0] = -scanDirection[0];
    00740     scanDirection[1] = -scanDirection[1];
    00741   }
    00742
    00743   // Compute the scan direction
    00744   switch (nearCount)
    00745   {
    00746   case 0:
    00747     // Focus is not close to any border
    00748     return noBorder;
    00749
    00750   case 1:
    00751   {
    00752     // Focus is close to one border, so allow scanning along this border in both
    00753     // directions, if the angle with the target direction is less than 60 degrees and if
    00754     // it hasn't been scanned before
    00755     double dotProduct = targetDirection*scanDirection[0];
    00756     if (scannedBefore[0] || fabs(dotProduct) < 0.5)
    00757       return noBorder;
    00758
    00759     // Reverse the scan direction if necessary
    00760     scanDirection[0] *= sgn(dotProduct);
    00761     break;
    00762   }
    00763   case 2:
    00764   default:
    00765     // Check if the target direction has an angle less than 60 degrees with one of the
    00766     // scan directions (that haven't been scanned before). Here the directions may not be
    00767     // reversed, because then scanning would run right into the corner
    00768     if (!scannedBefore[0] && targetDirection*scanDirection[0] > 0.5)
    00769     {
    00770       // Use this direction (scanDirection[0] will be used below, so nothing to do)
    00771     }
    00772     else if (!scannedBefore[1] && targetDirection*scanDirection[1] > 0.5)
    00773     {
    00774       scanDirection[0] = scanDirection[1];
    00775     }
    00776     else
    00777     {
    00778       // None of the potential directions meet the requirements
    00779       return noBorder;
    00780     }
    00781   }
    00782   ASSERT(scanDirection[0].x*scanDirection[0].y==0.0);
    00783
    00784   // Compute the start points for scanning
    00785   Vector2<int> newFocus = focus;
    00786   Vector2<int> scanOffset(0, 0);
    00787   VLCGoalRecognizer::ImageBorderSide scanSide;
    00788
    00789   if (nearBorder[topBorder] && scanDirection[0].y==0.0)
    00790   {
    00791     // Top
    00792     newFocus.y = 0;
    00793     scanOffset.y = 1;
    00794     scanSide = topBorder;
    00795   }
    00796   else if (nearBorder[leftBorder] && scanDirection[0].x==0.0)
    00797   {
    00798     // Left
    00799     newFocus.x = 0;
    00800     scanOffset.x = 1;
    00801     scanSide = leftBorder;
    00802   }
    00803   else if (nearBorder[bottomBorder] && scanDirection[0].y==0.0)
    00804   {
    00805     // Bottom
    00806     newFocus.y = horizonInfo.maxImageCoordinates.y;
    00807     scanOffset.y = -1;
    00808     scanSide = bottomBorder;
    00809   }
    00810   else
    00811   {
    00812     // Right
    00813     newFocus.x = horizonInfo.maxImageCoordinates.x;
    00814     scanOffset.x = -1;
    00815     scanSide = rightBorder;
    00816   }
    00817
    00818   /*DOT(imageProcessor_flagsAndGoals, newFocus.x, newFocus.y, Drawings::Color::orange, Drawings::Color::orange);
    00819   ARROW(imageProcessor_flagsAndGoals, newFocus.x, newFocus.y, newFocus.x+scanDirection[0].x*10, newFocus.y+scanDirection[0].y*10,   1, Drawings::PenStyle::ps_solid, Drawings::Color::orange);
    00820   */
    00821
    00822   // Scan along edge
    00823   BresenhamLineScan scanLine(scanDirection[0]);
    00824   int scannedCount;
    00825   scanAlongLine(detector, newFocus, scanLine, scanOffset, 1024, scannedCount);
    00826
    00827   // Check if any scanning was done at all
    00828   if (scannedCount < 0)
    00829   {
    00830     // Nothing scanned
    00831     return noBorder;
    00832   }
    00833   else
    00834   {
    00835     // Don't scan this side again
    00836     scannedSides[scanSide] = true;
    00837
    00838     // Return the new focus and on which side we did the scanning
    00839     LINE(imageProcessor_flagsAndGoals, focus.x, focus.y, newFocus.x, newFocus.y, 1, Drawings::ps_solid, Drawings::orange);
    00840     DOT(imageProcessor_flagsAndGoals, focus.x, focus.y, Drawings::white, Drawings::white);
    00841     DOT(imageProcessor_flagsAndGoals, newFocus.x, newFocus.y, Drawings::orange, Drawings::orange);
    00842     focus = newFocus;
    00843         return scanSide;
    00844   }
    00845 }
    00846
    00847
    00848
    00849 void VLCGoalRecognizer::scanCrossBar(VLCGoalRecognizer::EdgeDetector* detector,
    00850                                         Vector2<int>& focus,
    00851                                         VLCGoalRecognizer::GoalHypothesis& result,
    00852                                         bool* imageBorderScanned)
    00853 {
    00854   DOT(imageProcessor_flagsAndGoals, focus.x, focus.y, Drawings::orange, Drawings::orange);
    00855
    00856   // Check if start point is on the image border
    00857   if (nearImageBorder(focus, 3))
    00858   {
    00859     // Scan along the edge
    00860     scanOnImageBorder(detector, focus, horizonInfo.horizon.direction, 3, imageBorderScanned);
    00861   }
    00862   bool borderScan = false;
    00863   bool nonBorderScanDone = false;
    00864
    00865   DOT(imageProcessor_flagsAndGoals, focus.x, focus.y, pink, pink);
    00866
    00867
    00868   // Bresenham lines for scans towards the edge
    00869   BresenhamLineScan upwardsScan(-horizonInfo.vertLine.direction);
    00870
    00871   //
    00872   bool calculateScanLine = false;
    00873
    00874   // Last edge point (for extrapolating the optimal slope)
    00875   Vector2<int> lastEdgePoint = focus;
    00876
    00877
    00878   //-- Start with scanning exactly horizontally
    00879   Vector2<double> scanDirection = horizonInfo.horizon.direction;
    00880   BresenhamLineScan horizontalScan(scanDirection);
    00881   Vector2<int> horizontalScanOffset(static_cast<int>(floor(horizonInfo.vertLine.direction.x+0.5)),
    00882                                     static_cast<int>(floor(horizonInfo.vertLine.direction.y+0.5)));
    00883   focus += horizontalScanOffset;
    00884   detector[0].clear(focus, true);
    00885   detector[1].clear(focus, true);
    00886   int scanned;
    00887
    00888   switch (scanAlongLine(detector, focus, horizontalScan, horizontalScanOffset, 12, scanned))
    00889   {
    00890   case VLCGoalRecognizer::imageBorder:
    00891     // Scan along the border
    00892     borderScan = true;
    00893
    00894   case VLCGoalRecognizer::edge:
    00895   case VLCGoalRecognizer::deviationWithoutEdge:
    00896     if (scanned < 6)
    00897     {
    00898       // Scan again with a lower starting point
    00899       focus = lastEdgePoint + Vector2<int>(static_cast<int>(floor(horizonInfo.vertLine.direction.x*5 + horizonInfo.horizon.direction.x*2 + 0.5)),
    00900                                            static_cast<int>(floor(horizonInfo.vertLine.direction.y*5 + horizonInfo.horizon.direction.y*2 + 0.5)));
    00901       if (scanAlongLine(detector, focus, horizontalScan, horizontalScanOffset, 12, scanned) == imageBorder)
    00902       {
    00903         // Image border reached
    00904         borderScan = true;
    00905       }
    00906     }
    00907
    00908     // The horiziontal scanline was not ok, so recalculate it
    00909     calculateScanLine = true;
    00910   }
    00911
    00912
    00913   //-- Sense along the crossbar
    00914   for (int i = 0; !borderScan && i<25; ++i)
    00915   {
    00916     //ASSERT(i!=23);    // This loop should not be infinite without the limit i<25
    00917
    00918     // The horizontal scan did not reach the border, hence there is at least some of the
    00919     // crossbar visible
    00920     nonBorderScanDone = true;
    00921
    00922         // Get in touch with the edge again
    00923     detector[1].clear();
    00924     int scanTillEdgeLength;
    00925     if (detectEdge(detector[1], focus, upwardsScan, 10, scanTillEdgeLength) == imageBorder)
    00926     {
    00927       // Check, wether the focus is near the edge
    00928       if (borderScan = nearImageBorder(focus, 1)) // Single = is correct!
    00929         break;
    00930
    00931       // If the focus is not near the image border, we assume that there was no edge
    00932       // even if there might have been one. But if there actually is one, the subsequent
    00933       // step will run into it anyway.
    00934     }
    00935
    00936     // Recalculate scanning direction (if necessary)
    00937     if (calculateScanLine || scanTillEdgeLength>2)
    00938     {
    00939       // If the goal is seen from nearby, the crossbar may have different slopes and
    00940       // may appear curved to the left (in sanning direction from the left to the right
    00941       // goalpost). Therefore the following calculates the slope from the first two
    00942       // edge points (first iteration) or recalculates it, if the slope changed.
    00943       horizontalScan = BresenhamLineScan(lastEdgePoint, focus);
    00944       scanDirection.x = static_cast<double>(focus.x-lastEdgePoint.x);
    00945       scanDirection.y = static_cast<double>(focus.y-lastEdgePoint.y);
    00946       scanDirection.normalize();
    00947       Vector2<int> horizontalScanOffset(static_cast<int>(floor(-scanDirection.y+0.5)),
    00948                                         static_cast<int>(floor( scanDirection.x+0.5)));
    00949
    00950       // Don't allow scanDirections with angles greater than 60 degrees to the horizon,
    00951       // because they are unlikely and may lead to infinite loops (since the direction
    00952       // for scans towards the edge is not adapted)
    00953       if (horizonInfo.horizon.direction*scanDirection < 0.5)
    00954         break;
    00955     }
    00956
    00957     // Scan to the right
    00958     focus += horizontalScanOffset;
    00959     detector[0].clear();
    00960     detector[1].clear(); // WErte??
    00961     switch (scanAlongLine(detector, focus, horizontalScan, horizontalScanOffset, 12, scanned))
    00962     {
    00963     case VLCGoalRecognizer::imageBorder:
    00964       // Cross bar is outside the image, so quit the loop
    00965       borderScan = true;
    00966       break;
    00967
    00968     case VLCGoalRecognizer::edge:
    00969     case VLCGoalRecognizer::deviationWithoutEdge:
    00970       // The cross bar should appear straight or like a convex curve (with the sides of
    00971       // the goal; often if the dog is close to the goal). With the intitial scan
    00972       // direction calculation and the recalculations, the scan should not run into the
    00973       // top edge at this point. Hence assume the right goalpost.
    00974
    00975       // ScanAlongLine is keen to get through and returns the furthest point it could
    00976       // consider to be inside the goal. However here this might just have outrun the
    00977       // right goalpost, so we set the focus a bit back. The folowing goalpost analysis
    00978       // begins with a scan towards the edge anyway.
    00979       focus = focus - Vector2<int>(static_cast<int>(scanDirection.x*4),
    00980                                    static_cast<int>(scanDirection.y*4));
    00981       result.crossBarEndpoint = focus;
    00982       result.crossBarEndpointH = horizonInfo.toHorizonAligned(focus);
    00983       return;
    00984     }
    00985   }
    00986
    00987   // The current point is potentially useful to determine the height of the right goalpost
    00988   // (if its top endpoint is outside the image). Additionally it is used to filter false
    00989   // goal hypothesises
    00990   result.crossBarEndpoint = focus;
    00991   result.crossBarEndpointH = horizonInfo.toHorizonAligned(focus);
    00992
    00993   // Only scan along the edge again if there was any of the cross bar visible (and hence we
    00994   // haven't just done that)
    00995   if (nonBorderScanDone && borderScan)
    00996     scanOnImageBorder(detector, focus, horizonInfo.horizon.direction, 3, imageBorderScanned);
    00997 }
    00998
    00999 VLCGoalRecognizer::EdgeType VLCGoalRecognizer::detectEdge(VLCGoalRecognizer::EdgeDetector& detector,
    01000                                                                 Vector2<int>& focus,
    01001                                                                 BresenhamLineScan& scanLine,
    01002                                                                 int maxScanLength,
    01003                                                                 int& pixelsUntilEdge)
    01004 {
    01005   // Reset Bresenham scanline
    01006   scanLine.init();
    01007
    01008   // Feed detector with points on the Bresenham scanLine
    01009   int i;
    01010   for (i = 0; i<maxScanLength && focus.x>=0 && focus.y>=0 && focus.x<=horizonInfo.maxImageCoordinates.x && focus.y<=horizonInfo.maxImageCoordinates.y; ++i)
    01011   {
    01012     STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus.x, focus.y, BLACK);
    01013
    01014     // Colour and colour class of focused pixel
    01015     unsigned char color[3] =
    01016     {
    01017       colorCorrector.correct(focus.x, focus.y, 0, interfaces.image.image[focus.y][0][focus.x]),
    01018       colorCorrector.correct(focus.x, focus.y, 1, interfaces.image.image[focus.y][1][focus.x]),
    01019       colorCorrector.correct(focus.x, focus.y, 2, interfaces.image.image[focus.y][2][focus.x])
    01020     };
    01021     colorClass colorCls = COLOR_CLASS(color[0], color[1], color[2], (*bestColorTable));
    01022
    01023     switch (detector.inspectPixel(focus, i, colorCls, Vector3<int>(static_cast<int>(color[0]), static_cast<int>(color[1]), static_cast<int>(color[2]))))
    01024     {
    01025     case edge:
    01026       DOT(imageProcessor_flagsAndGoals, detector.getLastPointInClass().x, detector.getLastPointInClass().y, Drawings::black, Drawings::green);
    01027       STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus.x, focus.y, GREEN);
    01028
    01029       // Return edge via focus argument
    01030       detector.getLastPointInClass(focus, pixelsUntilEdge);
    01031       return edge;
    01032       break;
    01033
    01034     case deviationWithoutEdge:
    01035       DOT(imageProcessor_flagsAndGoals, detector.getLastPointInClass().x, detector.getLastPointInClass().y, Drawings::black, Drawings::white);
    01036       STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus.x, focus.y, RED);
    01037
    01038       // Return last point in class via focus argument
    01039       detector.getLastPointInClass(focus, pixelsUntilEdge);
    01040       return deviationWithoutEdge;
    01041       break;
    01042     }
    01043
    01044     // Get the next point on the line
    01045     scanLine.getNext(focus);
    01046   }
    01047
    01048   if (i==maxScanLength)
    01049   {
    01050     // Signalize no edge found withing scan length limit
    01051     DOT(imageProcessor_flagsAndGoals, detector.getLastPointInClass().x, detector.getLastPointInClass().y, Drawings::black, Drawings::red);
    01052     return none;
    01053   }
    01054   else
    01055   {
    01056     // Return last point in class via focus argument
    01057     detector.getLastPointInClass(focus, pixelsUntilEdge);
    01058     return imageBorder;
    01059   }
    01060 }
    01061
    01062 VLCGoalRecognizer::EdgeType VLCGoalRecognizer::scanAlongLine(
    01063                                     VLCGoalRecognizer::EdgeDetector* detector,
    01064                                     Vector2<int>& focus,
    01065                                     BresenhamLineScan& scanLine,
    01066                                     const Vector2<int>& scanLineOffset,
    01067                                     int maximumScan,
    01068                                     int& pixelsScanned)
    01069 {
    01070   // Scanning is done on two scanlines at once and only if both EdgeDetectors report
    01071   // edges/deviations stop scanning
    01072   bool edge[2];
    01073   bool withinBounds[2];
    01074
    01075   // Temporary variables
    01076   unsigned char color[3];
    01077   colorClass colorCls;
    01078   Vector2<int> focus2;
    01079
    01080   // Feed the detectors with points on the Bresenham scanLine
    01081   for (int i = 0; i <= maximumScan; ++i)
    01082   {
    01083     // First scanline
    01084     if (withinBounds[0] = focus.x>=0 && focus.y>=0 && focus.x<=horizonInfo.maxImageCoordinates.x && focus.y<=horizonInfo.maxImageCoordinates.y)
    01085     {
    01086       // Colour and colour class of focused pixel
    01087       color[0] = colorCorrector.correct(focus.x, focus.y, 0, interfaces.image.image[focus.y][0][focus.x]);
    01088       color[1] = colorCorrector.correct(focus.x, focus.y, 1, interfaces.image.image[focus.y][1][focus.x]);
    01089       color[2] = colorCorrector.correct(focus.x, focus.y, 2, interfaces.image.image[focus.y][2][focus.x]);
    01090       colorCls = COLOR_CLASS(color[0], color[1], color[2], (*bestColorTable));
    01091
    01092       // Feed EdgeDetector
    01093       edge[0] = none != detector[0].inspectPixel(focus, i, colorCls, Vector3<int>(static_cast<int>(color[0]), static_cast<int>(color[1]), static_cast<int>(color[2])));
    01094
    01095       if (edge[0])
    01096       {
    01097         STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus.x, focus.y, WHITE);
    01098       }
    01099       else
    01100       {
    01101         STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus.x, focus.y, BLUE);
    01102       }
    01103     }
    01104
    01105     // Second scanline
    01106     focus2 = focus + scanLineOffset;
    01107     if (withinBounds[1] = focus2.x>=0 && focus2.y>=0 && focus2.x<=horizonInfo.maxImageCoordinates.x && focus2.y<=horizonInfo.maxImageCoordinates.y)
    01108     {
    01109       // Colour and colour class of focused pixel
    01110       color[0] = colorCorrector.correct(focus2.x, focus2.y, 0, interfaces.image.image[focus2.y][0][focus2.x]);
    01111       color[1] = colorCorrector.correct(focus2.x, focus2.y, 1, interfaces.image.image[focus2.y][1][focus2.x]);
    01112       color[2] = colorCorrector.correct(focus2.x, focus2.y, 2, interfaces.image.image[focus2.y][2][focus2.x]);
    01113       colorCls = COLOR_CLASS(color[0], color[1], color[2], (*bestColorTable));
    01114
    01115       // Feed EdgeDetector
    01116       edge[1] = none != detector[1].inspectPixel(focus2, i, colorCls, Vector3<int>(static_cast<int>(color[0]), static_cast<int>(color[1]), static_cast<int>(color[2])));
    01117
    01118       if (edge[1])
    01119       {
    01120         STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus2.x, focus2.y, WHITE);
    01121       }
    01122       else
    01123       {
    01124         STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus2.x, focus2.y, BLUE);}
    01125     }
    01126
    01127     // Merge results of both scanlines
    01128     if (withinBounds[0] && withinBounds[1])
    01129     {
    01130       if (edge[0] && edge[1])
    01131       {
    01132         // Return point before edge/deviation via focus argument
    01133         int scannedFurther = detector[0].getLastIndexInClass()>detector[1].getLastIndexInClass() ? 0 : 1;
    01134         detector[scannedFurther].getLastPointInClass(focus, pixelsScanned);
    01135
    01136         // Signalize edge/deviation
    01137         // NB: This method does not distinguish between strong and weak edges!
    01138         return VLCGoalRecognizer::edge;
    01139       }
    01140     }
    01141     else
    01142     {
    01143       // Return the valid point via focus argument and signalize image border
    01144       int scannedFurther = detector[0].getLastIndexInClass()>detector[1].getLastIndexInClass() ? 0 : 1;
    01145       detector[scannedFurther].getLastPointInClass(focus, pixelsScanned);
    01146       return VLCGoalRecognizer::imageBorder;
    01147     }
    01148
    01149     // Get the next point on the line
    01150     scanLine.getNext(focus);
    01151   }
    01152
    01153   // Maximum scanning length reached. In this case there might actually be an edge at the
    01154   // focus which has not yet been detected, hence set the focus to the last point in the
    01155   // target class on the scanline that got further (same as all other return cases).
    01156   int scannedFurther = detector[0].getLastIndexInClass()>detector[1].getLastIndexInClass() ? 0 : 1;
    01157   detector[scannedFurther].getLastPointInClass(focus, pixelsScanned);
    01158   return VLCGoalRecognizer::none;
    01159 }
    01160
    01161 bool VLCGoalRecognizer::detectGreenBelowGoalpost(Vector2<int> focus, const Vector2<double>& directionOfEdge, int goalpostHeight)
    01162 {
    01163   // Calculate the number of horizontal lines to be scanned
    01164   int scanLineCount = 1 + max(2, goalpostHeight/16);
    01165
    01166   // Bresenham line for the scans
    01167   // Scanning is done against the directionOfEdge, because this is likely to be the side
    01168   // of the image border. This way we can skip pixels until the focus is back inside the
    01169   // image.
    01170   BresenhamLineScan scanLine(-directionOfEdge);
    01171   focus.x += static_cast<int>(directionOfEdge.x * 5); // no need to round, doesn't need to be exact
    01172   focus.y += static_cast<int>(directionOfEdge.y * 5);
    01173
    01174   // Vertical between the line segments
    01175   Vector2<int> verticalOffset(static_cast<int>(floor(horizonInfo.vertLine.direction.x*2+0.5)),
    01176                               static_cast<int>(floor(horizonInfo.vertLine.direction.y*2+0.5)));
    01177
    01178   // Check short horizontal line segments for green pixels
    01179   int greenCount = 0, pixelCount = 0;
    01180   colorClass color;
    01181   for (int i = 0; i < scanLineCount; ++i)
    01182   {
    01183     // Move focus downwards
    01184     focus += verticalOffset;
    01185
    01186     // Scan horizontally and count green pixels
    01187     greenCount = pixelCount = 0;
    01188     scanLine.init();
    01189     Vector2<int> focus2 = focus;
    01190     for (int j = 0; pixelCount < 5; ++j)
    01191     {
    01192       // Next scanline pixel
    01193       scanLine.getNext(focus2);
    01194
    01195       // Check if inside the image
    01196       if (focus2.x >= 0 && focus2.y >= 0 && focus2.x <= horizonInfo.maxImageCoordinates.x && focus2.y <= horizonInfo.maxImageCoordinates.y)
    01197       {
    01198         ++pixelCount;
    01199
    01200         // Check colour class
    01201         color = CORRECTED_COLOR_CLASS(focus2.x, focus2.y, interfaces.image.image[focus2.y][0][focus2.x], interfaces.image.image[focus2.y][1][focus2.x], interfaces.image.image[focus2.y][2][focus2.x], (*bestColorTable), colorCorrector);
    01202         if (color == green)
    01203         {
    01204           ++greenCount;
    01205           STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus2.x, focus2.y, YELLOW);
    01206         }
    01207         else
    01208         {
    01209           STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus2.x, focus2.y, GRAY);
    01210         }
    01211       }
    01212       else
    01213       {
    01214         // Skip this scanline if the image boundary wasn't reached within 8 pixels
    01215         if (j>=8)
    01216           break;
    01217       }
    01218     }
    01219
    01220     // Check at least 3 (out of the scanned 5) pixels where green
    01221     if (greenCount >= 3)
    01222       return true;
    01223   }
    01224
    01225   // None of the scanline where sufficiently green so return false
    01226   return false;
    01227 }
    01228
    01229 void VLCGoalRecognizer::lockArea(double x, double y, bool onGreen)
    01230 {
    01231   // If the goal hypothesis is stading on the green floor, don't scan below it at all
    01232   if (onGreen)
    01233     y = 1024.0;
    01234
    01235   // Pop locks that are well included in the new one. This way, a goal that is detected
    01236   // twice including the right goalpost (happens if the striker is close to the goal and
    01237   // the right goalpost scan fails to get to the bottom) results in two locks and hence
    01238   // a lock to the bottom (2nd lock is always to the bottom)
    01239   while (lockAreaCount>0)
    01240   {
    01241     if (x >= lockAreaStack[lockAreaCount-1].x && y >= lockAreaStack[lockAreaCount-1].y+8)
    01242       // Pop lock
    01243       --lockAreaCount;
    01244     else
    01245       break;
    01246   }
    01247
    01248   // Add lock to the stack
    01249   if (lockAreaCount == lockAreaStackSize-1)
    01250   {
    01251     // Make the last lock go all the way to the bottom (and don't increase the counter)
    01252     lockAreaStack[lockAreaCount].x = 1024.0;
    01253     lockAreaStack[lockAreaCount].y = y;
    01254   }
    01255   else if (lockAreaCount < lockAreaStackSize-1)
    01256   {
    01257     // Add to the stack the usual way
    01258     lockAreaStack[lockAreaCount].x = x;
    01259     lockAreaStack[lockAreaCount].y = y;
    01260     ++lockAreaCount;
    01261   }
    01262   //else doesn't happen
    01263 }
    01264
    01265 void VLCGoalRecognizer::calculateLockedPixels(const Vector2<int>& scanLineStart)
    01266 {
    01267   // Convert to horizon-aligned coordinates
    01268   Vector2<double> startH = horizonInfo.toHorizonAligned(scanLineStart);
    01269
    01270   // Pop locks that are no longer relevant (because there will be no more pixel inspection
    01271   // in these areas)
    01272   while (lockAreaCount>0)
    01273   {
    01274     if (lockAreaStack[lockAreaCount-1].x < startH.x)
    01275       // Pop lock
    01276       --lockAreaCount;
    01277     else
    01278       break;
    01279   }
    01280
    01281   // Compute locked pixels
    01282   lockedPixels = 0;
    01283   if (lockAreaCount>0)
    01284   {
    01285     // Calculate the number of pixels that will be inspected until the locked area is
    01286     // left. This consists of the following steps (merged to be efficient)
    01287     //  - calculate the vector from the scanline start point to the point where the
    01288     //    scanline leaves the locked area (in the horizon-aligned coordinate system);
    01289     //    this vector has an x-value 0
    01290     //  - if it points upwards, don't lock any pixels
    01291     //  - transform it to image coordinates
    01292     //  - calculate, how many pixels will be generated by BresenhamLineScan for this
    01293     //    line segment
    01294     double segmentLength = lockAreaStack[lockAreaCount-1].y - startH.y;
    01295     if (segmentLength <= 0)
    01296       return;
    01297     Vector2<int> segment = horizonInfo.toImageCoordinates(0.0, segmentLength);
    01298     lockedPixels = max(abs(segment.x), abs(segment.y));
    01299   }
    01300 }
    01301
    01302 void VLCGoalRecognizer::recalculateLockedPixels(const Vector2<int>& currentPoint)
    01303 {
    01304   // Compute locked pixels (like in calculateLockedPixels)
    01305   lockedPixels = 0;
    01306   if (lockAreaCount>0)
    01307   {
    01308     double segmentLength = lockAreaStack[lockAreaCount-1].y - horizonInfo.horizonAlignedYOf(currentPoint);
    01309     if (segmentLength <= 0)
    01310       return;
    01311     Vector2<int> segment = horizonInfo.toImageCoordinates(0.0, segmentLength);
    01312     lockedPixels = max(abs(segment.x), abs(segment.y));
    01313   }
    01314 }
    01315
    01316 void VLCGoalRecognizer::mergeFragments(bool* deleted)
    01317 {
    01318   //TODO: make efficient (with pointers?)
    01319   for (int i=0; i<maxHypothesises; ++i)
    01320     deleted[i] = false;
    01321
    01322   // Loop through all pairs of goals
    01323   for (int i=0; i<hypothesisCount; ++i)
    01324     if (!deleted[i])
    01325     {
    01326       for (int j=i+1; j<hypothesisCount; ++j)
    01327         if (!deleted[j])
    01328         {
    01329           // Preconditions for a merge
    01330           int volumeLeft  = hypothesis[i].height*hypothesis[i].width;
    01331           int volumeRight = hypothesis[j].height*hypothesis[j].width;
    01332           const int& heightLeft  = hypothesis[i].height;
    01333           const int& heightRight = hypothesis[j].height;
    01334           int verticalOffset = abs(static_cast<int>(hypothesis[j].goalpost[0].topPointH.y
    01335                                                   - hypothesis[i].goalpost[1].topPointH.y));
    01336           int horizontalGap = static_cast<int>(hypothesis[j].goalpost[0].edgePointH.x
    01337                                              - hypothesis[i].goalpost[1].edgePointH.x);
    01338
    01339           // Precondition: similar size
    01340           bool sizeSimilar = volumeLeft*3>volumeRight && volumeRight*3>volumeLeft
    01341                           || heightLeft*3>heightRight*2 && heightRight*3>heightLeft*2;
    01342
    01343           if (sizeSimilar)
    01344           {
    01345             // Check the vertical offset between the corners
    01346             bool verticalDifferenceAcceptable = verticalOffset < horizontalGap
    01347                                              || verticalOffset < min(hypothesis[i].height, hypothesis[j].height);
    01348
    01349             // Check that the horizontal gap is less than a third of the new
    01350             // width.
    01351             int newWidth = static_cast<int>(hypothesis[j].goalpost[1].edgePointH.x
    01352                                           - hypothesis[i].goalpost[0].edgePointH.x);
    01353
    01354             // Reduce the gap if the the inner goalposts were not found at all.
    01355             if (hypothesis[j].goalpost[0].nonExistant)
    01356               horizontalGap -= 8;
    01357             if (hypothesis[i].goalpost[1].nonExistant)
    01358               horizontalGap -= 8;
    01359
    01360             if (verticalDifferenceAcceptable
    01361               && horizontalGap*3<newWidth)
    01362             {
    01363               // Merge the two hypothesises
    01364               hypothesis[i].width = newWidth;
    01365               hypothesis[i].height = max(hypothesis[i].height, hypothesis[j].height);
    01366               hypothesis[i].onGreen = hypothesis[i].onGreen || hypothesis[j].onGreen;
    01367               hypothesis[i].goalpost[1] = hypothesis[j].goalpost[1];
    01368               deleted[j] = true;
    01369             }
    01370           }
    01371         }
    01372     }
    01373 }
    01374
    01375
    01376 void VLCGoalRecognizer::interpretResults(VLCGoalRecognizer::EdgeDetector* detector, bool* deleted)
    01377 {
    01378   // Variable for selected hypothesis
    01379   int selected = -1;
    01380
    01381   // Variables for interpretation result
    01382   bool visibleGoalpost[2];
    01383   VLCGoalRecognizer::FreeSide freeSideOfGoal = noFreeSide;
    01384   Vector2<int> edgeOfFreePart;
    01385
    01386   for (int i = 0; i<hypothesisCount; i++)
    01387   if (!deleted[i])
    01388   {
    01389     // Interpret goalposts
    01390     bool possibleGoalPart[2];
    01391     bool strongGoalpost[2];
    01392
    01393     for (int side=0; side<2; ++side)
    01394     {
    01395       possibleGoalPart[side] = hypothesis[i].goalpost[side].nonExistant == false
    01396                             && hypothesis[i].goalpost[side].height > minGoalpostHeight
    01397                             && hypothesis[i].goalpost[side].height*4 > hypothesis[i].width
    01398                             && hypothesis[i].goalpost[side].bottomPointH.y - horizonInfo.distanceTopLeftCorner > 0
    01399                             && hypothesis[i].goalpost[side].weakEdgeCount <= hypothesis[i].goalpost[side].strongEdgeCount/8 + 1;
    01400       visibleGoalpost[side] = possibleGoalPart[side]
    01401                             && hypothesis[i].goalpost[side].visibleHeight > 0
    01402                             && hypothesis[i].goalpost[side].height*2 > hypothesis[i].height;
    01403                             // hypothesis[i].goalpost[side].visibleHeight*2 > height
    01404                             // hypothesis[i].goalpost[side].curvature < ??;
    01405       strongGoalpost[side] = visibleGoalpost[side]
    01406                             && hypothesis[i].goalpost[side].onGreen;
    01407     }
    01408
    01409     // Interpret goal
    01410     bool minimalWidth = hypothesis[i].width > 10;
    01411     bool reasonableWidth = hypothesis[i].width > 20;
    01412     bool largeWidth = hypothesis[i].width > 100;
    01413     bool reasonableHeight = hypothesis[i].height > 30;
    01414     bool largeHeight = hypothesis[i].height > 50;
    01415     //OUTPUT(idText,text, "goal interpretation " << (strongGoalpost[0]?(largeHeight?"I":"|"):(hypothesis[i].goalpost[0].nonExistant?"x":"'")) << (minimalWidth?"-":"") << (reasonableWidth?"-":"") << (largeWidth?"-":"") << (strongGoalpost[1]?(largeHeight?"I":"|"):(hypothesis[i].goalpost[1].nonExistant?"x":"'")));
    01416
    01417     // Minimal prerequisites for a goal
    01418     if ((  (strongGoalpost[0] || strongGoalpost[1])
    01419            && (reasonableWidth || (reasonableHeight && minimalWidth)))
    01420         || (possibleGoalPart[0] || possibleGoalPart[1])
    01421            && largeWidth && largeHeight)
    01422     {
    01423       // Select this blob
    01424       selected = i;
    01425
    01426       // Determine free part of goal
    01427       int freeWidth;
    01428       freeSideOfGoal = detectFreePartOfGoal(detector, hypothesis[selected], hypothesis[selected].height, freeWidth, edgeOfFreePart);
    01429
    01430       // Decide whether to the detected information
    01431       if (visibleGoalpost[0] && visibleGoalpost[1])
    01432       {
    01433         // Free width must be at least a third of the goal
    01434         if (freeWidth*3 < hypothesis[i].width)
    01435           freeSideOfGoal = noFreeSide;
    01436       }
    01437       else
    01438       {
    01439         // Free width must be at least half of the height
    01440         if (freeWidth*2 < hypothesis[i].height)
    01441           freeSideOfGoal = noFreeSide;
    01442       }
    01443
    01444       break;
    01445     }
    01446   }
    01447
    01448   // Draw
    01449   for (int i = 0; i<hypothesisCount; i++)
    01450   if (!deleted[i])
    01451   {
    01452     HORIZON_ALIGNED_RECTANGLE(horizonInfo,
    01453     hypothesis[i].goalpost[0].edgePointH.x, min(min(hypothesis[i].goalpost[0].topPointH.y, hypothesis[i].goalpost[1].topPointH.y), hypothesis[i].crossBarEndpointH.y),
    01454     hypothesis[i].goalpost[1].edgePointH.x, max(hypothesis[i].goalpost[0].bottomPointH.y, hypothesis[i].goalpost[1].bottomPointH.y),
    01455     1, Drawings::ps_solid, i==selected ? Drawings::green : Drawings::red);
    01456   }
    01457
    01458   // No goal percept?
    01459   if (-1 == selected)
    01460     return;
    01461
    01462
    01463   // Calculate angles to sides
    01464   VLCGoalRecognizer::GoalHypothesis& h = hypothesis[selected];
    01465   Vector2<double> top, bottom, left, right;
    01466
    01467   Geometry::calculateAnglesForPoint((h.goalpost[0].topPoint+h.goalpost[1].topPoint)/2, interfaces.cameraMatrix, horizonInfo.previousCameraMatrix, interfaces.image.cameraInfo, top);
    01468   if (h.goalpost[0].bottomPointH.y > h.goalpost[1].bottomPointH.y)
    01469     Geometry::calculateAnglesForPoint(h.goalpost[0].bottomPoint, interfaces.cameraMatrix, horizonInfo.previousCameraMatrix, interfaces.image.cameraInfo, bottom);
    01470   else
    01471     Geometry::calculateAnglesForPoint(h.goalpost[1].bottomPoint, interfaces.cameraMatrix, horizonInfo.previousCameraMatrix, interfaces.image.cameraInfo, bottom);
    01472   Geometry::calculateAnglesForPoint(h.goalpost[0].edgePoint, interfaces.cameraMatrix, horizonInfo.previousCameraMatrix, interfaces.image.cameraInfo, left);
    01473   Geometry::calculateAnglesForPoint(h.goalpost[1].edgePoint, interfaces.cameraMatrix, horizonInfo.previousCameraMatrix, interfaces.image.cameraInfo, right);
    01474
    01475   ConditionalBoundary boundary;
    01476   boundary.addY(top.y, true);   // TODO: check if the true matters matters
    01477   boundary.addY(bottom.y, true);
    01478   boundary.addX(left.x, !visibleGoalpost[0]);
    01479   boundary.addX(right.x, !visibleGoalpost[1]);
    01480
    01481   // Calculate goal corners in the image
    01482   double topHY = (h.goalpost[0].topPointH.y + h.goalpost[1].topPointH.y)/2;
    01483   double bottomHY = max(h.goalpost[0].bottomPointH.y, h.goalpost[1].bottomPointH.y)/2;
    01484   Vector2<int> topLeftCorner = horizonInfo.toImageCoordinates(h.goalpost[0].edgePointH.x, topHY);
    01485   Vector2<int> bottomLeftCorner = horizonInfo.toImageCoordinates(h.goalpost[0].edgePointH.x, bottomHY);
    01486   Vector2<int> bottomRightCorner = horizonInfo.toImageCoordinates(h.goalpost[1].edgePointH.x, bottomHY);
    01487   Vector2<int> topRightCorner = horizonInfo.toImageCoordinates(h.goalpost[1].edgePointH.x, topHY);
    01488
    01489   // Publish goal percept
    01490   interfaces.landmarksPercept.addGoal(goalColor, boundary, topLeftCorner, topRightCorner, bottomLeftCorner, bottomRightCorner, interfaces.cameraMatrix);
    01491
    01492
    01493   // Free part of goal
    01494   switch (freeSideOfGoal)
    01495   {
    01496   case bothSides:
    01497     // Entire goal has been seen to be free
    01498     interfaces.obstaclesPercept.setFreePartOfGoal(goalColor, left.x, right.x, true, true,
    01499         horizonInfo.toImageCoordinates(h.goalpost[0].edgePointH.x, bottomHY),
    01500         horizonInfo.toImageCoordinates(h.goalpost[1].edgePointH.x, bottomHY));
    01501     break;
    01502
    01503   case leftSide:
    01504   case rightSide:
    01505     {
    01506       // Scans from the goalposts towards the midle found and obstacle (presumably
    01507       // the goaly) at the point endpointOfFreePart, hence the free part of the
    01508       // goal ranges from the left/right goalpost to this point.
    01509
    01510       // Calculate the angle to the endpoint at the goalie
    01511       Vector2<double> otherEndpointAngle;
    01512       Geometry::calculateAnglesForPoint(edgeOfFreePart, interfaces.cameraMatrix, horizonInfo.previousCameraMatrix, interfaces.image.cameraInfo, otherEndpointAngle);
    01513
    01514       // Calculate the HX coordinate of the second endpoint
    01515       double otherEndpointHX = horizonInfo.horizonAlignedXOf(edgeOfFreePart);
    01516
    01517       // Publish free part of goal
    01518       if (freeSideOfGoal == leftSide)
    01519         interfaces.obstaclesPercept.setFreePartOfGoal(goalColor, left.x, otherEndpointAngle.x, true, true,
    01520             horizonInfo.toImageCoordinates(h.goalpost[0].edgePointH.x, bottomHY),
    01521             horizonInfo.toImageCoordinates(otherEndpointHX, bottomHY));
    01522       else
    01523         interfaces.obstaclesPercept.setFreePartOfGoal(goalColor, otherEndpointAngle.x, right.x, true, true,
    01524             horizonInfo.toImageCoordinates(otherEndpointHX, bottomHY),
    01525             horizonInfo.toImageCoordinates(h.goalpost[1].edgePointH.x, bottomHY));
    01526     }
    01527   }
    01528 }
    01529
    01530
    01531 VLCGoalRecognizer::FreeSide VLCGoalRecognizer::detectFreePartOfGoal(VLCGoalRecognizer::EdgeDetector* detector,
    01532                                     const VLCGoalRecognizer::GoalHypothesis& goal,
    01533                                     int goalHeight,
    01534                                     int& freeWidth,
    01535                                     Vector2<int>& otherSide)
    01536 {
    01537   // If one of the goalpost has a significantly smaller height than the other one, it is
    01538   // assumed to be above the goalie, so scanning is only done from one goalpost. Otherwise
    01539   // scan from both goalposts (but start further down on the shorter one).
    01540   // NB: the following if treats the 2nd case first
    01541   if (goal.goalpost[0].height*3 >= goalHeight*2 && goal.goalpost[1].height*2 >= goalHeight)
    01542   {
    01543     // Scanline endpoints
    01544     Vector2<int> focus[2];
    01545
    01546     for (int i = 0; i<2; ++i)
    01547     {
    01548       //-- Calculate scanline
    01549       //if (goalpost[i].topPointVisible ...
    01550       //else
    01551
    01552       // Use the point half of the goal height below the top corners as endpoints
    01553       Vector2<double> endpointH(goal.goalpost[i].edgePointH.x,
    01554                                 goal.goalpost[i].topPointH.y + static_cast<double>(goalHeight)*0.75);
    01555       focus[i] = horizonInfo.toImageCoordinates(endpointH);
    01556
    01557       // Clip to image border
    01558       horizonInfo.clipToImageBoundary(focus[i]);
    01559     }
    01560
    01561     // Offset between the scanlines (scanning is done twice like usual)
    01562     Vector2<int> scanLineOffset(static_cast<int>(floor(horizonInfo.vertLine.direction.x*2.0+0.5)),
    01563                                 static_cast<int>(floor(horizonInfo.vertLine.direction.y*2.0+0.5)));
    01564
    01565     // Scan lines
    01566     BresenhamLineScan scanLine0(focus[0], focus[1]);
    01567     BresenhamLineScan scanLine1(focus[1], focus[0]);
    01568
    01569
    01570     //-- Look for the free part
    01571     // Points where the scanning detected the end of the goal-colour. Presumably this is
    01572     // where the goalie stands.
    01573     int goalieEdgeIndex[2] = {0, 0};
    01574
    01575     // Scan left to right
    01576     detector[0].clear(focus[0], false);
    01577     detector[1].clear(focus[0], false);
    01578     detector[0].setReferenceColor(goal.goalpost[0].color, 2);
    01579     detector[1].setReferenceColor(goal.goalpost[0].color, 2);
    01580     scanAlongLine(detector, focus[0], scanLine0, scanLineOffset, scanLine0.numberOfPixels+10, goalieEdgeIndex[0]);
    01581
    01582     // See whether its worth to scan in the other direction at all
    01583     if (goalieEdgeIndex[0]*8 < scanLine0.numberOfPixels*7)
    01584     {
    01585       // Scan right to left
    01586       detector[0].clear(focus[1], false);
    01587       detector[1].clear(focus[1], false);
    01588       detector[0].setReferenceColor(goal.goalpost[1].color, 2);
    01589       detector[1].setReferenceColor(goal.goalpost[1].color, 2);
    01590       scanAlongLine(detector, focus[1], scanLine1, scanLineOffset, scanLine1.numberOfPixels+10-goalieEdgeIndex[0], goalieEdgeIndex[1]);
    01591     }
    01592     if (goalieEdgeIndex[0]+goalieEdgeIndex[1] >= scanLine1.numberOfPixels)
    01593     {
    01594       // The entire visible goal is free
    01595       freeWidth = goal.width;
    01596       return bothSides;
    01597     }
    01598     else if (goalieEdgeIndex[0] > goalieEdgeIndex[1])
    01599     {
    01600       // Calculate the width of the free part and return the edge point
    01601       // Tricky calculation (TODO: explain)
    01602       freeWidth = goalieEdgeIndex[0]*goal.width/scanLine0.numberOfPixels;
    01603       otherSide = focus[0];
    01604       return leftSide;
    01605     }
    01606     else
    01607     {
    01608       // Calculate the width of the free part and return the edge point
    01609       freeWidth = goalieEdgeIndex[1]*goal.width/scanLine1.numberOfPixels;
    01610       otherSide = focus[1];
    01611       return rightSide;
    01612     }
    01613   }
    01614   else
    01615   {
    01616     //-- Compute scanline
    01617     // Only use the longer goalpost
    01618     int longerGoalpost = goal.goalpost[0].height > goal.goalpost[1].height ? 0 : 1;
    01619
    01620     // Start from the middle of the longer goalpost
    01621     Vector2<double> endpointH(goal.goalpost[longerGoalpost].edgePointH.x,
    01622                               goal.goalpost[longerGoalpost].topPointH.y + static_cast<int>(goalHeight)*0.75);
    01623     Vector2<int> focus = horizonInfo.toImageCoordinates(endpointH);
    01624
    01625     // Scan line parallel to the horizon
    01626     BresenhamLineScan scanLine(longerGoalpost==0 ? horizonInfo.horizon.direction
    01627                                                  : -horizonInfo.horizon.direction);
    01628
    01629     // Offset between the scanlines (scanning is done twice like usual)
    01630     Vector2<int> scanLineOffset(static_cast<int>(floor(horizonInfo.vertLine.direction.x*2.0+0.5)),
    01631                                 static_cast<int>(floor(horizonInfo.vertLine.direction.y*2.0+0.5)));
    01632
    01633
    01634     //-- Scan to find free part of goal
    01635         // Point where the scanning detected the end of the goal-colour.
    01636     int goalieEdgeIndex = 0;
    01637
    01638     // Scan from the longer goalpost
    01639     detector[0].clear(focus, false);
    01640     detector[1].clear(focus, false);
    01641     detector[0].setReferenceColor(goal.goalpost[longerGoalpost].color, 2);
    01642     detector[1].setReferenceColor(goal.goalpost[longerGoalpost].color, 2);
    01643     scanAlongLine(detector, focus, scanLine, scanLineOffset, scanLine.numberOfPixels+10, goalieEdgeIndex);
    01644
    01645     // Calculate the width of the free part and return the edge point
    01646     // WARNING: This uses the non-documented property that the direction vectors are
    01647     // multiplied by 1024.0 for the conversion to int!!!
    01648     freeWidth = goalieEdgeIndex*1024/scanLine.numberOfPixels;
    01649     otherSide = focus;
    01650     return longerGoalpost==0 ? leftSide : rightSide;
    01651   }
    01652 }
    01653
    01654
    01655
    01656 /** Function for initialization of const member in EdgeDetector. */
    01657 Matrix3x3<int> inverseCovarianceMatrix2(colorClass goalColor)
    01658 {
    01659   switch (goalColor)
    01660   {
    01661   case yellow:
    01662     return Matrix3x3<int>(Vector3<int>(279,-89,240),
    01663                               Vector3<int>(-89,1272,-82),
    01664                               Vector3<int>(240,-82,586));
    01665   case skyblue:
    01666     return Matrix3x3<int>(Vector3<int>(186,19,-3)*2,
    01667                               Vector3<int>(19,470,-29)*2,
    01668                               Vector3<int>(-3,-29,257)*2);
    01669   default:
    01670     return Matrix3x3<int>();
    01671   }
    01672 }
    01673
    01674 /** Constructor. */
    01675 VLCGoalRecognizer::EdgeDetector::EdgeDetector(colorClass target)
    01676   : targetColorClass(target),
    01677     ignoredClassification(target==skyblue ? blue : noColor),
    01678     invCovar(inverseCovarianceMatrix2(target)),
    01679     referenceColor(),
    01680     referenceColorBase(0),
    01681     deviationCounter(0),
    01682     edgeCounter(0),
    01683     lastPointInClass(),
    01684     lastPointInClassIndex(-1),
    01685     connectedToClass(false)
    01686 {
    01687 }
    01688
    01689 VLCGoalRecognizer::EdgeDetector::EdgeDetector(colorClass target, Vector3<int> referenceColor, int referenceColorBase)
    01690   : targetColorClass(target),
    01691     ignoredClassification(target==skyblue ? blue : noColor),
    01692     invCovar(inverseCovarianceMatrix2(target)),
    01693     referenceColor(referenceColor),
    01694     referenceColorBase(referenceColorBase),
    01695     deviationCounter(0),
    01696     edgeCounter(0),
    01697     lastPointInClass(),
    01698     lastPointInClassIndex(-1),
    01699     connectedToClass(false)
    01700 {
    01701 }
    01702
    01703 /** Thresholds for distances to reference colour. */
    01704 const float VLCGoalRecognizer::EdgeDetector::classThreshold = 300000;
    01705 const float VLCGoalRecognizer::EdgeDetector::edgeThreshold = 600000;
    01706