Modules/ImageProcessor/GT2004ImageProcessor/GT2004BeaconDetector.cpp

Go to the documentation of this file.

/**
* @file GT2004BeaconDetector.cpp
* 
* Implementation of class GT2004BeaconDetector.
*
* @author <a href="mailto:timlaue@tzi.de">Tim Laue</a>
* @author <a href="mailto:walter.nistico@uni-dortmund.de">Walter Nistico</a>
*/ 


#include "Representations/Perception/Image.h"
#ifdef CT32K_LAYOUT
#include "Representations/Perception/ColorTable32K.h"
#else
#include "Representations/Perception/ColorTable64.h"
#endif
#include "Representations/Perception/CameraMatrix.h"
#include "Representations/Perception/LandmarksPercept.h"
#include "Tools/FieldDimensions.h"
#include "Tools/RingBuffer.h"
#include "Tools/Math/Vector2.h"
#include "Modules/ImageProcessor/ImageProcessorTools/ColorCorrector.h"
#include "GT2004ImageProcessorTools.h"
#include "GT2004BeaconDetector.h"


const int Y(0),
          U(cameraResolutionWidth_ERS7),     /* Relative offset of U component. */
          V(2 * cameraResolutionWidth_ERS7); /* Relative offset of V component. */


GT2004BeaconDetector::GT2004BeaconDetector(const Image& image, 
                       const CameraMatrix& cameraMatrix,
                       const CameraMatrix& prevCameraMatrix,
                       const ImageInfo& imageInf,
                       const ColorTable& colorTable, 
                       const ColorCorrector& colorCorrector,
                       LandmarksPercept& landmarksPercept):
                       image(image), cameraMatrix(cameraMatrix), 
                       prevCameraMatrix(prevCameraMatrix),
                       imageInf(imageInf), colorTable(colorTable), 
                       colorCorrector(colorCorrector),
                       landmarksPercept(landmarksPercept),
                       flagSpecialist(colorCorrector),
                       horizontalBaseOffset(1.6), // was 1.5
                       numOfHorizontalScanLineAbove(10), 
                       numOfHorizontalScanLineBelow(3),
                       horizontalOffsetModifier(0.9),  // was 0.7
                       clusteringDistanceTolerance(6), 
                       minPinkRunLength(2), 
                       clusteringAspectRatio(1.5),
                       projectionAspectRatio(0.4),
                       minFlagConfidence(0.5),
                       edgeScanDepth(6),
                       edgeDetectionU(edgeThresholdU),
                       edgeDetectionV(edgeThresholdV)
{}


void GT2004BeaconDetector::execute()
{
  flagSpecialist.init(image);
  numOfBeaconCandidates = 0;
  double dist(horizontalBaseOffset);
  Geometry::Line scanLine;
  scanLine.direction = imageInf.horizon.direction;
  scanLine.base = imageInf.horizon.base;
  int i;
  for(i=0; i < numOfHorizontalScanLineBelow; i++)
  {
    scanLine.base += imageInf.vertLine.direction*dist;
    dist += horizontalOffsetModifier;
    Vector2<int> startPoint, endPoint;
    if(Geometry::getIntersectionPointsOfLineAndRectangle(
       Vector2<int>(0,0), imageInf.maxImageCoordinates, scanLine, startPoint, endPoint))
    {
      LINE(imageProcessor_general, startPoint.x, startPoint.y, endPoint.x, endPoint.y, 1, Drawings::ps_dash, Drawings::gray);
      scanForPink(startPoint, endPoint);
    }
    else
    {
      break;
    }
  }
  dist  = horizontalBaseOffset;
  scanLine.base = imageInf.horizon.base;
  for(i=0; i < numOfHorizontalScanLineAbove; i++)
  {
    scanLine.base -= imageInf.vertLine.direction*dist;
    dist += horizontalOffsetModifier;
    Vector2<int> startPoint, endPoint;
    if(Geometry::getIntersectionPointsOfLineAndRectangle(
       Vector2<int>(0,0), imageInf.maxImageCoordinates, scanLine, startPoint, endPoint))
    {
      LINE(imageProcessor_general, startPoint.x, startPoint.y, endPoint.x, endPoint.y, 1, Drawings::ps_dash, Drawings::yellow);
      scanForPink(startPoint, endPoint);
    }
    else
    {
      break;
    }
  }
  if(numOfBeaconCandidates)
  {
    clusterPinkBeaconParts();
  }
  flagSpecialist.getFlagPercept(cameraMatrix, prevCameraMatrix, image.cameraInfo, imageInf.horizon, landmarksPercept);
}


void GT2004BeaconDetector::scanForPink(const Vector2<int>& start, 
                                       const Vector2<int>& end)
{
  BresenhamLineScan bresenham(start, end);
  bresenham.init();
  Vector2<int> pixel(start), lastPixel(start);
  int Y(0),U(0),V(0),lastY(0),lastU(0),lastV(0);
  Run currentRun;
  bool isPinkRun(false);
  bool currentPixelPink(false);
  for(int i=0; i<bresenham.numberOfPixels; i++)
  {
    U = image.image[pixel.y][1][pixel.x];
    U = ColorCorrector::correct(pixel.x, pixel.y, 1, U);
    if(U > minPinkUValue)
    {
      Y = image.image[pixel.y][0][pixel.x];
      Y = ColorCorrector::correct(pixel.x, pixel.y, 0, Y);
      V = image.image[pixel.y][2][pixel.x];
      V = ColorCorrector::correct(pixel.x, pixel.y, 2, V);
      currentPixelPink = (COLOR_CLASS(Y,U,V) == pink);
    }
    else
    {
      currentPixelPink = false;
    }
    if(!isPinkRun)
    {
      if(currentPixelPink)
      {
        currentRun.scanLineStart = start;
        currentRun.start = pixel;
        currentRun.length = 1;
        isPinkRun = true;
      }
    }
    else //if(isPinkRun)
    {
      if(currentPixelPink)// || (COLOR_COMPONENT_DIST(U,lastU) < params.maxPinkUContrast))
      {
        currentRun.length++;
        lastY = Y;
        lastU = U;
        lastV = V;
      }
      else
      {
        isPinkRun = false;
        if(currentRun.length >= minPinkRunLength)
        {
          currentRun.end = lastPixel;
          if(!addCandidate(currentRun)) break; //List is full, stop searching
          LINE(imageProcessor_general, currentRun.start.x, currentRun.start.y, 
               currentRun.end.x, currentRun.end.y, 1, Drawings::ps_solid, Drawings::pink);
        }
      }
    }    
    //Compute next position according to Bresenham algorithm
    lastPixel = pixel;
    bresenham.getNext(pixel);
  }
  //Finish the last Run
  if(isPinkRun)
  {
    currentRun.end = pixel;
    addCandidate(currentRun);
    LINE(imageProcessor_general, currentRun.start.x, currentRun.start.y, 
               currentRun.end.x, currentRun.end.y, 1, Drawings::ps_solid, Drawings::pink);
  }
}


bool GT2004BeaconDetector::addCandidate(const Run& pinkRun)
{
  bool returnValue(numOfBeaconCandidates<MAX_NUMBER_OF_PINK_RUNS);
  if(returnValue)
  {
    if((numOfBeaconCandidates>0) &&
       (pinkRun.scanLineStart == beaconCandidates[numOfBeaconCandidates-1].scanLineStart) &&
       ((beaconCandidates[numOfBeaconCandidates-1].end - pinkRun.start).abs()<4))
    {
      beaconCandidates[numOfBeaconCandidates-1].end = pinkRun.end;
    }
    else
    {
      beaconCandidates[numOfBeaconCandidates++] = pinkRun;
    }
  }
  return returnValue;
}


void GT2004BeaconDetector::clusterPinkBeaconParts()
{
  //Transform pink runs
  double rotation(imageInf.horizon.direction.angle());
  double ca(cos(-rotation));
  double sa(sin(-rotation));
  Matrix2x2<double> rotMat(Vector2<double>(ca,sa),Vector2<double>(-sa,ca));
  Matrix2x2<double> invRotMat = rotMat.transpose(); //=rotMat.invert(), orthonormal matrix
  for(int i=0; i<numOfBeaconCandidates; i++)
  {
    transformedCandidates[i].transform(beaconCandidates[i], rotMat, i);
  }
  //Sort by starting position from left to right
  for(int j=0; j<numOfBeaconCandidates; j++)
  {
    int leftest(j);
    for(int k=j; k<numOfBeaconCandidates; k++)
    {
      if(transformedCandidates[k].start.x<transformedCandidates[leftest].start.x)
      {
        leftest = k;
      }
    }
    if(leftest != j)
    {
      TransformedRun help(transformedCandidates[j]);
      transformedCandidates[j] = transformedCandidates[leftest];
      transformedCandidates[leftest] = help;
    }
  }
  //Find overlapping runs
  int beginOfBeacon(0), endOfBeacon(0);
  double xRight(transformedCandidates[0].end.x);
  double xLeft(transformedCandidates[0].start.x);
  double yRight(transformedCandidates[0].end.y);
  double yLeft(transformedCandidates[0].start.y);
  double runLength(xRight-xLeft);
  double yMax, yMin;
  double originMinY, originMaxY;
  if (yRight > yLeft)
  {
    yMax = yRight;
    yMin = yLeft;
  }
  else 
  {
    yMax = yLeft;
    yMin = yRight;
  }
  originMinY = beaconCandidates[0].scanLineStart.y;
  originMaxY = beaconCandidates[0].scanLineStart.y;
  for(int l=1; l<numOfBeaconCandidates; l++)
  {
    if(transformedCandidates[l].start.x < xRight  + (double)clusteringDistanceTolerance)
    {// within merging distance
      if(transformedCandidates[l].end.x > xRight)
      { // new piece could extend the run
        double candidateLength = transformedCandidates[l].end.x-xLeft;
        bool newRunHigher = (transformedCandidates[l].end.y-yLeft > 0.0);
        double candidateHeight = newRunHigher ? (transformedCandidates[l].end.y-yLeft) : (yLeft-transformedCandidates[l].end.y);
        if (candidateHeight < candidateLength*clusteringAspectRatio) // the proportions of the landmark are consistent (1:1 ideally)
        {
          xRight = transformedCandidates[l].end.x;
          yRight = transformedCandidates[l].end.y;
          endOfBeacon = l;
          if (transformedCandidates[l].end.y-yMax > 0.0)
          {
            yMax = transformedCandidates[l].end.y;
            originMaxY = beaconCandidates[l].scanLineStart.y;
          }
          else if (yMin - transformedCandidates[l].end.y > 0.0)
          {
            yMin = transformedCandidates[l].end.y;
            originMinY = beaconCandidates[l].scanLineStart.y;
          }
        }
        else 
        { // we have to choose one run, the other is perhaps a false positive 
          double newLength = transformedCandidates[l].end.x - transformedCandidates[l].start.x;
          if (newLength > runLength) // the new run wins if longest
          {
            xRight = transformedCandidates[l].end.x;
            xLeft = transformedCandidates[l].start.x;
            yRight = transformedCandidates[l].end.y;
            yLeft = transformedCandidates[l].start.y;
            endOfBeacon = l;
            beginOfBeacon = l;
            runLength = newLength;
            if (yRight > yLeft)
            {
              yMax = yRight;
              yMin = yLeft;
            }
            else 
            {
              yMax = yLeft;
              yMin = yRight;
            }
            originMinY = beaconCandidates[l].scanLineStart.y;
            originMaxY = beaconCandidates[l].scanLineStart.y;
          }
        }
      }
      else
      { // the new run is horizontally contained 
        double newRunHeight = (transformedCandidates[l].end.y + transformedCandidates[l].start.y)/2.0;
        bool candidateMax = (newRunHeight-yMax > 0.0);
        bool candidateMin = (yMin - newRunHeight > 0.0);
        if (candidateMax)
        {
          double candidateHeight = newRunHeight - yMin;
          double candidateLength = xRight-xLeft;
          if (candidateHeight < candidateLength*clusteringAspectRatio) // the proportions of the landmark are consistent (1:1 ideally)
          {
            yMax = newRunHeight;
            originMaxY = beaconCandidates[l].scanLineStart.y;
          }        
        }
        else if (candidateMin)
        {
          double candidateHeight = yMax - newRunHeight;
          double candidateLength = xRight-xLeft;
          if (candidateHeight < candidateLength*clusteringAspectRatio) // the proportions of the landmark are consistent (1:1 ideally)
          {
            yMin = newRunHeight;
            originMinY = beaconCandidates[l].scanLineStart.y;
          }        
        }
      }
    }
    else
    {
      double originX = (beaconCandidates[beginOfBeacon].scanLineStart.x + beaconCandidates[endOfBeacon].scanLineStart.x)/2.0;
      //~ double originY = (beaconCandidates[beginOfBeacon].scanLineStart.y + beaconCandidates[endOfBeacon].scanLineStart.y)/2.0;
      double originY = (originMinY + originMaxY)/2;
      double mergedScanLineLength = xRight-xLeft;
      Vector2<double> leftOfBeacon, rightOfBeacon;
      leftOfBeacon.x = xLeft - originX;
      rightOfBeacon.x = xRight - originX;
      leftOfBeacon.y = (yMax+yMin)/2.0 - originY;  
      rightOfBeacon.y = leftOfBeacon.y;
      leftOfBeacon = (invRotMat*leftOfBeacon);
      rightOfBeacon = (invRotMat*rightOfBeacon);
      leftOfBeacon.x += originX;
      leftOfBeacon.y += originY;
      rightOfBeacon.x += originX;
      rightOfBeacon.y += originY;
      LINE(imageProcessor_general, int(leftOfBeacon.x+0.5), int(leftOfBeacon.y+0.5), 
            int(rightOfBeacon.x+0.5), int(rightOfBeacon.y+0.5), 1, Drawings::ps_dash, Drawings::red);
      analyzeBeacon(leftOfBeacon, mergedScanLineLength);
      beginOfBeacon = l;
      endOfBeacon = l;
      xRight = transformedCandidates[l].end.x;
      yRight = transformedCandidates[l].end.y;
      xLeft = transformedCandidates[l].start.x;
      yLeft = transformedCandidates[l].start.y;
      if (yRight > yLeft)
      {
        yMax = yRight;
        yMin = yLeft;
      }
      else 
      {
        yMax = yLeft;
        yMin = yRight;
      }
      originMinY = beaconCandidates[l].scanLineStart.y;
      originMaxY = beaconCandidates[l].scanLineStart.y;
    }
  }
  double originX = (beaconCandidates[beginOfBeacon].scanLineStart.x + beaconCandidates[endOfBeacon].scanLineStart.x)/2.0;
  //~ double originY = (beaconCandidates[beginOfBeacon].scanLineStart.y + beaconCandidates[endOfBeacon].scanLineStart.y)/2.0;
  double originY = (originMinY + originMaxY)/2;
  double mergedScanLineLength = xRight-xLeft;
  Vector2<double> leftOfBeacon, rightOfBeacon;
  leftOfBeacon.x = xLeft - originX;
  rightOfBeacon.x = xRight - originX;
  leftOfBeacon.y = (yMax+yMin)/2.0 - originY;  
  rightOfBeacon.y = leftOfBeacon.y;
  leftOfBeacon = (invRotMat*leftOfBeacon);
  rightOfBeacon = (invRotMat*rightOfBeacon);
  leftOfBeacon.x += originX;
  leftOfBeacon.y += originY;
  rightOfBeacon.x += originX;
  rightOfBeacon.y += originY;
  LINE(imageProcessor_general, int(leftOfBeacon.x+0.5), int(leftOfBeacon.y+0.5), 
        int(rightOfBeacon.x+0.5), int(rightOfBeacon.y+0.5), 1, Drawings::ps_dash, Drawings::red);
  analyzeBeacon(leftOfBeacon, mergedScanLineLength);
}

void GT2004BeaconDetector::analyzeBeacon(const Vector2<double>& left, const double pinkRunWidth)
{
  if (pinkRunWidth >= 3.0)
  {
    double rotation(imageInf.horizon.direction.angle());
    double ca(cos(rotation));
    double sa(sin(rotation));
    Matrix2x2<double> rotMat(Vector2<double>(ca,sa),Vector2<double>(-sa,ca));
    int flagReliability[4] = {0, 0, 0, 0}; 
    Flag::FlagType flagFound;
    Vector2<int> topEdge[4];
    Vector2<int> bottomEdge[4];
    int i;
    int totalWeight = 0;
    int highestReliability = 0;
    int mostReliableFlag = -1;
    for (i=0; i<4; i++)
    {
      topEdge[i].x = 0;
      topEdge[i].y = 0;
      bottomEdge[i].x = 0;
      bottomEdge[i].y = 0;
    }
    Vector2<double> currentPos;
    Vector2<double> step;
    int numberOfScanCol;
    if (pinkRunWidth <= 8.0)
    { // only 3 scan lines
      Vector2<double> displacement((pinkRunWidth-3.0)/2.0, 0); //1 pixel inner than theoretical border
      Vector2<double> init(1.0, 0.0);
      displacement = rotMat*displacement;
      init = rotMat*init;
      step = displacement;
      currentPos = left+init;
      numberOfScanCol = 3;
    }
    else
    {
      Vector2<double> displacement((pinkRunWidth-5.0)/3.0, 0); //2 pixels inner than theoretical border
      Vector2<double> init(2.0, 0.0);
      displacement = rotMat*displacement;
      init = rotMat*init;
      step = displacement;
      currentPos = left+init;
      numberOfScanCol = 4;
    }
    for (i=0; i<numberOfScanCol; i++)
    {
      Vector2<int> top, bottom;
      int reliability = scanForBeaconEdges(Vector2<int>(int(currentPos.x+0.5), int(currentPos.y+0.5)), 
            pinkRunWidth, flagFound, top, bottom);
      if (reliability != 0)
      {
        flagReliability[(int)flagFound] += reliability;
        topEdge[(int)flagFound] += top;
        bottomEdge[(int)flagFound] += bottom;
      }
      currentPos += step;
    }
    for (i=0; i<4; i++)
    {
      totalWeight += flagReliability[i];
      if (flagReliability[i] > highestReliability)
      {
        highestReliability = flagReliability[i];
        mostReliableFlag = i;
      }
    }
    if (mostReliableFlag != -1)
    {
      double confidence = highestReliability/totalWeight;
      if (confidence >= minFlagConfidence)
      {
        int posX = (int)((topEdge[mostReliableFlag].x + bottomEdge[mostReliableFlag].x)/(2*flagReliability[mostReliableFlag]) + 0.5);
        int posY = (int)((topEdge[mostReliableFlag].y + bottomEdge[mostReliableFlag].y)/(2*flagReliability[mostReliableFlag]) + 0.5);
        Vector2<int> center(posX, posY);
        const Vector2<int> border(0,0);
        Geometry::clipPointInsideRectange(border, imageInf.maxImageCoordinates-border, center);
        DOT(imageProcessor_ball4, center.x, center.y, Drawings::black, Drawings::blue);
        switch ((Flag::FlagType)mostReliableFlag)
        {
          case Flag::pinkAboveYellow:
              flagSpecialist.searchFlags(image, colorTable, cameraMatrix, yellow, true, imageInf.horizon, center.x, center.y);
              break;
          case Flag::pinkAboveSkyblue:
              flagSpecialist.searchFlags(image, colorTable, cameraMatrix, skyblue, true, imageInf.horizon, center.x, center.y);
              break;
          case Flag::yellowAbovePink:
              flagSpecialist.searchFlags(image, colorTable, cameraMatrix, yellow, false, imageInf.horizon, center.x, center.y);
              break;
          case Flag::skyblueAbovePink:
              flagSpecialist.searchFlags(image, colorTable, cameraMatrix, skyblue, false, imageInf.horizon, center.x, center.y);
              break;
        }
      }
    }
  }
}

int GT2004BeaconDetector::scanForBeaconEdges(const Vector2<int>& position, const double pinkRunWidth, 
        Flag::FlagType& flagType, Vector2<int>& topEdge, Vector2<int>& bottomEdge)
{
  Vector2<int> beaconCenter(position);
  Vector2<int> start, end;
  Geometry::Line vertLine;
  vertLine.direction = imageInf.vertLine.direction;
  vertLine.base.x = (double)beaconCenter.x;
  vertLine.base.y = (double)beaconCenter.y;
  Geometry::getIntersectionPointsOfLineAndRectangle(Vector2<int>(1,1), // 1 pixel wide border has to be discarded
              imageInf.maxImageCoordinates-Vector2<int>(1,1), vertLine, start, end); // due to the edge detection convolution mask
  Vector2<int> pos, pos2, pinkBottomEdge, pinkTopEdge, colorBottomEdge, colorTopEdge;
  colorClass bottomColor(noColor),topColor(noColor), baseColor(noColor), dontCare(noColor);
  bool topColorEdgeFound(false), pinkTopFound(false);
  bool unreliable(false); // this flag signals that an unsafe beacon hypothesis has been accepted
  Flag::FlagType beaconType(Flag::pinkAboveYellow);
  int reliability;
  // Scan to bottom of pink beacon part
  if(scanForBeaconPart(beaconCenter, end, pos, pinkBottomEdge, bottomColor))
  {
    if(bottomColor == white || bottomColor == gray)
    {
      // Scan to top of pink beacon part
      if(scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor))
      {
        if(topColor == skyblue)
        {
          beaconType = Flag::skyblueAbovePink;
          topColorEdgeFound = scanForBeaconPart(pos, start, pos2, colorTopEdge, dontCare);
        }
        else if(topColor == yellow)
        {
          beaconType = Flag::yellowAbovePink;
          topColorEdgeFound = scanForBeaconPart(pos, start, pos2, colorTopEdge, dontCare);
        }
        else
        {
          return 0; // unknown color above pink
        }
      }
      else
      {
        return 0; // pink part is at the upper border of the image
      }
    }
    else if((bottomColor == skyblue) || (bottomColor == yellow))
    {// Scan to bottom of color part
      if(scanForBeaconPart(pos, end, pos2, colorBottomEdge, baseColor))
      {
        if(baseColor == white || baseColor == gray)
        {
          if(bottomColor == skyblue)
          {
            beaconType = Flag::pinkAboveSkyblue;
            pinkTopFound = scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor);
          }
          else
          {
            beaconType = Flag::pinkAboveYellow;
            pinkTopFound = scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor);
          }
        }
        else
        { // perhaps we missed the real end of the colored part
          if (scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor))
          { // estimate colorBottomEdge from pinkTopEdge
            Vector2<int> toPink(pinkTopEdge - pinkBottomEdge);
            double distanceToPink = toPink.abs();
            double similarityPink;
            if (distanceToPink > pinkRunWidth)
              similarityPink = (pinkRunWidth/distanceToPink);
            else 
              similarityPink = (distanceToPink/pinkRunWidth);
            if (similarityPink > projectionAspectRatio) // make sure that this proposed edge is compatible with the aspect ratio
            {
              colorBottomEdge = pinkBottomEdge + (pinkBottomEdge - pinkTopEdge);
              pinkTopFound = true;
              if(bottomColor == skyblue)
                beaconType = Flag::pinkAboveSkyblue;
              else
                beaconType = Flag::pinkAboveYellow;
            }
            else
              return 0;
          }
          else
            return 0; // unable to determine top edge of pink part
        }
      }
      else
      { // we were unable to find the end of the colored part (no contrast?), and reached the border of the image
        // before giving up, let's see if we can at least find the top edge of the pink part
        if (scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor))
        { // estimate colorBottomEdge from pinkTopEdge
          Vector2<int> toPink(pinkTopEdge - pinkBottomEdge);
          double distanceToPink = toPink.abs();
          double similarityPink;
          if (distanceToPink > pinkRunWidth)
            similarityPink = (pinkRunWidth/distanceToPink);
          else 
            similarityPink = (distanceToPink/pinkRunWidth);
          if (similarityPink > projectionAspectRatio) // make sure that this proposed edge is compatible with the aspect ratio
          {
            colorBottomEdge = pinkBottomEdge + (pinkBottomEdge - pinkTopEdge);
            pinkTopFound = true;
            if(bottomColor == skyblue)
              beaconType = Flag::pinkAboveSkyblue;
            else
              beaconType = Flag::pinkAboveYellow;
          }
          else
            return 0;
        }
        else
          return 0; // unable to determine top edge of pink part
      }
    }
    else
    {
      return 0; // unknown color below pink
    }
  }
  else // unable to determine bottom edge (no gradient, or border of the image touched)
  { // so. try to scan to top
    if(scanForBeaconPart(beaconCenter, start, pos, pinkTopEdge, topColor))
    {
      DOT(imageProcessor_ball4, pinkTopEdge.x, pinkTopEdge.y, Drawings::black, Drawings::pink);
      if(topColor == skyblue)
      {
        beaconType = Flag::skyblueAbovePink;
        topColorEdgeFound = scanForBeaconPart(pos, start, pos2, colorTopEdge, dontCare);
        if (!topColorEdgeFound)
          return 0; // unable to calculate the vertical edges of the landmark
        DOT(imageProcessor_ball4, colorTopEdge.x, colorTopEdge.y, Drawings::black, Drawings::red);
      }
      else if(topColor == yellow)
      {
        beaconType = Flag::yellowAbovePink;
        topColorEdgeFound = scanForBeaconPart(pos, start, pos2, colorTopEdge, dontCare);
        if (!topColorEdgeFound)
          return 0; // unable to calculate the vertical edges of the landmark
        DOT(imageProcessor_ball4, colorTopEdge.x, colorTopEdge.y, Drawings::black, Drawings::red);
      }
      else // last attempt, try to use scan width information and top edge to make an hypothesis on bottom edge,
      { // check if this holds by looking at the color underlying the projected edge (should be yellow or skyblue)
        Vector2<double> projectedLenght(0, pinkRunWidth+3.0); //3 pixel more to make sure we jump over the pink/color edge
        double rotation(imageInf.horizon.direction.angle());
        double ca(cos(rotation));
        double sa(sin(rotation));
        Matrix2x2<double> rotMat(Vector2<double>(ca,sa),Vector2<double>(-sa,ca));
        Vector2<double> topToBottomDisplacement = rotMat*projectedLenght;
        Vector2<int> displacement((int)(topToBottomDisplacement.x+0.5), (int)(topToBottomDisplacement.y+0.5)); 
        pinkBottomEdge = pinkTopEdge + displacement;
        const Vector2<int> border(0,0);
        Geometry::clipPointInsideRectange(border, imageInf.maxImageCoordinates-border, pinkBottomEdge);
        unsigned char colorY(ColorCorrector::correct(pinkBottomEdge.x,pinkBottomEdge.y,0,image.image[pinkBottomEdge.y][0][pinkBottomEdge.x]));
        unsigned char colorU(ColorCorrector::correct(pinkBottomEdge.x,pinkBottomEdge.y,1,image.image[pinkBottomEdge.y][1][pinkBottomEdge.x]));
        unsigned char colorV(ColorCorrector::correct(pinkBottomEdge.x,pinkBottomEdge.y,2,image.image[pinkBottomEdge.y][2][pinkBottomEdge.x]));
        bottomColor = COLOR_CLASS(colorY,colorU,colorV);
        DOT(imageProcessor_ball4, pinkBottomEdge.x, pinkBottomEdge.y, Drawings::gray, ColorClasses::colorClassToDrawingsColor(bottomColor));
        if((bottomColor == skyblue) || (bottomColor == yellow))        
        {
          colorBottomEdge = pinkBottomEdge + displacement;
          Geometry::clipPointInsideRectange(border, imageInf.maxImageCoordinates-border, colorBottomEdge);
          colorY = ColorCorrector::correct(colorBottomEdge.x,colorBottomEdge.y,0,image.image[colorBottomEdge.y][0][colorBottomEdge.x]);
          colorU = ColorCorrector::correct(colorBottomEdge.x,colorBottomEdge.y,1,image.image[colorBottomEdge.y][1][colorBottomEdge.x]);
          colorV = ColorCorrector::correct(colorBottomEdge.x,colorBottomEdge.y,2,image.image[colorBottomEdge.y][2][colorBottomEdge.x]);
          baseColor = COLOR_CLASS(colorY,colorU,colorV);
          DOT(imageProcessor_ball4, colorBottomEdge.x, colorBottomEdge.y, Drawings::white, ColorClasses::colorClassToDrawingsColor(baseColor));
          if (baseColor == white || baseColor == gray)
          {
            if (bottomColor == skyblue)
            {
              beaconType = Flag::pinkAboveSkyblue;
              pinkTopFound = true;
              unreliable = true;
            }
            else if (bottomColor == yellow)
            {
              beaconType = Flag::pinkAboveYellow;
              pinkTopFound = true;
              unreliable = true;
            }
            else
              return 0;
          }
          else if (baseColor == pink) // we could have missed the middle edge of a landmark whose lower part is pink
          {
            if (bottomColor == skyblue) 
            {
              beaconType = Flag::skyblueAbovePink;
              pinkTopEdge = pinkBottomEdge; // we have to swap up and down, because it seems 
              pinkBottomEdge = colorBottomEdge; //that the assumption pinkAboveSkyblue was wrong
              topColor = skyblue;
              bottomColor = pink;
              unreliable = true;
            }
            else if (bottomColor == yellow)
            {
              beaconType = Flag::yellowAbovePink;
              pinkTopEdge = pinkBottomEdge; // we have to swap up and down, because it seems 
              pinkBottomEdge = colorBottomEdge; //that the assumption pinkAboveSkyblue was wrong
              topColor = yellow;
              bottomColor = pink;
              unreliable = true;
            }
            else
              return 0;
          }
          else
            return 0;
        }
        else
        {
          return 0; // unknown color above pink
        }
      }
    }
    else
    {
      return 0;
    }
  }

  //~ DOT(imageProcessor_ball4, pos.x, pos.y, Drawings::white, Drawings::black);
  //~ DOT(imageProcessor_ball4, pos2.x, pos2.y, Drawings::black, Drawings::gray);
  
  if(beaconType == Flag::pinkAboveYellow || beaconType == Flag::pinkAboveSkyblue)
  {
    if(pinkTopFound)
    {
      //~ DOT(imageProcessor_ball4, pinkBottomEdge.x, pinkBottomEdge.y, Drawings::black, Drawings::green);
      //~ DOT(imageProcessor_ball4, pinkTopEdge.x, pinkTopEdge.y, Drawings::white, Drawings::black);
      //~ DOT(imageProcessor_ball4, colorBottomEdge.x, colorBottomEdge.y, Drawings::black, Drawings::white);  
      Vector2<int> topPosition(pinkBottomEdge);
      Vector2<int> toColor(pinkBottomEdge - colorBottomEdge);
      Vector2<int> toPink(pinkTopEdge - pinkBottomEdge);
      double distanceToColor = toColor.abs();
      double distanceToPink = toPink.abs();
      double similarityColor, similarityPink;
      if (distanceToColor > pinkRunWidth)
        similarityColor = (pinkRunWidth/distanceToColor);
      else
        similarityColor = (distanceToColor/pinkRunWidth);
      if (distanceToPink > pinkRunWidth)
        similarityPink = (pinkRunWidth/distanceToPink);
      else
        similarityPink = (distanceToPink/pinkRunWidth);
      if (similarityColor > similarityPink)
        topPosition += toColor;
      else
        topPosition += toPink;
      //~ DOT(imageProcessor_ball4, topPosition.x, topPosition.y, Drawings::black, Drawings::gray);
      //~ DOT(imageProcessor_ball4, pinkBottomEdge.x, topPosition.y+(pinkBottomEdge.y-topPosition.y)/2, Drawings::black, Drawings::blue);
      if (unreliable)
        reliability = lowReliability;
      else 
        reliability = highReliability;
      flagType = beaconType;
      topEdge = topPosition*reliability;
      bottomEdge = pinkBottomEdge*reliability;
      return reliability;
    }
    else
    {
      // compute a theoretical position of the pink top edge:
      Vector2<int> topPosition(pinkBottomEdge);
      Vector2<int> relVec(pinkBottomEdge - colorBottomEdge);
      topPosition += relVec;
      //~ DOT(imageProcessor_ball4, pinkBottomEdge.x, topPosition.y+(pinkBottomEdge.y-topPosition.y)/2, Drawings::black, Drawings::blue);
      //~ DOT(imageProcessor_ball4, pinkBottomEdge.x, pinkBottomEdge.y, Drawings::black, Drawings::red);
      //~ DOT(imageProcessor_ball4, topPosition.x, topPosition.y, Drawings::white, Drawings::black);
      reliability = mediumReliability;
      flagType = beaconType;
      topEdge = topPosition*reliability;
      bottomEdge = pinkBottomEdge*reliability;
      return reliability;
    }
  }
  else
  { // pink is bottom
    if (bottomColor == noColor && topColorEdgeFound) // no pink bottom edge found
    { //theoretical position will be calculated from color top edge
      Vector2<int> bottomPosition(pinkTopEdge);
      Vector2<int> relVec(pinkTopEdge - colorTopEdge);
      bottomPosition += relVec;
      flagSpecialist.searchFlags(image, colorTable, cameraMatrix, topColor, false, 
          imageInf.horizon, (int)((pinkTopEdge.x+bottomPosition.x+1.0)/2), (int)((pinkTopEdge.y+bottomPosition.y+1.0)/2));
      DOT(imageProcessor_ball4, pinkTopEdge.x, pinkTopEdge.y, Drawings::white, Drawings::black);
      DOT(imageProcessor_ball4, bottomPosition.x, bottomPosition.y, Drawings::black, Drawings::yellow);
      DOT(imageProcessor_ball4, (int)((pinkTopEdge.x+bottomPosition.x+1.0)/2), (int)((pinkTopEdge.y+bottomPosition.y+1.0)/2), Drawings::black, Drawings::blue);
      reliability = mediumReliability;
      flagType = beaconType;
      topEdge = pinkTopEdge*reliability;
      bottomEdge = bottomPosition*reliability;
      return reliability;
    }
    else{
      Vector2<int> bottomPosition;
      if (topColorEdgeFound) //color edge information used just for redundancy check
      {
        bottomPosition = pinkTopEdge;
        Vector2<int> toColor(pinkTopEdge - colorTopEdge);
        Vector2<int> toPink(pinkBottomEdge - pinkTopEdge);
        double distanceToColor = toColor.abs();
        double distanceToPink = toPink.abs();
        double similarityColor, similarityPink;
        if (distanceToColor > pinkRunWidth)
          similarityColor = (pinkRunWidth/distanceToColor);
        else
          similarityColor = (distanceToColor/pinkRunWidth);
        if (distanceToPink > pinkRunWidth)
          similarityPink = (pinkRunWidth/distanceToPink);
        else
          similarityPink = (distanceToPink/pinkRunWidth);
        if (similarityColor > similarityPink)
          bottomPosition += toColor;
        else
          bottomPosition += toPink;
        if (unreliable)
          reliability = lowReliability;
        else
          reliability = highReliability;
        flagType = beaconType;
        topEdge = pinkTopEdge*reliability;
        bottomEdge = bottomPosition*reliability;
        return reliability;
      }
      else
      {
        if (unreliable)
          reliability = lowReliability;
        else
          reliability = mediumReliability;
        flagType = beaconType;
        topEdge = pinkTopEdge*reliability;
        bottomEdge = pinkBottomEdge*reliability;
        return reliability;
      }
    }
  }
  return true;
}


bool GT2004BeaconDetector::scanForBeaconPart(const Vector2<int>& start, const Vector2<int>& end,
                                             Vector2<int>& position, Vector2<int>& edge, colorClass& color)
{
  // DOT(imageProcessor_ball4, start.x, start.y, Drawings::blue, Drawings::orange);
  // DOT(imageProcessor_ball4, end.x, end.y, Drawings::yellow, Drawings::white);
  BresenhamLineScan bresenham(start, end);
  bresenham.init();
  Vector2<int> pixel(start);
  for(int i=0; i<bresenham.numberOfPixels; i++)
  {
    // bool edgeY = edgeDetectionY.isEdgePoint(image, pixel.x, pixel.y, SUSANEdgeDetectionLite::componentA);
    bool edgeU = edgeDetectionU.isEdgePoint(image, pixel.x, pixel.y, SUSANEdgeDetectionLite::componentB);
    bool edgeV = edgeDetectionV.isEdgePoint(image, pixel.x, pixel.y, SUSANEdgeDetectionLite::componentC);
    DOT(imageProcessor_ball4, pixel.x, pixel.y, Drawings::black, Drawings::orange);
  
    if (edgeU || edgeV)
    {
      position = pixel;
      edge = pixel;
      color = noColor;
      unsigned char colorY(ColorCorrector::correct(edge.x,edge.y,0,image.image[edge.y][0][edge.x]));
      unsigned char colorU(ColorCorrector::correct(edge.x,edge.y,1,image.image[edge.y][1][edge.x]));
      unsigned char colorV(ColorCorrector::correct(edge.x,edge.y,2,image.image[edge.y][2][edge.x]));
      colorClass edgeColor = COLOR_CLASS(colorY,colorU,colorV);
      color = edgeColor;
      int counter;
      for (counter = 0; counter < edgeScanDepth && 
            ((color == noColor)||(color==edgeColor)||(color==orange)||edgeU||edgeV); counter++)
      {
        bresenham.getNext(position);
        unsigned char colorY(ColorCorrector::correct(position.x,position.y,0,image.image[position.y][0][position.x]));
        unsigned char colorU(ColorCorrector::correct(position.x,position.y,1,image.image[position.y][1][position.x]));
        unsigned char colorV(ColorCorrector::correct(position.x,position.y,2,image.image[position.y][2][position.x]));
        color = COLOR_CLASS(colorY,colorU,colorV);
        edgeU = edgeDetectionU.isEdgePoint(image, position.x, position.y, SUSANEdgeDetectionLite::componentB);
        edgeV = edgeDetectionV.isEdgePoint(image, position.x, position.y, SUSANEdgeDetectionLite::componentC);
      }
      if (counter != edgeScanDepth) 
        return true;
    }
    //Compute next position according to Bresenham algorithm
    bresenham.getNext(pixel);
  }
  return 0;
}

void GT2004BeaconDetector::analyzeColorTable()
{
  Vector3<int> nearPoint,farPoint;
#ifdef CT32K_LAYOUT
  ((ColorTable32K&) colorTable).getBoxAroundColorClass(pink,nearPoint,farPoint);
#else
  ((ColorTable64&) colorTable).getBoxAroundColorClass(pink,nearPoint,farPoint);
#endif
  minPinkUValue = nearPoint.y;
}


/*
 * $Log: GT2004BeaconDetector.cpp,v $
 * Revision 1.20  2004/09/08 14:39:02  wachter
 * - Fixed some doxygen-errors
 *
 * Revision 1.19  2004/07/09 14:12:55  nistico
 * Some more safety checks
 *
 * Revision 1.18  2004/06/14 22:11:52  nistico
 * Minor improvement in clustering
 *
 * Revision 1.17  2004/06/13 21:22:14  nistico
 * Minor bug fixes
 *
 * Revision 1.16  2004/06/12 17:37:00  nistico
 * Eventually, would be nice to have only one Bresenham on the whole
 * GT2004ImageProcessor
 *
 * Revision 1.15  2004/06/09 15:04:14  nistico
 * Changed distribution of scanlines
 * Some cleanup
 * Test result positive
 *
 * Revision 1.14  2004/06/08 16:00:33  nistico
 * Final(?) improvement to the beaconSpecialist: 3 or 4 columns (depending on size)
 * are always scanned, and the results are merged based on validity, which is
 * calculated from the number of edges found and consistency checks
 *
 * Revision 1.13  2004/06/05 19:48:45  nistico
 * Added one more special situation to BeaconDetector
 * imageProcessorGradients now visualized edges used by
 * BeaconDetector, for debugging
 *
 * Revision 1.12  2004/06/05 07:58:21  roefer
 * Compensation for motion distortions of images
 *
 * Revision 1.11  2004/06/04 16:34:22  juengel
 * fixed gcc compile error
 *
 * Revision 1.10  2004/06/04 16:07:32  nistico
 * AnalyzeBeacon: more special cases added,
 * should be (almost) final
 *
 * Revision 1.9  2004/06/04 14:16:34  nistico
 * AnalyzeBeacon: additional recovery situation from missing edge (gradient too smooth) debugged
 *
 * Revision 1.8  2004/06/03 16:28:49  nistico
 * AnalyzeBeacon: additional recovery situation from missing edge (gradient too smooth)
 * doesn't work properly yet
 *
 * Revision 1.5  2004/06/02 14:35:59  nistico
 * - Pink scanline clustering further improved
 *
 * Revision 1.4  2004/06/01 16:07:28  nistico
 * Clustering of beacon pink scanlines improved
 *
 * Revision 1.1.1.1  2004/05/22 17:19:41  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.5  2004/05/18 18:39:25  nistico
 * BeaconDetector improved, the landmarks are recognized much more often,
 * and the width is estimated correctly most of the time.
 * Some things are still less then ideal, though (i'll post something on the forum),
 *
 * Revision 1.4  2004/05/15 15:00:22  nistico
 * Beacon scanlines begin slightly below the horizon.
 * Scanline spacing changed.
 *
 * Revision 1.3  2004/05/14 12:19:24  tim
 * fixed bug
 *
 * Revision 1.2  2004/05/06 16:03:56  nistico
 * Supports ColorTable32K through CT32K_LAYOUT switch located into
 * GT2004ImageProcessorTools.h
 *
 * Revision 1.1  2004/05/04 13:40:19  tim
 * added GT2004ImageProcessor
 *
 */

---