Modules/ImageProcessor/VLCImageProcessor/VLCGoalRecognizer.cpp

Go to the documentation of this file.

/**
 * @file VLCGoalRecognizer.cpp
 *
 * Implementation of class VLCGoalRecognizer
 *
 * @author <a href="mailto:oberlies@sim.tu-darmstadt.de">Tobias Oberlies</a>
 */

//#include "Tools/Player.h"
//#include "Tools/FieldDimensions.h"
//#include "Tools/RangeArray.h"
//#include "Tools/Debugging/View.h"
//#include "Representations/Perception/Image.h"
//#include "Representations/Perception/ColorTable.h"
//#include "Representations/Perception/ObstaclesPercept.h"
//#include "Representations/Perception/LandmarksPercept.h"
//#include "Representations/Perception/CameraMatrix.h"
//#include "Modules/ImageProcessor/ImageProcessorTools/ColorTableReferenceColor.h"
#include "Tools/ColorClasses.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Debugging/DebugImages.h"
#include "Tools/Debugging/DebugDrawings.h"
#include "Platform/GTAssert.h"
#include "Representations/Perception/LandmarksPercept.h"
#include "VLCImageProcessorTools.h"
#include "VLCGoalRecognizer.h"



/** Parameters */
enum parameters
{
  minGoalpostHeight = 7,
  maxScanLengthToGoalpostEdge = 14
};



/**
 * Macro to draw a rectangle that is given in horizon-aligned coordinates. 
 * @param horizoninfo The current ImageInfo instance.
 * @param x1,y1,x2,y2 The coordinates of 2 opposite corners in horizon-aligned coordinates.
 * @param penwidth The line width.
 * @param penstyle The line style, e.g. ls_solid.
 * @param pencolor The drawing colour.
 */
#ifdef NDEBUG
#define HORIZON_ALIGNED_RECTANGLE(horizoninfo, x1, y1, x2, y2, penwidth, penstyle, pencolor) ;

#else
#define HORIZON_ALIGNED_RECTANGLE(horizoninfo, x1, y1, x2, y2, penwidth, penstyle, pencolor) \
  { \
    Vector2<int> _topleft = horizoninfo.toImageCoordinates(x1, y1); \
    Vector2<int> _bottomleft = horizonInfo.toImageCoordinates(x1, y2); \
    Vector2<int> _bottomright = horizoninfo.toImageCoordinates(x2, y2); \
    Vector2<int> _topright = horizoninfo.toImageCoordinates(x2, y1); \
        QUADRANGLE(imageProcessor_flagsAndGoals, _topleft.x, _topleft.y, _bottomleft.x, _bottomleft.y, _bottomright.x, _bottomright.y, _topright.x, _topright.y, penwidth, penstyle, pencolor); \
  }

#endif


VLCGoalRecognizer::VLCGoalRecognizer(colorClass color, const ImageProcessorInterfaces& interfaces, const ColorCorrector& colorCorrector, const ImageInfo& horizonInfo) 
  : goalColor(color),
    interfaces(interfaces),
    colorCorrector(colorCorrector),
    horizonInfo(horizonInfo)
{
}

void VLCGoalRecognizer::notifyAboutNewImage()
{ 
  // Reset detection counter
  detectionCounter = 0;

  // Clear locked areas and goal hypothesises
  lockAreaCount = 0;
  hypothesisCount = 0;

  // Copy current image into the debug image
  if (yellow == goalColor)
  {
    INIT_DEBUG_IMAGE(imageProcessorGoal1, interfaces.image);
  }
  else
  {
    INIT_DEBUG_IMAGE(imageProcessorGoal2, interfaces.image);
  }
}

bool VLCGoalRecognizer::inspectNeighbourhood(const Vector2<int>& point)
{
  // Skip if on the image border
  if (nearImageBorder(point, 0))
    return false;

  // Look at 9-connected neighbourhood and count the number of pixels in target colour
  int targetCount = 0;
  for (int i = -1; i<=1; ++i)
    for (int j = -1; j<=1; ++j)
      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))
      {
        // This pixel has the goal colour
        ++targetCount;
      }

  // Positive result if the majority of the pixels are in goal colour
  return targetCount>4;
}

void VLCGoalRecognizer::notifyAboutFinish()
{
  // Edge detector state machines
  VLCGoalRecognizer::EdgeDetector detector[2] = 
  {
    VLCGoalRecognizer::EdgeDetector(goalColor),
    VLCGoalRecognizer::EdgeDetector(goalColor)
  };

  // Merge goal fragments
  bool deletedHypothesis[maxHypothesises];
  mergeFragments(deletedHypothesis);

  // Interpret goal hypothesises
  interpretResults(detector, deletedHypothesis);


  if (yellow == goalColor)
  {
    // Send debug image
    SEND_DEBUG_IMAGE(imageProcessorGoal1);
  }
  else
  {
    // This is the second GoalRecognizer, so send drawing stuff now
    DEBUG_DRAWING_FINISHED(imageProcessor_flagsAndGoals);
    SEND_DEBUG_IMAGE(imageProcessorGoal2);
  }
}

void VLCGoalRecognizer::analyzeGoal(Vector2<int> focus)
{
  // Edge detector state machines
  VLCGoalRecognizer::EdgeDetector detector[2] = 
  {
    VLCGoalRecognizer::EdgeDetector(goalColor), 
    VLCGoalRecognizer::EdgeDetector(goalColor)
  };

  // Flags storing on which image border scanning has been done, to prevent repeated scans 
  // on the image borders within one analysis step or in different steps. The array is 
  // indexed by the enum imageBorderType.
  bool imageBorderScanned[4] = {false, false, false, false};

  // Objects to store detection result
  VLCGoalRecognizer::GoalHypothesis result;


  //-- Analyze the left goalpost
  analyzeGoalpost(detector, -horizonInfo.horizon.direction, true, focus, result.goalpost[0], imageBorderScanned);


  //-- Scan along the cross bar
  scanCrossBar(detector, focus, result, imageBorderScanned);

  // Filter obvious false hypothesis
  result.width = static_cast<int>(result.crossBarEndpointH.x-result.goalpost[0].edgePointH.x);
  if (result.goalpost[0].height+result.width < 15 || result.width <= 5)
  {
    // Prevent this blob to be analysed again
    lockArea(result.crossBarEndpointH.x + 10, result.goalpost[0].bottomPointH.y + 10, result.goalpost[0].onGreen);

    HORIZON_ALIGNED_RECTANGLE(horizonInfo, 
      result.goalpost[0].edgePointH.x, min(result.goalpost[0].topPointH.y, result.crossBarEndpointH.y),
            result.crossBarEndpointH.x, result.goalpost[0].bottomPointH.y,
      1, Drawings::ps_solid, Drawings::red);

    return;
  }


  //-- Analyze the right goalpost
  analyzeGoalpost(detector, horizonInfo.horizon.direction, false, focus, result.goalpost[1], imageBorderScanned);
  
  // Possibly recalculate the height of the left goalpost (necessary if scanCrossBar 
  // scanned along the image border downwards)
  if (result.crossBarEndpointH.y < result.goalpost[1].topPointH.y)
  {
    result.goalpost[1].topPoint = result.crossBarEndpoint;
    result.goalpost[1].topPointH = result.crossBarEndpointH;
    result.goalpost[1].height = static_cast<int>(result.goalpost[1].bottomPointH.y - result.goalpost[1].topPointH.y);
  }

  // Calculate the width and height
  result.width = static_cast<int>(result.goalpost[1].edgePointH.x - result.goalpost[0].edgePointH.x);
  result.height = max(result.goalpost[0].height, result.goalpost[1].height);

  // Goal hypothesis is on the green field?
  result.onGreen = result.goalpost[0].onGreen || result.goalpost[1].onGreen;

  //-- Interpretation
  // Prevent that this goal is discovered again
  lockArea(result.goalpost[1].edgePoint.x + 5, max(result.goalpost[0].bottomPointH.y, result.goalpost[1].bottomPointH.y) + 5, result.goalpost[0].onGreen);

  // Add goal to hypothsises
  if (hypothesisCount < maxHypothesises)
    hypothesis[hypothesisCount++] = result;
}

void VLCGoalRecognizer::analyzeGoalpost(VLCGoalRecognizer::EdgeDetector* detector, 
                                           const Vector2<double>& directionOfEdge, 
                                           bool leftGoalpost,
                                           Vector2<int>& focus, 
                                           VLCGoalRecognizer::Goalpost& result,
                                           bool* imageBorderScanned)
{
  // Container to hold the detected points on the edge
  EdgePointList edgePoints;

  // Scan along the image border, because the normal scanning would violate the borders
  bool borderScan = false;

  // While scanning towards the edge, a huge target-coloured area was discovered. This 
  // usually happens, if a goal was only partially detected in a preceding goal analysis 
  // and hence not entirely locked and detected again. As a result, the same goal might be 
  // scanned again, which is a waste of processing time. Therefore I cancel the detection
  // in these cases
  bool detectionCanceled = false;

  // Bresenham scanlines along and perpendicular to the goalpost
  // The vertical scanline is slightly rotated inwards (i.e. away from the edge) to account 
  // for motion skew or a non-perfect horizon.
  const double verticalScanSlope = 0.1;
  BresenhamLineScan horizontalScan(directionOfEdge);
  BresenhamLineScan verticalScan(horizonInfo.vertLine.direction - directionOfEdge*verticalScanSlope);

  // Offset between the two parallel scanlines (horizontal and vertical scans are usually 
  // done on two scanlines for more noise tolerance)
  Vector2<int> horizontalScanOffset(static_cast<int>(floor(-horizonInfo.vertLine.direction.x*2+0.5)),
                                    static_cast<int>(floor(-horizonInfo.vertLine.direction.y*2+0.5)));
  Vector2<int> verticalScanOffset(static_cast<int>(floor(-directionOfEdge.x+0.5)), 
                                  static_cast<int>(floor(-directionOfEdge.y+0.5)));

  //-- Scan towards the edge
  detector[0].clear(focus, false);
  detector[1].clear(focus, true);
  switch (detectEdgeTwice(detector, focus, horizontalScan, horizontalScanOffset, maxScanLengthToGoalpostEdge, edgePoints))
  {
  case imageBorder:
    // Check whether the focus is on the image border (might not be there, because 
    // the last point in the target class is returned)
    borderScan = nearImageBorder(focus, 1);

    // Otherwise assume that there is an edge
    // Even if the focus is not on the image border, it is still possible that there is 
    // no edge. The edge detector would have needed to investigates some more pixels (not 
    // available due to the image border) to decide this. But since assuming a (weak) 
    // edge is not harmful if its wrong, so we do this.
    break;

  case none:
    // Scanning towards the edge did find nothing but goal-coloured pixels. In this case
    // we cancel the scanning, because it is likely, that we are running into an area,
    // that has been scanned during a preceding goal analysis
    detectionCanceled = true;
  }
  
  // Save this point for upwards scanning
  Vector2<int> firstEdgePoint = focus;

  // Variable to store if the downwards scan started with a scan on the image border
  VLCGoalRecognizer::ImageBorderSide firstEdgeImageBorder = noBorder;


  //-- Sense along the edge downwards
  if (borderScan)
  {
    // Scan along the image border and store on which image border the scan has been done.
    // The scanned side will be scanned again in the other direction at the beginning of
    // the upwards sensing step
    firstEdgeImageBorder =
      scanOnImageBorder(detector, focus, horizonInfo.vertLine.direction, 3, imageBorderScanned);
    borderScan = false;

    if (firstEdgeImageBorder != noBorder)
    {
      // Store image border point
      edgePoints.add(focus, imageBorder);

      // Set slightly inwards for the following scans
      focus += verticalScanOffset*2;
      horizonInfo.clipToImageBoundary(focus);
    }
  }

  for (int i = 0; !detectionCanceled && i<25; ++i)
  {
    ASSERT(i!=23);  // This loop should be not be infinite without the limit i<12

    if (borderScan)
    {
      // Slide alon the image border to the bottom
      if (noBorder == scanOnImageBorder(detector, focus, horizonInfo.vertLine.direction, 3, imageBorderScanned))
      {
        // The border scan failed without finding any pixels in the target class, so
        // assume the bottom point has been outrun or scanning along the image border
        // the focus has run into doesn't make sense
        break;
      }
      borderScan = false;

      // Store image border point
      edgePoints.add(focus, imageBorder);

      // Set slightly inwards
      focus += verticalScanOffset*2;
      horizonInfo.clipToImageBoundary(focus);
    }
    else
    {
      // Scan downwards for 20 pixels (at maximum)
      detector[0].clear(focus, true);
      detector[1].clear(focus, true);
      int scanned;
      if (scanAlongLine(detector, focus, verticalScan, verticalScanOffset, 20, scanned) == imageBorder)
      {
        // Assuming that we either hit the bottom border, or the image border and the 
        // edge converge, continue scanning on the edge (not true if there is only a 
        // very thin strip of the goal in the image, but don't worry too much about 
        // this case)
        borderScan = true;

        // Store image border point
        edgePoints.add(focus, imageBorder);
        continue;
      }

      // Only scan horizontal again if vertical scan made it at least some pixels
      if (scanned>4)
      {
        // Scan towards the edge
        detector[0].clear(focus, true);
        detector[1].clear(focus, true);
        switch(detectEdgeTwice(detector, focus, horizontalScan, horizontalScanOffset, maxScanLengthToGoalpostEdge, edgePoints))
        {
        case imageBorder:
          // Check whether the focus is on the image border
          borderScan = nearImageBorder(focus, 1);
          break;

        case none:
          // There is no edge, so cancel goalpost detection
          detectionCanceled = true;
        }
      }

      // Leave the loop if vertical scan didn't get far
      if (scanned<8)
        break;
    }
  } // Downwards sensing loop

  // Store bottom point
  result.bottomPoint = focus;
  result.bottomPointH = horizonInfo.toHorizonAligned(focus);
  //int bottomPointIndex = edgePoints.size() - 1;

  // Store reference colour of the edge detector used on the inner scanline. It is needed
  // for the detection of the free part of the goal (is done in the analysis at the end)
  detector[1].getReferenceColor(result.color);

  //-- Sense along edge upwards
  // This is only necessary on the left goalpost. The crossbar analysis ends at the top 
  // point of the right goalpost.
  focus = firstEdgePoint;

  if (leftGoalpost && !detectionCanceled)
  {
    verticalScan = BresenhamLineScan(-horizonInfo.vertLine.direction - directionOfEdge*verticalScanSlope);
    horizontalScanOffset = -horizontalScanOffset;
    borderScan = false;

    if (firstEdgeImageBorder != noBorder)
    {
      // The scan towards the edge ran into the image border and as a consequence, the 
      // sensing towards the bottom began with a scan along the border. It might be 
      // necessary to begin with a scan along the image border again, but this would 
      // usually be prohibited (no image side may be scanned twice). So we need to 
      // explicitly allow the scan on the same side.
      imageBorderScanned[firstEdgeImageBorder] = false;

      // Now scan along the image border
      if (noBorder != scanOnImageBorder(detector, focus, -horizonInfo.vertLine.direction, 3, imageBorderScanned))
      {
        // Store image border point
        edgePoints.add(focus, imageBorder);

        // Set slightly inwards for the following scans
        focus += verticalScanOffset*2;
        horizonInfo.clipToImageBoundary(focus);
      }

      // Disallow the scanned image border side again (the call to scanOnImageBorder 
      // might not have scanned along any side or along a different side, both would be 
      // okay)
      imageBorderScanned[firstEdgeImageBorder] = true;
    }

    for (int i = 0; !detectionCanceled && i<12; ++i)
    {
      //ASSERT(i!=10);    // This loop should be not be infinite without the limit i<12

      if (borderScan)
      {
        // Slide alon the image border to the top
        if (noBorder == scanOnImageBorder(detector, focus, horizonInfo.vertLine.direction, 3, imageBorderScanned))
        {
          // The border scan failed without finding any pixels in the target class, 
          // so assume the bottom point has been outrun or that the focus ran into
          // an image border not worth to be followed (with resepect to our current
          // desired scanning direction)
          break;
        }
        borderScan = false;

        // Store image border point
        edgePoints.add(focus, imageBorder);

        // Set slightly inwards for the following scanning
        focus += verticalScanOffset*2;
        horizonInfo.clipToImageBoundary(focus);
      }
      else
      {
        // Scan upwards for 20 pixels (at maximum)
        detector[0].clear(focus, true);
        detector[1].clear(focus, true);
        int scanned;
        if (scanAlongLine(detector, focus, verticalScan, verticalScanOffset, 20, scanned) == imageBorder)
        {
          // Store image border point
          edgePoints.add(focus, imageBorder);

          // Scan along the image border
          borderScan = true;
          continue;
        }
      
        // Only scan horizontal again if vertical scan made it at least some pixels
        if (scanned>8)
        {
          // Scan towards the edge
          detector[0].clear(focus, true);
          detector[1].clear(focus, true);
          switch(detectEdgeTwice(detector, focus, horizontalScan, horizontalScanOffset, maxScanLengthToGoalpostEdge, edgePoints))
          {
          case imageBorder:
            // Scan along the border
            borderScan = true;
            break;

          case none:
            // Assume there is actually no goalpost
            detectionCanceled = true;
          }
        }
        else
          break;
      }

    } // Upwards sensing loop
  }
  
  // If the top point of the goalpost still has to be found (for the following cross bar 
  // analysis), but there was no goapost, just scan straight upwards
  if (detectionCanceled && leftGoalpost)
  {
    verticalScan = BresenhamLineScan(-horizonInfo.vertLine.direction);
    
    // Scan straight upwards (unlike before, where the scanline is slightly slanted away
    // from the edge)
    detector[0].clear(focus, true);
    detector[1].clear(focus, true);
    
    int dummy;
    if (scanAlongLine(detector, focus, verticalScan, verticalScanOffset, 1024, dummy) == imageBorder)
    {
      // Trouble with the image border again
      detector[0].clear(focus, false);
      detector[1].clear(focus, false);
      scanOnImageBorder(detector, focus, horizonInfo.vertLine.direction, 3, imageBorderScanned);
    }
  }


  // Store top point
  result.topPoint = focus;
  result.topPointH = horizonInfo.toHorizonAligned(focus);

  //-- Analysis
  result.nonExistant = detectionCanceled;
  
  if (detectionCanceled)
  {
    // Set default values
    result.height = 0;
    result.visibleHeight = 0;
    result.edgePoint = result.topPoint;
    result.edgePointH = result.topPointH;
    result.onGreen = false;
  }
  else
  {
    // Calculate the height of the goalpost
    result.height = static_cast<int>(result.bottomPointH.y - result.topPointH.y);
    //OUTPUT(idText,text,result.height[index]);

    // Check if there is green below the goalpost
    if (result.height > minGoalpostHeight)
      result.onGreen = detectGreenBelowGoalpost(result.bottomPoint, directionOfEdge, result.height);
    else
      result.onGreen = false;


    // TODO replace by proper one
    result.visibleHeight = edgePoints.getCount(edge)+edgePoints.getCount(deviationWithoutEdge)>0 ? 1 : 0;

    if (result.visibleHeight > 0)
    {
      // Do additional scans
      // Check straightness
      // Store slope

      // Average all edge point as the goalpost edge
      for (int i = 0; i<edgePoints.size(); ++i)
      {
        switch (edgePoints.getType(i))
        {
        case edge:
        case deviationWithoutEdge:
          result.edgePoint += edgePoints[i];
        }
      }
      result.edgePoint /= edgePoints.getCount(edge)+edgePoints.getCount(deviationWithoutEdge);
      result.edgePointH = horizonInfo.invRotation * Vector2<double>(static_cast<double>(result.edgePoint.x),
                                    static_cast<double>(result.edgePoint.y));

      // Store the number of weak and strong edges
      result.strongEdgeCount = edgePoints.getCount(edge);
      result.weakEdgeCount = edgePoints.getCount(deviationWithoutEdge);
    }
    else // visibleHeight == 0
    {
      ASSERT(edgePoints.getCount(imageBorder)>0);

      // Use the leftmost/rightmost 
      double extremeHX = leftGoalpost ? 1024.0f : -1024.0f;

      for (int i = 0; i<edgePoints.size(); ++i)
      {
        if (edgePoints.getType(i) == imageBorder)
        {
          // Transform into the horizon-aligned coordinate system
          double HX = horizonInfo.invRotation.c[0].x * edgePoints[i].x
              + horizonInfo.invRotation.c[1].x * edgePoints[i].y;
          if ((HX < extremeHX) == leftGoalpost)
          {
            // Use this point as extreme point. 
            // NB: with goalpostVisibleHeight==0 it is obvious that albeit the name,
            // the value is not an edge point
            result.edgePoint = edgePoints[i];
            result.edgePointH.x = HX;
            extremeHX = HX;
          }
        }
      }

      // Store edge point as well in horizon-aligned coordinates
      result.edgePointH.y = horizonInfo.invRotation.c[0].y * result.edgePoint.x
                + horizonInfo.invRotation.c[1].y * result.edgePoint.y;
    }
  }


  // Possible improvements
  // - straightness check with added scans to the edge -> need to store more edge point so
  //   that scanlines with weak edges aren't scanned again & no scans towards the image 
  //   boundary
}

VLCGoalRecognizer::EdgeType VLCGoalRecognizer::detectEdgeTwice(VLCGoalRecognizer::EdgeDetector* detector,
                                                                     Vector2<int>& focus,
                                                                     BresenhamLineScan& scanLine,
                                                                     const Vector2<int>& scanLineOffset,
                                                                     int maxScanLength,
                                                                     EdgePointList& edgePoints)
{
  // Start point for the first scanline (scanning from focus is done second)
  Vector2<int> focus0 = focus + scanLineOffset;

  int distance[2];
  bool onImageBorder[2];// = {false, false};

  // Scan towards the edge
  VLCGoalRecognizer::EdgeType type =
    detectEdge(detector[0], focus0, scanLine, maxScanLength, distance[0]);
  if (type == none)
  {
    // Signalize that there is an large goal-coloured area where the edge was expected
    return none;
  }
  else
  {
    // Store the detected edge
    edgePoints.add(focus0, type);
    onImageBorder[0] = type == imageBorder;
  }
  
  // Scan towards the edge again
  scanLine.init();
  Vector2<int> focus1 = focus;
  type = detectEdge(detector[1], focus1, scanLine, maxScanLength, distance[1]);
  if (type == none)
  {
    // Signalize that there is an large goal-coloured area where the edge was expected
    return none;
  }
  else
  {
    // Store the detected edge
    edgePoints.add(focus1, type);
    onImageBorder[1] = type == imageBorder;
  }

  //-- Use the point closer the start point as new focus
  if (onImageBorder[0])
  {
    if (onImageBorder[1])
    {
      // Return the lastPointInClass nearer to the start point as new focus
      if (distance[0]<distance[1])
        focus = focus0;
      else
        focus = focus1;

      // Both edges on the image border
      return imageBorder;
    }
    else
    {
      // Return edge point in focus
      focus = focus1;
      return edge;
    }
  }
  else
  {
    if (onImageBorder[1])
    {
      // Return edge point in focus
      focus = focus0;
      return edge;
    }
    else
    {
      // Use the edge point nearer to the start point as new focus
      if (distance[0]<distance[1])
        focus = focus0;
      else
        focus = focus1;
      
      // Edge detected
      return edge;
    }
  }
}

VLCGoalRecognizer::ImageBorderSide VLCGoalRecognizer::scanOnImageBorder(
                                           VLCGoalRecognizer::EdgeDetector* detector,
                                           Vector2<int>& focus,
                                           const Vector2<double>& targetDirection,
                                           int maxBorderDistance,
                                           bool* scannedSides)
{
  // Exclude the possibility, that the focus is considered near to more than two adjacent
  // borders
  ASSERT(maxBorderDistance < horizonInfo.maxImageCoordinates.x/2 &&
         maxBorderDistance < horizonInfo.maxImageCoordinates.y/2);


  // Check to which borders the focus is close
  // If the focus is close to two borders, i.e. in a corner of the image, the following 
  // code also computes two vectors pointing away from that corner. These can be used to 
  // decide, whether a border scan makes any sense.
  bool nearBorder[4] = {false, false, false, false};
  int nearCount = 0;
  bool scannedBefore[2];
  Vector2<double> scanDirection[2];
  double awayFromEdgeFactor = 1.0;
  
  if (focus.y <= maxBorderDistance)
  {
    // Top
    nearBorder[topBorder] = true;
    scannedBefore[nearCount] = scannedSides[topBorder];
    scanDirection[nearCount++].x = awayFromEdgeFactor;
    awayFromEdgeFactor *= -1.0;

  }
  if (focus.x <= maxBorderDistance)
  {
    // Left
    nearBorder[leftBorder] = true;
    scannedBefore[nearCount] = scannedSides[leftBorder];
    scanDirection[nearCount++].y = -awayFromEdgeFactor;
    awayFromEdgeFactor *= -1.0;
  }
  if (horizonInfo.maxImageCoordinates.y-focus.y <= maxBorderDistance)
  {
    // Bottom
    nearBorder[bottomBorder] = true;
    scannedBefore[nearCount] = scannedSides[bottomBorder];
    scanDirection[nearCount++].x = -awayFromEdgeFactor;
    awayFromEdgeFactor *= -1.0;
  }
  if (horizonInfo.maxImageCoordinates.x-focus.x <= maxBorderDistance)
  {
    // Right
    nearBorder[rightBorder] = true;
    scannedBefore[nearCount] = scannedSides[rightBorder];
    scanDirection[nearCount++].y = awayFromEdgeFactor;
    //awayFromEdgeFactor *= -1.0;
  }

  // If the focus is near the top right corner, the vectors are pointing towards the edge,
  // so reverse them
  if (nearBorder[topBorder] && nearBorder[rightBorder])
  {
    scanDirection[0] = -scanDirection[0];
    scanDirection[1] = -scanDirection[1];
  }

  // Compute the scan direction
  switch (nearCount)
  {
  case 0:
    // Focus is not close to any border
    return noBorder;
  
  case 1:
  {
    // Focus is close to one border, so allow scanning along this border in both 
    // directions, if the angle with the target direction is less than 60 degrees and if
    // it hasn't been scanned before
    double dotProduct = targetDirection*scanDirection[0];
    if (scannedBefore[0] || fabs(dotProduct) < 0.5)
      return noBorder;

    // Reverse the scan direction if necessary
    scanDirection[0] *= sgn(dotProduct);
    break;
  }
  case 2:
  default:
    // Check if the target direction has an angle less than 60 degrees with one of the 
    // scan directions (that haven't been scanned before). Here the directions may not be 
    // reversed, because then scanning would run right into the corner
    if (!scannedBefore[0] && targetDirection*scanDirection[0] > 0.5)
    {
      // Use this direction (scanDirection[0] will be used below, so nothing to do)
    }
    else if (!scannedBefore[1] && targetDirection*scanDirection[1] > 0.5)
    {
      scanDirection[0] = scanDirection[1];
    }
    else
    {
      // None of the potential directions meet the requirements
      return noBorder;
    }
  }
  ASSERT(scanDirection[0].x*scanDirection[0].y==0.0);

  // Compute the start points for scanning
  Vector2<int> newFocus = focus;
  Vector2<int> scanOffset(0, 0);
  VLCGoalRecognizer::ImageBorderSide scanSide;

  if (nearBorder[topBorder] && scanDirection[0].y==0.0)
  {
    // Top
    newFocus.y = 0;
    scanOffset.y = 1;
    scanSide = topBorder;
  }
  else if (nearBorder[leftBorder] && scanDirection[0].x==0.0)
  {
    // Left
    newFocus.x = 0;
    scanOffset.x = 1;
    scanSide = leftBorder;
  }
  else if (nearBorder[bottomBorder] && scanDirection[0].y==0.0)
  {
    // Bottom
    newFocus.y = horizonInfo.maxImageCoordinates.y;
    scanOffset.y = -1;
    scanSide = bottomBorder;
  }
  else
  {
    // Right
    newFocus.x = horizonInfo.maxImageCoordinates.x;
    scanOffset.x = -1;
    scanSide = rightBorder;
  }

  /*DOT(imageProcessor_flagsAndGoals, newFocus.x, newFocus.y, Drawings::Color::orange, Drawings::Color::orange);
  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);
  */

  // Scan along edge
  BresenhamLineScan scanLine(scanDirection[0]);
  int scannedCount;
  scanAlongLine(detector, newFocus, scanLine, scanOffset, 1024, scannedCount);
  
  // Check if any scanning was done at all
  if (scannedCount < 0)
  {
    // Nothing scanned
    return noBorder;
  }
  else
  {
    // Don't scan this side again
    scannedSides[scanSide] = true;

    // Return the new focus and on which side we did the scanning
    LINE(imageProcessor_flagsAndGoals, focus.x, focus.y, newFocus.x, newFocus.y, 1, Drawings::ps_solid, Drawings::orange);
    DOT(imageProcessor_flagsAndGoals, focus.x, focus.y, Drawings::white, Drawings::white);
    DOT(imageProcessor_flagsAndGoals, newFocus.x, newFocus.y, Drawings::orange, Drawings::orange);
    focus = newFocus;
        return scanSide;
  }
}



void VLCGoalRecognizer::scanCrossBar(VLCGoalRecognizer::EdgeDetector* detector,
                                        Vector2<int>& focus,
                                        VLCGoalRecognizer::GoalHypothesis& result,
                                        bool* imageBorderScanned)
{
  DOT(imageProcessor_flagsAndGoals, focus.x, focus.y, Drawings::orange, Drawings::orange);  
  
  // Check if start point is on the image border
  if (nearImageBorder(focus, 3))
  {
    // Scan along the edge
    scanOnImageBorder(detector, focus, horizonInfo.horizon.direction, 3, imageBorderScanned);
  }
  bool borderScan = false;
  bool nonBorderScanDone = false;
  
  DOT(imageProcessor_flagsAndGoals, focus.x, focus.y, pink, pink);


  // Bresenham lines for scans towards the edge
  BresenhamLineScan upwardsScan(-horizonInfo.vertLine.direction);

  //
  bool calculateScanLine = false;
  
  // Last edge point (for extrapolating the optimal slope)
  Vector2<int> lastEdgePoint = focus;


  //-- Start with scanning exactly horizontally 
  Vector2<double> scanDirection = horizonInfo.horizon.direction;
  BresenhamLineScan horizontalScan(scanDirection);
  Vector2<int> horizontalScanOffset(static_cast<int>(floor(horizonInfo.vertLine.direction.x+0.5)),
                                    static_cast<int>(floor(horizonInfo.vertLine.direction.y+0.5)));
  focus += horizontalScanOffset;
  detector[0].clear(focus, true);
  detector[1].clear(focus, true);
  int scanned;

  switch (scanAlongLine(detector, focus, horizontalScan, horizontalScanOffset, 12, scanned))
  {
  case VLCGoalRecognizer::imageBorder:
    // Scan along the border
    borderScan = true;

  case VLCGoalRecognizer::edge:
  case VLCGoalRecognizer::deviationWithoutEdge:
    if (scanned < 6)
    {
      // Scan again with a lower starting point
      focus = lastEdgePoint + Vector2<int>(static_cast<int>(floor(horizonInfo.vertLine.direction.x*5 + horizonInfo.horizon.direction.x*2 + 0.5)),
                                           static_cast<int>(floor(horizonInfo.vertLine.direction.y*5 + horizonInfo.horizon.direction.y*2 + 0.5)));
      if (scanAlongLine(detector, focus, horizontalScan, horizontalScanOffset, 12, scanned) == imageBorder)
      {
        // Image border reached
        borderScan = true;
      }
    }
    
    // The horiziontal scanline was not ok, so recalculate it
    calculateScanLine = true;
  }


  //-- Sense along the crossbar
  for (int i = 0; !borderScan && i<25; ++i)
  {
    //ASSERT(i!=23);    // This loop should not be infinite without the limit i<25

    // The horizontal scan did not reach the border, hence there is at least some of the 
    // crossbar visible
    nonBorderScanDone = true;

        // Get in touch with the edge again
    detector[1].clear();
    int scanTillEdgeLength;
    if (detectEdge(detector[1], focus, upwardsScan, 10, scanTillEdgeLength) == imageBorder)
    {
      // Check, wether the focus is near the edge
      if (borderScan = nearImageBorder(focus, 1)) // Single = is correct!
        break;

      // If the focus is not near the image border, we assume that there was no edge
      // even if there might have been one. But if there actually is one, the subsequent
      // step will run into it anyway.
    }

    // Recalculate scanning direction (if necessary)
    if (calculateScanLine || scanTillEdgeLength>2)
    {
      // If the goal is seen from nearby, the crossbar may have different slopes and 
      // may appear curved to the left (in sanning direction from the left to the right 
      // goalpost). Therefore the following calculates the slope from the first two 
      // edge points (first iteration) or recalculates it, if the slope changed. 
      horizontalScan = BresenhamLineScan(lastEdgePoint, focus);
      scanDirection.x = static_cast<double>(focus.x-lastEdgePoint.x);
      scanDirection.y = static_cast<double>(focus.y-lastEdgePoint.y);
      scanDirection.normalize();
      Vector2<int> horizontalScanOffset(static_cast<int>(floor(-scanDirection.y+0.5)),
                                        static_cast<int>(floor( scanDirection.x+0.5)));

      // Don't allow scanDirections with angles greater than 60 degrees to the horizon,
      // because they are unlikely and may lead to infinite loops (since the direction
      // for scans towards the edge is not adapted)
      if (horizonInfo.horizon.direction*scanDirection < 0.5)
        break;
    }

    // Scan to the right
    focus += horizontalScanOffset;
    detector[0].clear();
    detector[1].clear(); // WErte??
    switch (scanAlongLine(detector, focus, horizontalScan, horizontalScanOffset, 12, scanned))
    {
    case VLCGoalRecognizer::imageBorder:
      // Cross bar is outside the image, so quit the loop
      borderScan = true;
      break;

    case VLCGoalRecognizer::edge:
    case VLCGoalRecognizer::deviationWithoutEdge:
      // The cross bar should appear straight or like a convex curve (with the sides of
      // the goal; often if the dog is close to the goal). With the intitial scan 
      // direction calculation and the recalculations, the scan should not run into the
      // top edge at this point. Hence assume the right goalpost.

      // ScanAlongLine is keen to get through and returns the furthest point it could
      // consider to be inside the goal. However here this might just have outrun the 
      // right goalpost, so we set the focus a bit back. The folowing goalpost analysis
      // begins with a scan towards the edge anyway.
      focus = focus - Vector2<int>(static_cast<int>(scanDirection.x*4),
                                   static_cast<int>(scanDirection.y*4));
      result.crossBarEndpoint = focus;
      result.crossBarEndpointH = horizonInfo.toHorizonAligned(focus);
      return;
    }
  }

  // The current point is potentially useful to determine the height of the right goalpost
  // (if its top endpoint is outside the image). Additionally it is used to filter false
  // goal hypothesises
  result.crossBarEndpoint = focus;
  result.crossBarEndpointH = horizonInfo.toHorizonAligned(focus);

  // Only scan along the edge again if there was any of the cross bar visible (and hence we
  // haven't just done that)
  if (nonBorderScanDone && borderScan)
    scanOnImageBorder(detector, focus, horizonInfo.horizon.direction, 3, imageBorderScanned);
}

VLCGoalRecognizer::EdgeType VLCGoalRecognizer::detectEdge(VLCGoalRecognizer::EdgeDetector& detector, 
                                                                Vector2<int>& focus, 
                                                                BresenhamLineScan& scanLine,
                                                                int maxScanLength,
                                                                int& pixelsUntilEdge)
{
  // Reset Bresenham scanline
  scanLine.init();

  // Feed detector with points on the Bresenham scanLine
  int i;
  for (i = 0; i<maxScanLength && focus.x>=0 && focus.y>=0 && focus.x<=horizonInfo.maxImageCoordinates.x && focus.y<=horizonInfo.maxImageCoordinates.y; ++i)
  {
    STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus.x, focus.y, BLACK);

    // Colour and colour class of focused pixel
    unsigned char color[3] = 
    {
      colorCorrector.correct(focus.x, focus.y, 0, interfaces.image.image[focus.y][0][focus.x]),
      colorCorrector.correct(focus.x, focus.y, 1, interfaces.image.image[focus.y][1][focus.x]),
      colorCorrector.correct(focus.x, focus.y, 2, interfaces.image.image[focus.y][2][focus.x])
    };
    colorClass colorCls = COLOR_CLASS(color[0], color[1], color[2], (*bestColorTable));

    switch (detector.inspectPixel(focus, i, colorCls, Vector3<int>(static_cast<int>(color[0]), static_cast<int>(color[1]), static_cast<int>(color[2]))))
    {
    case edge:
      DOT(imageProcessor_flagsAndGoals, detector.getLastPointInClass().x, detector.getLastPointInClass().y, Drawings::black, Drawings::green);
      STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus.x, focus.y, GREEN);

      // Return edge via focus argument
      detector.getLastPointInClass(focus, pixelsUntilEdge);
      return edge;
      break;

    case deviationWithoutEdge:
      DOT(imageProcessor_flagsAndGoals, detector.getLastPointInClass().x, detector.getLastPointInClass().y, Drawings::black, Drawings::white);
      STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus.x, focus.y, RED);

      // Return last point in class via focus argument
      detector.getLastPointInClass(focus, pixelsUntilEdge);
      return deviationWithoutEdge;
      break;
    }

    // Get the next point on the line
    scanLine.getNext(focus);
  }
  
  if (i==maxScanLength)
  {
    // Signalize no edge found withing scan length limit
    DOT(imageProcessor_flagsAndGoals, detector.getLastPointInClass().x, detector.getLastPointInClass().y, Drawings::black, Drawings::red);
    return none;
  }
  else
  {
    // Return last point in class via focus argument
    detector.getLastPointInClass(focus, pixelsUntilEdge);
    return imageBorder;
  }
}

VLCGoalRecognizer::EdgeType VLCGoalRecognizer::scanAlongLine(
                                    VLCGoalRecognizer::EdgeDetector* detector,
                                    Vector2<int>& focus,
                                    BresenhamLineScan& scanLine,
                                    const Vector2<int>& scanLineOffset,
                                    int maximumScan,
                                    int& pixelsScanned)
{
  // Scanning is done on two scanlines at once and only if both EdgeDetectors report 
  // edges/deviations stop scanning
  bool edge[2];
  bool withinBounds[2];

  // Temporary variables
  unsigned char color[3];
  colorClass colorCls;
  Vector2<int> focus2;

  // Feed the detectors with points on the Bresenham scanLine
  for (int i = 0; i <= maximumScan; ++i)
  {
    // First scanline
    if (withinBounds[0] = focus.x>=0 && focus.y>=0 && focus.x<=horizonInfo.maxImageCoordinates.x && focus.y<=horizonInfo.maxImageCoordinates.y)
    {
      // Colour and colour class of focused pixel
      color[0] = colorCorrector.correct(focus.x, focus.y, 0, interfaces.image.image[focus.y][0][focus.x]);
      color[1] = colorCorrector.correct(focus.x, focus.y, 1, interfaces.image.image[focus.y][1][focus.x]);
      color[2] = colorCorrector.correct(focus.x, focus.y, 2, interfaces.image.image[focus.y][2][focus.x]);
      colorCls = COLOR_CLASS(color[0], color[1], color[2], (*bestColorTable));

      // Feed EdgeDetector
      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])));

      if (edge[0])
      {
        STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus.x, focus.y, WHITE);
      }
      else
      {
        STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus.x, focus.y, BLUE);
      }
    }
    
    // Second scanline
    focus2 = focus + scanLineOffset;
    if (withinBounds[1] = focus2.x>=0 && focus2.y>=0 && focus2.x<=horizonInfo.maxImageCoordinates.x && focus2.y<=horizonInfo.maxImageCoordinates.y)
    {
      // Colour and colour class of focused pixel
      color[0] = colorCorrector.correct(focus2.x, focus2.y, 0, interfaces.image.image[focus2.y][0][focus2.x]);
      color[1] = colorCorrector.correct(focus2.x, focus2.y, 1, interfaces.image.image[focus2.y][1][focus2.x]);
      color[2] = colorCorrector.correct(focus2.x, focus2.y, 2, interfaces.image.image[focus2.y][2][focus2.x]);
      colorCls = COLOR_CLASS(color[0], color[1], color[2], (*bestColorTable));

      // Feed EdgeDetector
      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])));

      if (edge[1])
      {
        STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus2.x, focus2.y, WHITE);
      }
      else
      {
        STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus2.x, focus2.y, BLUE);}
    }

    // Merge results of both scanlines
    if (withinBounds[0] && withinBounds[1])
    {
      if (edge[0] && edge[1])
      {
        // Return point before edge/deviation via focus argument
        int scannedFurther = detector[0].getLastIndexInClass()>detector[1].getLastIndexInClass() ? 0 : 1;
        detector[scannedFurther].getLastPointInClass(focus, pixelsScanned);

        // Signalize edge/deviation
        // NB: This method does not distinguish between strong and weak edges!
        return VLCGoalRecognizer::edge;
      }
    }
    else
    {
      // Return the valid point via focus argument and signalize image border
      int scannedFurther = detector[0].getLastIndexInClass()>detector[1].getLastIndexInClass() ? 0 : 1;
      detector[scannedFurther].getLastPointInClass(focus, pixelsScanned);
      return VLCGoalRecognizer::imageBorder;
    }

    // Get the next point on the line
    scanLine.getNext(focus);
  }

  // Maximum scanning length reached. In this case there might actually be an edge at the 
  // focus which has not yet been detected, hence set the focus to the last point in the 
  // target class on the scanline that got further (same as all other return cases).
  int scannedFurther = detector[0].getLastIndexInClass()>detector[1].getLastIndexInClass() ? 0 : 1;
  detector[scannedFurther].getLastPointInClass(focus, pixelsScanned);
  return VLCGoalRecognizer::none;
}

bool VLCGoalRecognizer::detectGreenBelowGoalpost(Vector2<int> focus, const Vector2<double>& directionOfEdge, int goalpostHeight)
{
  // Calculate the number of horizontal lines to be scanned
  int scanLineCount = 1 + max(2, goalpostHeight/16);
  
  // Bresenham line for the scans
  // Scanning is done against the directionOfEdge, because this is likely to be the side
  // of the image border. This way we can skip pixels until the focus is back inside the
  // image.
  BresenhamLineScan scanLine(-directionOfEdge);
  focus.x += static_cast<int>(directionOfEdge.x * 5); // no need to round, doesn't need to be exact
  focus.y += static_cast<int>(directionOfEdge.y * 5);

  // Vertical between the line segments
  Vector2<int> verticalOffset(static_cast<int>(floor(horizonInfo.vertLine.direction.x*2+0.5)), 
                              static_cast<int>(floor(horizonInfo.vertLine.direction.y*2+0.5)));

  // Check short horizontal line segments for green pixels
  int greenCount = 0, pixelCount = 0;
  colorClass color;
  for (int i = 0; i < scanLineCount; ++i)
  {
    // Move focus downwards
    focus += verticalOffset;

    // Scan horizontally and count green pixels
    greenCount = pixelCount = 0;
    scanLine.init();
    Vector2<int> focus2 = focus;
    for (int j = 0; pixelCount < 5; ++j)
    {
      // Next scanline pixel
      scanLine.getNext(focus2);

      // Check if inside the image
      if (focus2.x >= 0 && focus2.y >= 0 && focus2.x <= horizonInfo.maxImageCoordinates.x && focus2.y <= horizonInfo.maxImageCoordinates.y)
      {
        ++pixelCount;

        // Check colour class
        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);
        if (color == green)
        {
          ++greenCount;
          STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus2.x, focus2.y, YELLOW);
        }
        else
        {
          STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, focus2.x, focus2.y, GRAY);
        }
      }
      else
      {
        // Skip this scanline if the image boundary wasn't reached within 8 pixels
        if (j>=8)
          break;
      }
    }

    // Check at least 3 (out of the scanned 5) pixels where green
    if (greenCount >= 3)
      return true;
  }

  // None of the scanline where sufficiently green so return false
  return false;
}

void VLCGoalRecognizer::lockArea(double x, double y, bool onGreen)
{
  // If the goal hypothesis is stading on the green floor, don't scan below it at all
  if (onGreen)
    y = 1024.0;

  // Pop locks that are well included in the new one. This way, a goal that is detected 
  // twice including the right goalpost (happens if the striker is close to the goal and 
  // the right goalpost scan fails to get to the bottom) results in two locks and hence
  // a lock to the bottom (2nd lock is always to the bottom)
  while (lockAreaCount>0)
  {
    if (x >= lockAreaStack[lockAreaCount-1].x && y >= lockAreaStack[lockAreaCount-1].y+8)
      // Pop lock
      --lockAreaCount;
    else
      break;
  }

  // Add lock to the stack
  if (lockAreaCount == lockAreaStackSize-1)
  {
    // Make the last lock go all the way to the bottom (and don't increase the counter)
    lockAreaStack[lockAreaCount].x = 1024.0;
    lockAreaStack[lockAreaCount].y = y;
  }
  else if (lockAreaCount < lockAreaStackSize-1)
  {
    // Add to the stack the usual way
    lockAreaStack[lockAreaCount].x = x;
    lockAreaStack[lockAreaCount].y = y;
    ++lockAreaCount;
  }
  //else doesn't happen
}

void VLCGoalRecognizer::calculateLockedPixels(const Vector2<int>& scanLineStart)
{
  // Convert to horizon-aligned coordinates
  Vector2<double> startH = horizonInfo.toHorizonAligned(scanLineStart);

  // Pop locks that are no longer relevant (because there will be no more pixel inspection
  // in these areas)
  while (lockAreaCount>0)
  {
    if (lockAreaStack[lockAreaCount-1].x < startH.x)
      // Pop lock
      --lockAreaCount;
    else
      break;
  }

  // Compute locked pixels
  lockedPixels = 0;
  if (lockAreaCount>0)
  {
    // Calculate the number of pixels that will be inspected until the locked area is 
    // left. This consists of the following steps (merged to be efficient)
    //  - calculate the vector from the scanline start point to the point where the 
    //    scanline leaves the locked area (in the horizon-aligned coordinate system);
    //    this vector has an x-value 0
    //  - if it points upwards, don't lock any pixels
    //  - transform it to image coordinates
    //  - calculate, how many pixels will be generated by BresenhamLineScan for this 
    //    line segment
    double segmentLength = lockAreaStack[lockAreaCount-1].y - startH.y;
    if (segmentLength <= 0)
      return;
    Vector2<int> segment = horizonInfo.toImageCoordinates(0.0, segmentLength);
    lockedPixels = max(abs(segment.x), abs(segment.y));
  }
}

void VLCGoalRecognizer::recalculateLockedPixels(const Vector2<int>& currentPoint)
{
  // Compute locked pixels (like in calculateLockedPixels)
  lockedPixels = 0;
  if (lockAreaCount>0)
  {
    double segmentLength = lockAreaStack[lockAreaCount-1].y - horizonInfo.horizonAlignedYOf(currentPoint);
    if (segmentLength <= 0)
      return;
    Vector2<int> segment = horizonInfo.toImageCoordinates(0.0, segmentLength);
    lockedPixels = max(abs(segment.x), abs(segment.y));
  }
}

void VLCGoalRecognizer::mergeFragments(bool* deleted)
{
  //TODO: make efficient (with pointers?)
  for (int i=0; i<maxHypothesises; ++i)
    deleted[i] = false;

  // Loop through all pairs of goals
  for (int i=0; i<hypothesisCount; ++i)
    if (!deleted[i])
    {
      for (int j=i+1; j<hypothesisCount; ++j)
        if (!deleted[j])
        {
          // Preconditions for a merge
          int volumeLeft  = hypothesis[i].height*hypothesis[i].width;
          int volumeRight = hypothesis[j].height*hypothesis[j].width;
          const int& heightLeft  = hypothesis[i].height;
          const int& heightRight = hypothesis[j].height;
          int verticalOffset = abs(static_cast<int>(hypothesis[j].goalpost[0].topPointH.y
                                                  - hypothesis[i].goalpost[1].topPointH.y));
          int horizontalGap = static_cast<int>(hypothesis[j].goalpost[0].edgePointH.x
                                             - hypothesis[i].goalpost[1].edgePointH.x);

          // Precondition: similar size
          bool sizeSimilar = volumeLeft*3>volumeRight && volumeRight*3>volumeLeft
                          || heightLeft*3>heightRight*2 && heightRight*3>heightLeft*2;

          if (sizeSimilar)
          {
            // Check the vertical offset between the corners
            bool verticalDifferenceAcceptable = verticalOffset < horizontalGap
                                             || verticalOffset < min(hypothesis[i].height, hypothesis[j].height);

            // Check that the horizontal gap is less than a third of the new
            // width. 
            int newWidth = static_cast<int>(hypothesis[j].goalpost[1].edgePointH.x
                                          - hypothesis[i].goalpost[0].edgePointH.x);
            
            // Reduce the gap if the the inner goalposts were not found at all.
            if (hypothesis[j].goalpost[0].nonExistant)
              horizontalGap -= 8;
            if (hypothesis[i].goalpost[1].nonExistant)
              horizontalGap -= 8;
  
            if (verticalDifferenceAcceptable
              && horizontalGap*3<newWidth)
            {
              // Merge the two hypothesises
              hypothesis[i].width = newWidth;
              hypothesis[i].height = max(hypothesis[i].height, hypothesis[j].height);
              hypothesis[i].onGreen = hypothesis[i].onGreen || hypothesis[j].onGreen;
              hypothesis[i].goalpost[1] = hypothesis[j].goalpost[1];
              deleted[j] = true;
            }
          }
        }
    }
}


void VLCGoalRecognizer::interpretResults(VLCGoalRecognizer::EdgeDetector* detector, bool* deleted)
{
  // Variable for selected hypothesis
  int selected = -1;

  // Variables for interpretation result
  bool visibleGoalpost[2];
  VLCGoalRecognizer::FreeSide freeSideOfGoal = noFreeSide;
  Vector2<int> edgeOfFreePart;

  for (int i = 0; i<hypothesisCount; i++)
  if (!deleted[i])
  {
    // Interpret goalposts
    bool possibleGoalPart[2];
    bool strongGoalpost[2];

    for (int side=0; side<2; ++side)
    {
      possibleGoalPart[side] = hypothesis[i].goalpost[side].nonExistant == false
                            && hypothesis[i].goalpost[side].height > minGoalpostHeight
                            && hypothesis[i].goalpost[side].height*4 > hypothesis[i].width
                            && hypothesis[i].goalpost[side].bottomPointH.y - horizonInfo.distanceTopLeftCorner > 0
                            && hypothesis[i].goalpost[side].weakEdgeCount <= hypothesis[i].goalpost[side].strongEdgeCount/8 + 1;
      visibleGoalpost[side] = possibleGoalPart[side] 
                            && hypothesis[i].goalpost[side].visibleHeight > 0
                            && hypothesis[i].goalpost[side].height*2 > hypothesis[i].height;
                            // hypothesis[i].goalpost[side].visibleHeight*2 > height
                            // hypothesis[i].goalpost[side].curvature < ??;
      strongGoalpost[side] = visibleGoalpost[side]
                            && hypothesis[i].goalpost[side].onGreen;
    }

    // Interpret goal
    bool minimalWidth = hypothesis[i].width > 10;
    bool reasonableWidth = hypothesis[i].width > 20;
    bool largeWidth = hypothesis[i].width > 100;
    bool reasonableHeight = hypothesis[i].height > 30;
    bool largeHeight = hypothesis[i].height > 50;
    //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":"'")));

    // Minimal prerequisites for a goal
    if ((  (strongGoalpost[0] || strongGoalpost[1])
           && (reasonableWidth || (reasonableHeight && minimalWidth)))
        || (possibleGoalPart[0] || possibleGoalPart[1])
           && largeWidth && largeHeight)
    {
      // Select this blob
      selected = i;

      // Determine free part of goal
      int freeWidth;
      freeSideOfGoal = detectFreePartOfGoal(detector, hypothesis[selected], hypothesis[selected].height, freeWidth, edgeOfFreePart);

      // Decide whether to the detected information
      if (visibleGoalpost[0] && visibleGoalpost[1])
      {
        // Free width must be at least a third of the goal
        if (freeWidth*3 < hypothesis[i].width)
          freeSideOfGoal = noFreeSide;
      }
      else
      {
        // Free width must be at least half of the height
        if (freeWidth*2 < hypothesis[i].height)
          freeSideOfGoal = noFreeSide;
      }

      break;
    }
  }

  // Draw
  for (int i = 0; i<hypothesisCount; i++)
  if (!deleted[i])
  {
    HORIZON_ALIGNED_RECTANGLE(horizonInfo,
    hypothesis[i].goalpost[0].edgePointH.x, min(min(hypothesis[i].goalpost[0].topPointH.y, hypothesis[i].goalpost[1].topPointH.y), hypothesis[i].crossBarEndpointH.y),
    hypothesis[i].goalpost[1].edgePointH.x, max(hypothesis[i].goalpost[0].bottomPointH.y, hypothesis[i].goalpost[1].bottomPointH.y),
    1, Drawings::ps_solid, i==selected ? Drawings::green : Drawings::red);
  }

  // No goal percept?
  if (-1 == selected)
    return;

  
  // Calculate angles to sides
  VLCGoalRecognizer::GoalHypothesis& h = hypothesis[selected];
  Vector2<double> top, bottom, left, right;

  Geometry::calculateAnglesForPoint((h.goalpost[0].topPoint+h.goalpost[1].topPoint)/2, interfaces.cameraMatrix, horizonInfo.previousCameraMatrix, interfaces.image.cameraInfo, top);
  if (h.goalpost[0].bottomPointH.y > h.goalpost[1].bottomPointH.y)
    Geometry::calculateAnglesForPoint(h.goalpost[0].bottomPoint, interfaces.cameraMatrix, horizonInfo.previousCameraMatrix, interfaces.image.cameraInfo, bottom);
  else
    Geometry::calculateAnglesForPoint(h.goalpost[1].bottomPoint, interfaces.cameraMatrix, horizonInfo.previousCameraMatrix, interfaces.image.cameraInfo, bottom);
  Geometry::calculateAnglesForPoint(h.goalpost[0].edgePoint, interfaces.cameraMatrix, horizonInfo.previousCameraMatrix, interfaces.image.cameraInfo, left);
  Geometry::calculateAnglesForPoint(h.goalpost[1].edgePoint, interfaces.cameraMatrix, horizonInfo.previousCameraMatrix, interfaces.image.cameraInfo, right);

  ConditionalBoundary boundary;
  boundary.addY(top.y, true);   // TODO: check if the true matters matters
  boundary.addY(bottom.y, true);
  boundary.addX(left.x, !visibleGoalpost[0]);
  boundary.addX(right.x, !visibleGoalpost[1]);

  // Calculate goal corners in the image
  double topHY = (h.goalpost[0].topPointH.y + h.goalpost[1].topPointH.y)/2;
  double bottomHY = max(h.goalpost[0].bottomPointH.y, h.goalpost[1].bottomPointH.y)/2;
  Vector2<int> topLeftCorner = horizonInfo.toImageCoordinates(h.goalpost[0].edgePointH.x, topHY);
  Vector2<int> bottomLeftCorner = horizonInfo.toImageCoordinates(h.goalpost[0].edgePointH.x, bottomHY);
  Vector2<int> bottomRightCorner = horizonInfo.toImageCoordinates(h.goalpost[1].edgePointH.x, bottomHY);
  Vector2<int> topRightCorner = horizonInfo.toImageCoordinates(h.goalpost[1].edgePointH.x, topHY);

  // Publish goal percept
  interfaces.landmarksPercept.addGoal(goalColor, boundary, topLeftCorner, topRightCorner, bottomLeftCorner, bottomRightCorner, interfaces.cameraMatrix); 


  // Free part of goal
  switch (freeSideOfGoal)
  {
  case bothSides:
    // Entire goal has been seen to be free
    interfaces.obstaclesPercept.setFreePartOfGoal(goalColor, left.x, right.x, true, true,
        horizonInfo.toImageCoordinates(h.goalpost[0].edgePointH.x, bottomHY),
        horizonInfo.toImageCoordinates(h.goalpost[1].edgePointH.x, bottomHY));
    break;

  case leftSide:
  case rightSide:
    {
      // Scans from the goalposts towards the midle found and obstacle (presumably 
      // the goaly) at the point endpointOfFreePart, hence the free part of the 
      // goal ranges from the left/right goalpost to this point.

      // Calculate the angle to the endpoint at the goalie
      Vector2<double> otherEndpointAngle;
      Geometry::calculateAnglesForPoint(edgeOfFreePart, interfaces.cameraMatrix, horizonInfo.previousCameraMatrix, interfaces.image.cameraInfo, otherEndpointAngle);

      // Calculate the HX coordinate of the second endpoint
      double otherEndpointHX = horizonInfo.horizonAlignedXOf(edgeOfFreePart);

      // Publish free part of goal
      if (freeSideOfGoal == leftSide)
        interfaces.obstaclesPercept.setFreePartOfGoal(goalColor, left.x, otherEndpointAngle.x, true, true, 
            horizonInfo.toImageCoordinates(h.goalpost[0].edgePointH.x, bottomHY),
            horizonInfo.toImageCoordinates(otherEndpointHX, bottomHY));
      else
        interfaces.obstaclesPercept.setFreePartOfGoal(goalColor, otherEndpointAngle.x, right.x, true, true,
            horizonInfo.toImageCoordinates(otherEndpointHX, bottomHY),
            horizonInfo.toImageCoordinates(h.goalpost[1].edgePointH.x, bottomHY));
    }
  }
}


VLCGoalRecognizer::FreeSide VLCGoalRecognizer::detectFreePartOfGoal(VLCGoalRecognizer::EdgeDetector* detector, 
                                    const VLCGoalRecognizer::GoalHypothesis& goal, 
                                    int goalHeight, 
                                    int& freeWidth, 
                                    Vector2<int>& otherSide)
{
  // If one of the goalpost has a significantly smaller height than the other one, it is 
  // assumed to be above the goalie, so scanning is only done from one goalpost. Otherwise
  // scan from both goalposts (but start further down on the shorter one).
  // NB: the following if treats the 2nd case first
  if (goal.goalpost[0].height*3 >= goalHeight*2 && goal.goalpost[1].height*2 >= goalHeight)
  {
    // Scanline endpoints
    Vector2<int> focus[2];

    for (int i = 0; i<2; ++i)
    {
      //-- Calculate scanline
      //if (goalpost[i].topPointVisible ...
      //else

      // Use the point half of the goal height below the top corners as endpoints
      Vector2<double> endpointH(goal.goalpost[i].edgePointH.x, 
                                goal.goalpost[i].topPointH.y + static_cast<double>(goalHeight)*0.75);
      focus[i] = horizonInfo.toImageCoordinates(endpointH);

      // Clip to image border
      horizonInfo.clipToImageBoundary(focus[i]);
    }

    // Offset between the scanlines (scanning is done twice like usual)
    Vector2<int> scanLineOffset(static_cast<int>(floor(horizonInfo.vertLine.direction.x*2.0+0.5)),
                                static_cast<int>(floor(horizonInfo.vertLine.direction.y*2.0+0.5)));

    // Scan lines
    BresenhamLineScan scanLine0(focus[0], focus[1]);
    BresenhamLineScan scanLine1(focus[1], focus[0]);


    //-- Look for the free part
    // Points where the scanning detected the end of the goal-colour. Presumably this is 
    // where the goalie stands.
    int goalieEdgeIndex[2] = {0, 0};

    // Scan left to right
    detector[0].clear(focus[0], false);
    detector[1].clear(focus[0], false);
    detector[0].setReferenceColor(goal.goalpost[0].color, 2);
    detector[1].setReferenceColor(goal.goalpost[0].color, 2);
    scanAlongLine(detector, focus[0], scanLine0, scanLineOffset, scanLine0.numberOfPixels+10, goalieEdgeIndex[0]);

    // See whether its worth to scan in the other direction at all
    if (goalieEdgeIndex[0]*8 < scanLine0.numberOfPixels*7)
    {
      // Scan right to left
      detector[0].clear(focus[1], false);
      detector[1].clear(focus[1], false);
      detector[0].setReferenceColor(goal.goalpost[1].color, 2);
      detector[1].setReferenceColor(goal.goalpost[1].color, 2);
      scanAlongLine(detector, focus[1], scanLine1, scanLineOffset, scanLine1.numberOfPixels+10-goalieEdgeIndex[0], goalieEdgeIndex[1]);
    }
    if (goalieEdgeIndex[0]+goalieEdgeIndex[1] >= scanLine1.numberOfPixels)
    {
      // The entire visible goal is free
      freeWidth = goal.width;
      return bothSides;
    }
    else if (goalieEdgeIndex[0] > goalieEdgeIndex[1])
    {
      // Calculate the width of the free part and return the edge point
      // Tricky calculation (TODO: explain)
      freeWidth = goalieEdgeIndex[0]*goal.width/scanLine0.numberOfPixels;
      otherSide = focus[0];
      return leftSide;
    }
    else
    {
      // Calculate the width of the free part and return the edge point
      freeWidth = goalieEdgeIndex[1]*goal.width/scanLine1.numberOfPixels;
      otherSide = focus[1];
      return rightSide;
    }
  }
  else
  {
    //-- Compute scanline
    // Only use the longer goalpost
    int longerGoalpost = goal.goalpost[0].height > goal.goalpost[1].height ? 0 : 1;

    // Start from the middle of the longer goalpost
    Vector2<double> endpointH(goal.goalpost[longerGoalpost].edgePointH.x,
                              goal.goalpost[longerGoalpost].topPointH.y + static_cast<int>(goalHeight)*0.75);
    Vector2<int> focus = horizonInfo.toImageCoordinates(endpointH);
    
    // Scan line parallel to the horizon
    BresenhamLineScan scanLine(longerGoalpost==0 ? horizonInfo.horizon.direction
                                                 : -horizonInfo.horizon.direction);

    // Offset between the scanlines (scanning is done twice like usual)
    Vector2<int> scanLineOffset(static_cast<int>(floor(horizonInfo.vertLine.direction.x*2.0+0.5)),
                                static_cast<int>(floor(horizonInfo.vertLine.direction.y*2.0+0.5)));

    
    //-- Scan to find free part of goal
        // Point where the scanning detected the end of the goal-colour. 
    int goalieEdgeIndex = 0;

    // Scan from the longer goalpost
    detector[0].clear(focus, false);
    detector[1].clear(focus, false);
    detector[0].setReferenceColor(goal.goalpost[longerGoalpost].color, 2);
    detector[1].setReferenceColor(goal.goalpost[longerGoalpost].color, 2);
    scanAlongLine(detector, focus, scanLine, scanLineOffset, scanLine.numberOfPixels+10, goalieEdgeIndex);

    // Calculate the width of the free part and return the edge point
    // WARNING: This uses the non-documented property that the direction vectors are
    // multiplied by 1024.0 for the conversion to int!!!
    freeWidth = goalieEdgeIndex*1024/scanLine.numberOfPixels;
    otherSide = focus;
    return longerGoalpost==0 ? leftSide : rightSide;
  }
}



/** Function for initialization of const member in EdgeDetector. */
Matrix3x3<int> inverseCovarianceMatrix2(colorClass goalColor)
{
  switch (goalColor)
  {
  case yellow:
    return Matrix3x3<int>(Vector3<int>(279,-89,240),
                              Vector3<int>(-89,1272,-82),
                              Vector3<int>(240,-82,586));
  case skyblue:
    return Matrix3x3<int>(Vector3<int>(186,19,-3)*2,
                              Vector3<int>(19,470,-29)*2,
                              Vector3<int>(-3,-29,257)*2);
  default:
    return Matrix3x3<int>();
  }
}

/** Constructor. */
VLCGoalRecognizer::EdgeDetector::EdgeDetector(colorClass target)
  : targetColorClass(target),
    ignoredClassification(target==skyblue ? blue : noColor),
    invCovar(inverseCovarianceMatrix2(target)),
    referenceColor(),
    referenceColorBase(0),
    deviationCounter(0),
    edgeCounter(0),
    lastPointInClass(),
    lastPointInClassIndex(-1),
    connectedToClass(false)
{
}

VLCGoalRecognizer::EdgeDetector::EdgeDetector(colorClass target, Vector3<int> referenceColor, int referenceColorBase)
  : targetColorClass(target),
    ignoredClassification(target==skyblue ? blue : noColor),
    invCovar(inverseCovarianceMatrix2(target)),
    referenceColor(referenceColor),
    referenceColorBase(referenceColorBase),
    deviationCounter(0),
    edgeCounter(0),
    lastPointInClass(),
    lastPointInClassIndex(-1),
    connectedToClass(false)
{
}

/** Thresholds for distances to reference colour. */
const float VLCGoalRecognizer::EdgeDetector::classThreshold = 300000;
const float VLCGoalRecognizer::EdgeDetector::edgeThreshold = 600000;

---