Modules/ImageProcessor/SlamImageProcessor/SlamBeaconDetector.cpp

Go to the documentation of this file.

/**
* @file SlamBeaconDetector.cpp
* 
* Implementation of class SlamBeaconDetector.
*
* @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 "SlamImageProcessorTools.h"
#include "SlamBeaconDetector.h"

#include "Representations/Perception/SlamPercept.h"

#define slamPercept (*(SlamPercept *)specialPercept.slamData)

#define MIN_SIMILARITY 0.65

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


SlamBeaconDetector::SlamBeaconDetector(const Image& image, 
                       const CameraMatrix& cameraMatrix,
                       const CameraMatrix& prevCameraMatrix,
                       const ImageInfo& imageInf,
                       const ColorTable& colorTable, 
                       const ColorCorrector& colorCorrector,
             const RobotPose& robotPose,
                       LandmarksPercept& landmarksPercept,
             SpecialPercept& specialPercept):
                       image(image), cameraMatrix(cameraMatrix), 
                       prevCameraMatrix(prevCameraMatrix),
                       imageInf(imageInf), colorTable(colorTable), 
                       colorCorrector(colorCorrector),
             robotPose(robotPose),
                       landmarksPercept(landmarksPercept),
             specialPercept(specialPercept),
                       flagSpecialist(colorCorrector),
                       horizontalBaseOffset(1.6), // was 1.5
                       numOfHorizontalScanLineAbove(10), 
//                       numOfHorizontalScanLineBelow(3),
                       numOfHorizontalScanLineBelow(10),
                       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)
{
  frameCount = 0;
  numFlags = 0;
}


void SlamBeaconDetector::execute()
{
  NDECLARE_DEBUGDRAWING("slam-ip:new-flag", "drawingOnImage", "New found flags in the image");

  numOfClusteredBeaconCandidates = 0;

  direction = 0;
  directionCount = 0;

  if (slamPercept.slamState == SlamPercept::LearnNewFlags)
  {
    CIRCLE(percepts_robot, robotPose.translation.x, robotPose.translation.y, 500, 50, Drawings::ps_solid, blue);
  }
  else
  {
    CIRCLE(percepts_robot, robotPose.translation.x, robotPose.translation.y, 500, 50, Drawings::ps_solid, red);
  }

  frameCount++;
  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 < numFlags; i++)
  {
    double flagDirection = specialFlags[i].direction / ((double)specialFlags[i].directionCount);

    LINE (percepts_robot, robotPose.translation.x, robotPose.translation.y, robotPose.translation.x + 500.0 * cos (flagDirection), robotPose.translation.y + 500.0 * sin (flagDirection), 30, Drawings::ps_solid, Drawings::red);
  };

  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);

  if (directionCount > 0)
  {
  direction /= ((double)directionCount);
  
  slamPercept.viewAngle = direction;
  slamPercept.viewAngleValid = true;

  ARROW (percepts_robot, robotPose.translation.x, robotPose.translation.y, 750 * cos (direction) + robotPose.translation.x, 750 * sin (direction) + robotPose.translation.y, 50, Drawings::ps_solid, Drawings::red);
  }
  else
  slamPercept.viewAngleValid = false;
}


void SlamBeaconDetector::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 SlamBeaconDetector::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 SlamBeaconDetector::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, Vector2<int> ((int)((leftOfBeacon.x + rightOfBeacon.x) / 2), (int)((leftOfBeacon.y + rightOfBeacon.y) / 2)), 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, Vector2<int> ((int)((leftOfBeacon.x + rightOfBeacon.x) / 2), (int)((leftOfBeacon.y + rightOfBeacon.y) / 2)), mergedScanLineLength);
}

double SlamBeaconDetector::FindSimilarFlag(SpecialFlag &specialFlag)
{
  double similarity = 0;
  double maxSim = 0;
  int maxSimFlag = -1;

  for (int i = 0; i < numFlags; i++)
  {
    similarity = 0;

    for (int sector = 0; sector < NUM_SECTORS; sector++)
    {
      if (specialFlags[i].sectors[sector] == specialFlag.sectors[sector])
        similarity++;
    }

    similarity /= ((double)NUM_SECTORS);

    if (similarity > maxSim)
    {
      maxSim = similarity;
      maxSimFlag = i;
    };
  }

  if (maxSimFlag != -1)
  {
    specialFlag = specialFlags[maxSimFlag];
  };

  return maxSim;
};

void SlamBeaconDetector::findSpecial(const Vector2<int>& center, const double pinkRunWidth)
{
  if ((center.x < 20) || (center.y < 20) || (image.cameraInfo.resolutionWidth - center.x < 20) || (image.cameraInfo.resolutionHeight - center.y < 20))
  {
//    OUTPUT (idText, text, "drop flag, too near to image border");

    return;
  };
  
  double rotation(imageInf.horizon.direction.angle());

  //first scan for special landmarks
  SpecialFlag specialFlag;

  //scan sectors
  LINE (imageProcessor_general, center.x, center.y, center.x + sin (rotation) * 50, center.y + cos (rotation) * 50, 1, Drawings::ps_solid, Drawings::blue);

  for (int sector = 0; sector < NUM_SECTORS; sector++)
  {
    double scan_direction = rotation - pi_2 + ((double)sector) * (pi2 / ((double) NUM_SECTORS));

    Vector2<int> endPoint = Vector2<int> (center.x + (int)(sin (scan_direction - pi_2) * 2 * pinkRunWidth), center.y + (int)(cos (scan_direction - pi_2) * 2 * pinkRunWidth));

    if (endPoint.x < 0)
      endPoint.x = 0;

    if (endPoint.x >= image.cameraInfo.resolutionWidth)
      endPoint.x = image.cameraInfo.resolutionWidth - 1;

    if (endPoint.y < 0)
      endPoint.y = 0;

    if (endPoint.y >= image.cameraInfo.resolutionHeight)
      endPoint.y = image.cameraInfo.resolutionHeight - 1;

    BresenhamLineScan bresenham (center, endPoint);

    Vector2<int> pixel = center;
      
    bresenham.init ();

    char colors[(int)numOfColors];

    memset (colors, 0x00, numOfColors);

    int count = 0;
    bool finished = false;

    colorClass currentColor = noColor;
    int currentColorCount = 0;

//    for (count = 0; count < bresenham.numberOfPixels; count++)
    while (!finished)
    {
      bresenham.getNext (pixel);

      if ((pixel.x < 0) | (pixel.y < 0) | (pixel.x >= image.cameraInfo.resolutionWidth) | (pixel.y >= image.cameraInfo.resolutionHeight) | (count++ > 25))
      {
        finished = true;

        continue;
      }
      
      unsigned char colorY(colorCorrector.correct(pixel.x,pixel.y,0,image.image[pixel.y][0][pixel.x]));
      unsigned char colorU(colorCorrector.correct(pixel.x,pixel.y,1,image.image[pixel.y][1][pixel.x]));
      unsigned char colorV(colorCorrector.correct(pixel.x,pixel.y,2,image.image[pixel.y][2][pixel.x]));

      colorClass pixelColor = COLOR_CLASS(colorY,colorU,colorV);

//      if (pixelColor != pink)
//        colors[pixelColor]++;

      if ((pixelColor == pink) || (pixelColor == noColor))
        continue;

      currentColor = pixelColor;

      break;

      if (pixelColor == currentColor)
      {
        currentColorCount++;
      }
      else
      {
        currentColor = pixelColor;
        currentColorCount = 0;
      };

      if (currentColorCount > min (pinkRunWidth / 2.0, (double)10))
        finished = true;
    };

    //calc most found color
    //count = 0;

    //for (int i = 0; i < numOfColors; i++)
    //  if (colors[i] > count)
    //  {
    //    specialFlag.sectors[sector] = (colorClass)i;

    //    count = colors[i];
    //  };

    specialFlag.sectors[sector] = currentColor;

    LINE (imageProcessor_general, center.x, center.y, pixel.x, pixel.y, 1, Drawings::ps_solid, ColorClasses::colorClassToDrawingsColor (specialFlag.sectors[sector]));
  };

  Vector2<int>  pointOnField;
  Vector2<double> angles;

  Geometry::calculateAnglesForPoint (center, cameraMatrix, image.cameraInfo, angles);

  double flagDirection = angles.x + robotPose.getAngle ();

  LINE (percepts_robot, robotPose.translation.y, robotPose.translation.y, 500.0 * cos (flagDirection), 500.0 * sin (flagDirection), 50, Drawings::ps_solid, Drawings::pink);

  SpecialFlag originalFlag = specialFlag;

  double similarity = FindSimilarFlag (specialFlag);

  if (similarity > MIN_SIMILARITY)
  {
    //found corresponding flag
    CIRCLE(imageProcessor_general, center.x, center.y, 10, 1, Drawings::ps_solid, blue);

    OUTPUT (idText, text, "recognized flag " << specialFlag.index << " with similarity " << similarity);

    switch (slamPercept.slamState)
    {
    case SlamPercept::DebugJustLearn:
    case SlamPercept::LearnNewFlags:
      {
        specialFlag.direction += flagDirection;
        specialFlag.directionCount++;

        break;
      };
    case SlamPercept::DebugJustRecognize:
    case SlamPercept::UseNewFlags:
      {
        if (specialFlag.directionCount > 1)
        {
          specialFlag.direction /= specialFlag.directionCount;
          specialFlag.directionCount = 1;
        };

        direction += specialFlag.direction;
        directionCount++;

        break;
      };
    };
  }
  else
  {
    //add new
    CIRCLE(imageProcessor_general, center.x, center.y, 10, 1, Drawings::ps_solid, red);

    if ((slamPercept.slamState == SlamPercept::LearnNewFlags) || (slamPercept.slamState == SlamPercept::DebugJustLearn))
    {
      OUTPUT (idText, text, "similarity only " << similarity);

      originalFlag.direction = flagDirection;
      originalFlag.directionCount = 1;
      originalFlag.index = numFlags;

      if (numFlags < NUM_SPECIAL_FLAGS)
      {
        OUTPUT (idText, text, "Learned new Flag " << numFlags << " @ " << originalFlag.direction);
        OUTPUT (idText, text, "");

        specialFlags[numFlags] = originalFlag;
        numFlags++;
      }
      else
      {
        OUTPUT (idText, text, "Found to much Flags!!!");
        OUTPUT (idText, text, "new Flag could not be learned:" << originalFlag.direction);
        OUTPUT (idText, text, "");
      }
    }
  };
};

void SlamBeaconDetector::characterizeNewFlag(const Vector2<int>& center)
{
  SlamFlagLocator::flagCharacteristics left,right,above,below;
  Vector2<double> direction = imageInf.horizon.direction; // directs right I hope   ;)
  right = scanForFlagCharacterization(center,direction);
  direction.rotateRight();
  below = scanForFlagCharacterization(center,direction);
  direction.rotateRight();
  left = scanForFlagCharacterization(center,direction);
  direction.rotateRight();
  above = scanForFlagCharacterization(center,direction);

  // insert the result into the NewFlagModell
  Vector2<double> angles;
  Geometry::calculateAnglesForPoint(center,cameraMatrix,image.cameraInfo,angles);
  int characterisationNumber = slamPercept.flagLocator.getCharacterisationNumber(above,below,left,right);
  slamPercept.flagLocator.addFlagDirection(robotPose,angles.x,characterisationNumber);
}

SlamFlagLocator::flagCharacteristics SlamBeaconDetector::scanForFlagCharacterization(const Vector2<int>& start, const Vector2<double>& direction)
{
  BresenhamLineScan bls(start,direction,image.cameraInfo);
  Vector2<int> actual(start);
  int noPinkSince = 0;
  int blueCount = 0;
  int yellowCount = 0;
  int somethingElseCount = 0;
  bool stillOnPink = true;
  bool onPinkAgain = false;

  unsigned char y,u,v;
  colorClass c1,c2,c3;

  bool vertical = (fabs(direction.x) >= fabs(direction.y));
  
  bool use3PixelScanLine = true;
  
  DEBUG_RESPONSE("slam-ip:only-use-a-single-scanlines-to-characterise-flags",
    use3PixelScanLine = false;
  );
  
  

  while (   (actual.x>0 && actual.x<image.cameraInfo.resolutionWidth-1 && actual.y>0 && actual.y<image.cameraInfo.resolutionHeight-1)
         && (noPinkSince<=5)
         && ( !onPinkAgain)    )
  {    
    y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    c1 = COLOR_CLASS(y, u, v);
    
    if (use3PixelScanLine) // use all three pixels?
    {
      if (vertical)
      {
        // left of actual
        y = colorCorrector.correct(actual.x-1, actual.y, 0, image.image[actual.y][0][actual.x-1]);
        u = colorCorrector.correct(actual.x-1, actual.y, 1, image.image[actual.y][1][actual.x-1]);
        v = colorCorrector.correct(actual.x-1, actual.y, 2, image.image[actual.y][2][actual.x-1]);
        c2 = COLOR_CLASS(y, u, v);
        // right of actual
        y = colorCorrector.correct(actual.x+1, actual.y, 0, image.image[actual.y][0][actual.x+1]);
        u = colorCorrector.correct(actual.x+1, actual.y, 1, image.image[actual.y][1][actual.x+1]);
        v = colorCorrector.correct(actual.x+1, actual.y, 2, image.image[actual.y][2][actual.x+1]);
        c3 = COLOR_CLASS(y, u, v);
      }
      else
      {
        // below actual
        y = colorCorrector.correct(actual.x, actual.y-1, 0, image.image[actual.y-1][0][actual.x]);
        u = colorCorrector.correct(actual.x, actual.y-1, 1, image.image[actual.y-1][1][actual.x]);
        v = colorCorrector.correct(actual.x, actual.y-1, 2, image.image[actual.y-1][2][actual.x]);
        c2 = COLOR_CLASS(y, u, v);
        // above actual
        y = colorCorrector.correct(actual.x, actual.y+1, 0, image.image[actual.y+1][0][actual.x]);
        u = colorCorrector.correct(actual.x, actual.y+1, 1, image.image[actual.y+1][1][actual.x]);
        v = colorCorrector.correct(actual.x, actual.y+1, 2, image.image[actual.y+1][2][actual.x]);
        c3 = COLOR_CLASS(y, u, v);
      }

      if ( (c1==pink && c2==pink) || (c2==pink && c3==pink) || (c1==pink && c3==pink) )
      {
        if (!stillOnPink && noPinkSince>1)
        {
          onPinkAgain = true;
        }
        else
        {
          stillOnPink = true;
          noPinkSince=0;
          somethingElseCount=0; // forget the noise pixels inside the pink part
        }
      }
      else 
      {
        stillOnPink = false;
        noPinkSince++;
      }

      switch (c1)
      {
        case pink:
          break;
        case skyblue:
          blueCount++;
          break;
        case yellow:
          yellowCount++;
          break;
        default:
          somethingElseCount++;
          break;
      }
      switch (c2)
      {
        case pink:
          break;
        case skyblue:
          blueCount++;
          break;
        case yellow:
          yellowCount++;
          break;
        default:
          somethingElseCount++;
          break;
      }
      switch (c3)
      {
        case pink:
          break;
        case skyblue:
          blueCount++;
          break;
        case yellow:
          yellowCount++;
          break;
        default:
          somethingElseCount++;
          break;
      }

      //
    }
    else
    {
      // use just a one-pixel-scanline only
      switch (c1)
      {
        case pink:
          if (!stillOnPink && noPinkSince>1)
          {
            onPinkAgain = true;
          }
          else
          {
            stillOnPink = true;
            noPinkSince=0;
            somethingElseCount=0; // forget the noise pixels inside the pink part
          }
          break;
        case skyblue:
          stillOnPink = false;
          noPinkSince++;
          blueCount++;
          break;
        case yellow:
          stillOnPink = false;
          noPinkSince++;
          yellowCount++;
          break;
        default:
          stillOnPink = false;
          noPinkSince++;
          somethingElseCount++;
          break;
      }
    }

    bls.getNext(actual);
  }

  int threshold;
  if (use3PixelScanLine)
  {
    threshold = 6;
  }
  else
  {
    threshold = 2;
  }

  SlamFlagLocator::flagCharacteristics characteristics;

  // interpret the result of the scanning
  if (blueCount>=threshold && yellowCount<threshold && blueCount>somethingElseCount)
  {
    // characteristics = blue
    characteristics = SlamFlagLocator::blueOnThisSide;
  }
  else if (yellowCount>=threshold && blueCount<threshold && yellowCount>somethingElseCount)
  {
    // characteristics = yellow
    characteristics = SlamFlagLocator::yellowOnThisSide;
  }
  else
  {
    // characteristics = somethingElse
    characteristics = SlamFlagLocator::somethingElseOnThisSide;
  }

  // drawings
  NLINE("slam-ip:new-flag",start.x,start.y,actual.x,actual.y,1,Drawings::ps_solid,
    (characteristics==SlamFlagLocator::blueOnThisSide)?(Drawings::blue):((characteristics==SlamFlagLocator::yellowOnThisSide)?(Drawings::yellow):(Drawings::dark_gray)));

  return characteristics;
}

void SlamBeaconDetector::analyzeBeacon(const Vector2<double>& left, const Vector2<int>& center, const double pinkRunWidth)
{
  if (pinkRunWidth >= 3.0)
  {
    numOfClusteredBeaconCandidates++;

    findSpecial (center, pinkRunWidth);

    characterizeNewFlag(center);
    
  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 SlamBeaconDetector::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 SlamBeaconDetector::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 SlamBeaconDetector::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;
}

---