Modules/ImageProcessor/VLCImageProcessor/VLCLineFinder_DeterministicApproach.cpp

Go to the documentation of this file.

#include "VLCLineFinder_DeterministicApproach.h"

VLCLineFinder_DeterministicApproach::VLCLineFinder_DeterministicApproach(const ColorCorrector& colorCorrector, const ColorTable& colorTable)
  :colorCorrector(colorCorrector), colorTable(colorTable)
{
  normDistance = 3.0;
  normProjection = 0.9925; // arccos(7°)
  normProjectionPerpendicularToHorizon = 0.97; // arccos(14°)

  framesSinceLastSeen=1000;
  frameCount = 0;
  circleCount = 0;
}

VLCLineFinder_DeterministicApproach::~VLCLineFinder_DeterministicApproach(void)
{
}

void VLCLineFinder_DeterministicApproach::reset()
{
  numberOfLineFragments = 0;
  numberOfLines = 0;
  numberOfLinePoints = 0;
  foundByCircleFinder = false;
  foundByOwnHandling = false;
  framesSinceLastSeen++;
  frameCount++;
  if (frameCount>200)
  {
    if (circleCount>0)
    {
      // OUTPUT(idText,text, "MSHLineFinder: " << "Circle recognition rate:" << (((double)circleCount)/((double)frameCount)) );
    }
    frameCount = 0;
    circleCount = 0;
  }
}

void VLCLineFinder_DeterministicApproach::considerLinePoint(Vector2<int> & pointOnLine, Vector2<double> & normalToLine)
{
  // just store the point, if there is still a free space in the array
  if (numberOfLinePoints < maxNumberOfLinePoints)
  {
    linePoints[numberOfLinePoints].pointOnLine = pointOnLine;
    linePoints[numberOfLinePoints].normalToLine = normalToLine;
    linePoints[numberOfLinePoints].belongsToLineNo = -1;
    numberOfLinePoints++;
  }
  else
  {
    //OUTPUT(idText,text,"LineFinder: linePoint drop (raise maxNumberOfLinePoints)");
  }
}

void VLCLineFinder_DeterministicApproach::execute(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image, const ImageInfo& imageInfo, const RobotPose & robotPose)
{
  lines = new LineFragment[maxNumberOfLines];
  lineFragments = new LineFragment[maxNumberOfLines*2];

  foundByCircleFinder = false;

  findLineFragments();
  handleCenterCircle(cameraMatrix, image, robotPose);
  findLines(linesPercept, cameraMatrix, image, imageInfo);
  findIntersections(linesPercept, cameraMatrix, image, robotPose, imageInfo);
  addCenterCirclePercept(linesPercept, cameraMatrix, image);

  delete[] lines;
  delete[] lineFragments;

  DEBUG_DRAWING_FINISHED(lineCrossingsField);
  DEBUG_DRAWING_FINISHED(imageProcessor_lines);
  DEBUG_DRAWING_FINISHED(imageProcessor_linefragments);
}

void VLCLineFinder_DeterministicApproach::execute(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image, const ImageInfo& imageInfo, const RobotPose & robotPose, const bool circleFound, const Vector2<double> & circleOnField)
{  
  lines = new LineFragment[maxNumberOfLines];
  lineFragments = new LineFragment[maxNumberOfLines*2];

  foundByCircleFinder = false; // circleFound;
  fromCircleFinder.x = (int)circleOnField.x;
  fromCircleFinder.y = (int)circleOnField.y;

  if (foundByCircleFinder)
  {
    handleCenterCircle(circleOnField, cameraMatrix, image);
    
    // for validity tests
    /*
    Vector2<double> circleOnFieldGlobal = Geometry::relative2FieldCoord(robotPose,circleOnField.x,circleOnField.y);
    OUTPUT(idText,text, robotPose.translation.x << " " << robotPose.translation.y << " " << circleOnFieldGlobal.x << " " << circleOnFieldGlobal.y);
    CIRCLE(circlePoints_Field,circleOnFieldGlobal.x,circleOnFieldGlobal.y,180,3,Drawings::ps_solid,Drawings::red);
    */
  }
  findLineFragments();
  if (!foundByCircleFinder)
  {
    handleCenterCircle(cameraMatrix, image, robotPose);
  }
  findLines(linesPercept, cameraMatrix, image, imageInfo);
  findIntersections(linesPercept, cameraMatrix, image, robotPose, imageInfo);
  addCenterCirclePercept(linesPercept, cameraMatrix, image);

  delete[] lines;
  delete[] lineFragments;

  DEBUG_DRAWING_FINISHED(lineCrossingsField);
  DEBUG_DRAWING_FINISHED(imageProcessor_lines);
  DEBUG_DRAWING_FINISHED(imageProcessor_linefragments);
}

void VLCLineFinder_DeterministicApproach::findLineFragments()
{
  int i, j;  
  Vector2<double> temp;

  // for all lines, calculate how many other lines are similar/near in sense of this special norm
  bool similar[maxNumberOfLinePoints][maxNumberOfLinePoints];
  for(i = 0; i < numberOfLinePoints; i++)
  {
    similar[i][i] = true;
    for(j = i + 1; j < numberOfLinePoints; j++)
    {
      // similar/near if distance is small and normal is in the same direction
      bool sim = false;
      double projection = linePoints[i].normalToLine * linePoints[j].normalToLine; // projection := cos(angle)
      if(projection > normProjection)
      {
        temp.x = (double)(linePoints[j].pointOnLine.x - linePoints[i].pointOnLine.x);
        temp.y = (double)(linePoints[j].pointOnLine.y - linePoints[i].pointOnLine.y);
        double distance1 = fabs(temp * linePoints[i].normalToLine);
        if(distance1 < normDistance)
        {
          double distance2 = fabs(temp * linePoints[j].normalToLine);
          sim = (distance2 < normDistance);
        }
      }
      similar[i][j] = sim;
      similar[j][i] = sim;
    }
  }

  // analyse detected points and extract line fragments
  int maxWeight;
  int linePointWithHighestWeight;

  
  for(int n=0; n<maxNumberOfLines*2; n++)
  {
    maxWeight = 0;
    linePointWithHighestWeight = -1;

    // find point with most similar points
    int simCount[maxNumberOfLinePoints];
    for(i=0; i<numberOfLinePoints; i++)
    {
      simCount[i]=1; // the point itself
    }
    for(i=0; i < numberOfLinePoints; i++)
    {
      if(linePoints[i].belongsToLineNo == -1) // only if point is not yet matched to a line fragment
      {
        for (j=i+1; j < numberOfLinePoints; j++)
        {
          if(linePoints[j].belongsToLineNo == -1) // only if point is not yet matched to a line fragment
          {
            if(similar[i][j])
            {
              // weight added to both points because of reflexivity
              simCount[i]++;
              simCount[j]++;

              if(simCount[i] > maxWeight)
              {
                maxWeight = simCount[i];
                linePointWithHighestWeight = i;
              }
              if(simCount[j] > maxWeight)
              {
                maxWeight = simCount[j];
                linePointWithHighestWeight = j;
              }
            }
          }        
        }
      }
    }
    if (maxWeight<pointsNeededForLine)
    {
      break;
    }

    // build line from point with highest weight
    Vector2<int> start(300,300);
    Vector2<int> end(-1,-1);
    // here the angle to the image border is used to determine, by which criteria start and end are selected (angle has no meaning in relation to the horizon)
    double angle = atan2(linePoints[linePointWithHighestWeight].normalToLine.y,linePoints[linePointWithHighestWeight].normalToLine.x);
    if (angle <= pi_4 && angle >= -1*pi_4) // line is vertical, select start and end by y-Value
    {
      for (i=0; i<numberOfLinePoints; i++)
      {
        if (similar[i][linePointWithHighestWeight] && linePoints[i].belongsToLineNo==-1)
        {
          linePoints[i].belongsToLineNo=numberOfLineFragments;
          if (linePoints[i].pointOnLine.y < start.y)
          {
            start = linePoints[i].pointOnLine;
          }
          if (linePoints[i].pointOnLine.y > end.y)
          {
            end = linePoints[i].pointOnLine;
          }
        }
      }
    }
    else  // line is horizontal, select start and end by x-Value
    {
      for (i=0; i<numberOfLinePoints; i++)
      {
        if (similar[i][linePointWithHighestWeight] && linePoints[i].belongsToLineNo==-1)
        {
          linePoints[i].belongsToLineNo=numberOfLineFragments;
          if (linePoints[i].pointOnLine.x < start.x)
          {
            start = linePoints[i].pointOnLine;
          }
          if (linePoints[i].pointOnLine.x > end.x)
          {
            end = linePoints[i].pointOnLine;
          }
        }
      }
    }

    // for fragments with only few points, remove the long ones, because they may be caused by random noise linePoints
    if (maxWeight==pointsNeededForLine)
    {
      Vector2<int> length = end - start;
      if ( length.abs() > 70 )
      { // skip this fragment and mark points as "are not and will not be used"
        for (i=0; i<numberOfLinePoints; i++)
        {
          if (linePoints[i].belongsToLineNo==numberOfLineFragments)
          {
            linePoints[i].belongsToLineNo=-2;
          }
        }
        continue;
      }
    }

    // accept the line fragment
    Vector2<double> normalToLine((double)(end.x-start.x), (double)(end.y-start.y));
    normalToLine.rotateLeft();
    normalToLine.normalize();
    Vector2<int> middle((start.x+end.x)/2, (start.y+end.y)/2);
    lineFragments[numberOfLineFragments].base   = middle;
    lineFragments[numberOfLineFragments].normal = normalToLine;
    lineFragments[numberOfLineFragments].start  = start;
    lineFragments[numberOfLineFragments].end    = end;
    lineFragments[numberOfLineFragments].lineFragmentAlreadyConsidered = false;
    lineFragments[numberOfLineFragments].averageStep = (end-start).abs() / maxWeight;
    LINE(imageProcessor_linefragments,start.x,start.y,end.x,end.y,1,1,numberOfLineFragments);
    numberOfLineFragments++;
  }
}

void VLCLineFinder_DeterministicApproach::findLines(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image, const ImageInfo& imageInfo)
{
  int i, j;
  Vector2<double> temp, temp2;
  Vector2<double> horizonDirection = imageInfo.horizon.direction;
  horizonDirection.normalize();


  // if both sides or more than one fragment of a field line are detected, then filter these out and take their middle instead
  for (i=0; i<numberOfLineFragments && numberOfLines<maxNumberOfLines; i++)
  {
    if (lineFragments[i].lineFragmentAlreadyConsidered)
    {
      continue;
    }
    lines[numberOfLines].base = lineFragments[i].base;
    lines[numberOfLines].normal = lineFragments[i].normal;
    lines[numberOfLines].start = lineFragments[i].start;
    lines[numberOfLines].end = lineFragments[i].end;
    lines[numberOfLines].averageStep = lineFragments[i].averageStep;
    int numberOfLineFragmentsUsed = 1;
    lineFragments[i].lineFragmentAlreadyConsidered = true;

    double angle = atan2(lines[numberOfLines].normal.y,lines[numberOfLines].normal.x);
    bool sortByY = angle <= pi_4 && angle >= -1*pi_4;
    
    for (j=i+1; j<numberOfLineFragments; j++)
    {
      if (lineFragments[j].lineFragmentAlreadyConsidered)
      {
        continue;
      }
      double p = lineFragments[i].normal * lineFragments[j].normal;
      Vector2<double> nij;
      if (p>0)
      {
        nij = lineFragments[i].normal + lineFragments[j].normal;
      }
      else
      {
        nij = lineFragments[i].normal - lineFragments[j].normal;
      }
      nij.normalize();
      bool perpendicularToHorizon = fabs(horizonDirection * nij) > 0.93;

      if (     (perpendicularToHorizon || p>normProjection || p<-1*normProjection) // if not perpendicular, than decide using normProjection
            && (!perpendicularToHorizon || p>normProjectionPerpendicularToHorizon || p<-1*normProjectionPerpendicularToHorizon)) // else, use normProjectionPerpendicularToHorizon
      {
        temp.x = (double)(lineFragments[i].base.x-lineFragments[j].base.x);
        temp.y = (double)(lineFragments[i].base.y-lineFragments[j].base.y);
        int lineHeight = max( Geometry::calculateLineSize(lineFragments[i].base, cameraMatrix, image.cameraInfo),
                              Geometry::calculateLineSize(lineFragments[j].base, cameraMatrix, image.cameraInfo));
        lineHeight = max( lineHeight, 2 );
        
        if ( (fabs(temp * lineFragments[i].normal) < 1.5 * lineHeight) && (fabs(temp * lineFragments[j].normal) < 1.5 * lineHeight) )
        {
          
          if (p<0) // wrong orientation to sum it up, so invert it
          {
            lineFragments[j].normal *= -1.0;
          }
          lines[numberOfLines].base    += lineFragments[j].base;
          lines[numberOfLines].normal  += lineFragments[j].normal;
          lines[numberOfLines].averageStep += lineFragments[i].averageStep;

          if (sortByY) // line is vertical, select start and end by y-Value
          {
            if (lineFragments[j].start.y < lines[numberOfLines].start.y)
            {
              lines[numberOfLines].start = lineFragments[j].start;
            }
            if (lineFragments[j].end.y > lines[numberOfLines].end.y)
            {
              lines[numberOfLines].end = lineFragments[j].end;
            }
          }
          else  // line is horizontal, select start and end by x-Value
          {
            if (lineFragments[j].start.x < lines[numberOfLines].start.x)
            {
              lines[numberOfLines].start = lineFragments[j].start;
            }
            if (lineFragments[j].end.x > lines[numberOfLines].end.x)
            {
              lines[numberOfLines].end = lineFragments[j].end;
            }
          }
          
          numberOfLineFragmentsUsed++;
          lineFragments[j].lineFragmentAlreadyConsidered = true;
        }
      }
    }
    // the resulting line is a middle of its fragments
    lines[numberOfLines].base /= numberOfLineFragmentsUsed;
    lines[numberOfLines].averageStep /= numberOfLineFragmentsUsed;
    lines[numberOfLines].normal.normalize();
    
    numberOfLines++;
  }

  /*
  // create new fieldLine-percepts based on the found lines (if at least one line was found)
  double angle;
  Vector2<int> temp3;
  if (numberOfLines > 0)
  {
    linesPercept.numberOfPoints[LinesPercept::field] = 0;
    for (i=0; i<numberOfLines; i++)
    {
      calculateLineOnField(i,temp,temp2,cameraMatrix,image);
      angle = normalize(temp2.angle() + pi_2);
      // the following if's will always work, because all these points are already evaluated to lie on the field, but better save than sorry
      if (Geometry::calculatePointOnField(lines[i].base.x,lines[i].base.y,cameraMatrix,image.cameraInfo,temp3))
      {
        linesPercept.add(LinesPercept::field,temp3,angle);
      }
      if (Geometry::calculatePointOnField(lines[i].start.x,lines[i].start.y,cameraMatrix,image.cameraInfo,temp3))
      {
        linesPercept.add(LinesPercept::field,temp3,angle);
      }
      if (Geometry::calculatePointOnField(lines[i].end.x,lines[i].end.y,cameraMatrix,image.cameraInfo,temp3))
      {
        linesPercept.add(LinesPercept::field,temp3,angle);
      }
    }
  }
  */

  // draw all lines
  Vector2<int> r1(0,0);
  Vector2<int> r2(image.cameraInfo.resolutionWidth-1,image.cameraInfo.resolutionHeight-1);    
  for (i=0; i<numberOfLines; i++)
  {
    // in the image
    Vector2<int> pointOnLine;
    Vector2<double> direction;
    getLine(i, pointOnLine, direction);
    direction.rotateLeft();
    Geometry::Line a(pointOnLine, direction);
    Vector2<int> point1, point2;
    Geometry::getIntersectionPointsOfLineAndRectangle(r1,r2,a,point1,point2);
    LINE(
      imageProcessor_lines,
      point1.x,
      point1.y,
      point2.x,
      point2.y,
      1,
      Drawings::ps_solid,
      Drawings::skyblue
      );
  }
}

void VLCLineFinder_DeterministicApproach::findIntersections(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image, const RobotPose & robotPose, const ImageInfo& imageInfo)
{
  int i,j;
  // calculate line crossings
  Geometry::Line lines2[maxNumberOfLines];
  Vector2<double> intersection;
  Vector2<int> intersectionOnField;
  for (i=0; i<numberOfLines; i++)
  {
    Vector2<double> direction(lines[i].normal);
    direction.rotateLeft();
    direction.normalize();
    lines2[i] = Geometry::Line(lines[i].base,direction);
  }
  for (i=0; i<numberOfLines; i++)
  {
    for (j=i+1; j<numberOfLines; j++)
    {
      if (linesPerpendicularOnField(i, j, cameraMatrix, image))
      {
        Geometry::getIntersectionOfLines(lines2[i],lines2[j],intersection);
        if (Geometry::calculatePointOnField((int)intersection.x,(int)intersection.y,cameraMatrix,image.cameraInfo,intersectionOnField))
        {
          if (intersectionOnField.abs() < 3600)
          {
            addCrossingsPercept(intersection, intersectionOnField, i, j, linesPercept, image, imageInfo, cameraMatrix, robotPose);
          }
        }
      }
    }
  }

  // draw lines on field
  for (i=0; i<numberOfLines; i++)
  {
    Vector2<int> pointOnField1, pointOnField2, directionOnField;
    Geometry::calculatePointOnField(lines[i].base.x, lines[i].base.y, cameraMatrix, image.cameraInfo, pointOnField1);
    Geometry::calculatePointOnField((int)(lines[i].base.x+50*lines[i].normal.y), (int)(lines[i].base.y-50*lines[i].normal.x), cameraMatrix, image.cameraInfo, pointOnField2);
    directionOnField = pointOnField2-pointOnField1;
    pointOnField1 = pointOnField1 - directionOnField * 10;
    pointOnField2 = pointOnField2 + directionOnField * 10;
    Vector2<double> point1OnFieldGlobal, point2OnFieldGlobal;
    point1OnFieldGlobal = Geometry::relative2FieldCoord(robotPose, (double)pointOnField1.x, (double)pointOnField1.y);
    point2OnFieldGlobal = Geometry::relative2FieldCoord(robotPose, (double)pointOnField2.x, (double)pointOnField2.y);
    //LINE(lineCrossingsField, point1OnFieldGlobal.x, point1OnFieldGlobal.y, point2OnFieldGlobal.x, point2OnFieldGlobal.y, 3, Drawings::ps_solid, Drawings::skyblue); 
  }
}

void VLCLineFinder_DeterministicApproach::addCrossingsPercept(const Vector2<double> & intersectionInImage, const Vector2<int> & intersectionOnField, int lineNumber1, int lineNumber2, LinesPercept & linesPercept, const Image& image, const ImageInfo& imageInfo, const CameraMatrix & cameraMatrix, const RobotPose & robotPose)
{
  // prepare stuff and calculate bisection on field
  Vector2<double> temp1,temp2, directionOnField1,directionOnField2;
  bool firstLineIsLeft;
  Vector2<double> directionOfLine1 = lines[lineNumber1].normal;
  directionOfLine1.rotateRight();
  Vector2<double> directionOfLine2 = lines[lineNumber2].normal;
  directionOfLine2.rotateRight();
  if (  !calculateLineOnField(lineNumber1,temp1,directionOnField1,cameraMatrix,image)
    ||  !calculateLineOnField(lineNumber2,temp1,directionOnField2,cameraMatrix,image) )
  {
    // something went wrong and the calculation failed
    return;
  }
  Vector2<double> bisectionOnField = directionOnField1 + directionOnField2;
  bisectionOnField.normalize();
  double angleOnField = atan2(bisectionOnField.y,bisectionOnField.x) + pi_4; // this is more stable than just taking the angle of one of the lines

  
  Vector2<double> angleBisection = directionOfLine1 + directionOfLine2; // this is NOT the projection of the bisectionOnField into the image, this vector merely lies in the same quadrant, and therefore can be used here
  angleBisection.normalize(); 
  // decide, which line is lying on which side of the bisection
  double angleOfBisection = atan2(angleBisection.y,angleBisection.x);
  double angleOfLine1 = atan2(directionOfLine1.y,directionOfLine1.x);
  double angleOfLine2 = atan2(directionOfLine2.y,directionOfLine2.x);
  // BEWARE: the orientation in image coordinates and in field coordinates is invers, so sides in clockwise order in the image result in sides in contra-clockwise (i.e. mathematical positive) order on the field
  if ( ( angleOfLine1 < angleOfBisection + pi_2 && angleOfLine1 > angleOfBisection )
    || ( angleOfLine1 + pi2 < angleOfBisection + pi_2 && angleOfLine1 + pi2 > angleOfBisection )
    || ( angleOfLine1 - pi2 < angleOfBisection + pi_2 && angleOfLine1 - pi2 > angleOfBisection ) )
  { // line1 lies direcly right of the bisection
    firstLineIsLeft=false;
  }
  else
  {
    firstLineIsLeft=true;
  }



  bool bothSidesOfLine1,bothSidesOfLine2;
  
  temp1.x = (double)lines[lineNumber1].start.x - intersectionInImage.x;
  temp1.y = (double)lines[lineNumber1].start.y - intersectionInImage.y;
  temp2.x = (double)lines[lineNumber1].end.x - intersectionInImage.x;
  temp2.y = (double)lines[lineNumber1].end.y - intersectionInImage.y;
  if ( temp1*temp2 < 0 && temp1.abs() > 10 && temp2.abs() > 10 ) // points lie on different sides and with enough distance to the crossing point
  {
    bothSidesOfLine1=true;
  }
  else
  {
    bothSidesOfLine1=false; // or at least not decidable without additional scanning
  }

  temp1.x = (double)lines[lineNumber2].start.x - intersectionInImage.x;
  temp1.y = (double)lines[lineNumber2].start.y - intersectionInImage.y;
  temp2.x = (double)lines[lineNumber2].end.x - intersectionInImage.x;
  temp2.y = (double)lines[lineNumber2].end.y - intersectionInImage.y;
  if ( temp1*temp2 < 0 && temp1.abs() > 10 && temp2.abs() > 10 ) // points lie on different sides and with enough distance to the crossing point
  {
    bothSidesOfLine2=true;
  }
  else
  {
    bothSidesOfLine2=false; // or at least not decidable without additional scanning
  }

  
  LinesPercept::CrossingCharacteristic side1 = LinesPercept::dontKnow;
  LinesPercept::CrossingCharacteristic side2 = LinesPercept::dontKnow;
  LinesPercept::CrossingCharacteristic side3 = LinesPercept::dontKnow;
  LinesPercept::CrossingCharacteristic side4 = LinesPercept::dontKnow;

  if ( intersectionInImage.x < 0 || intersectionInImage.y < 0 || intersectionInImage.x >= image.cameraInfo.resolutionWidth || intersectionInImage.y >= image.cameraInfo.resolutionHeight )
  { // intersection is outside the image
    if (linesPercept.numberOfLineCrossings<linesPercept.maxNumberOfLineCrossings)
    {
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].locationOnField = intersectionOnField;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].locationInImage = Vector2<int>((int)intersectionInImage.x,(int)intersectionInImage.y); 
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleOnField = angleOnField;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleInImage1 = angleOfLine1;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleInImage2 = angleOfLine2;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side1 = side1;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side2 = side2;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side3 = side3;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side4 = side4;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].outOfImage = true;
      linesPercept.numberOfLineCrossings++;
    }
  }
  else
  { // intersection is inside the image

    // do some additional scanning to get characteristics of this crossing...
    Vector2<int> temp((int)intersectionInImage.x,(int)intersectionInImage.y);
    int lineSize = Geometry::calculateLineSize(temp, cameraMatrix, image.cameraInfo);

    if (firstLineIsLeft)
    {
      if (bothSidesOfLine1)
      {
        side1 = LinesPercept::lineOnThisSide;
        side3 = LinesPercept::lineOnThisSide;
      }
      else
      {
        doVerificationScan(intersectionInImage, directionOfLine1, directionOfLine2, lineSize, side1, cameraMatrix, image);
        doVerificationScan(intersectionInImage, directionOfLine1 * -1.0, directionOfLine2, lineSize, side3, cameraMatrix, image);
      }
      if (bothSidesOfLine2)
      {
        side2 = LinesPercept::lineOnThisSide;
        side4 = LinesPercept::lineOnThisSide;
      }
      else
      {
        doVerificationScan(intersectionInImage, directionOfLine2 * -1.0, directionOfLine1, lineSize, side2, cameraMatrix, image);
        doVerificationScan(intersectionInImage, directionOfLine2, directionOfLine1, lineSize, side4, cameraMatrix, image); 
      }
    }
    else
    {
      
      if (bothSidesOfLine1)
      {
        side2 = LinesPercept::lineOnThisSide;
        side4 = LinesPercept::lineOnThisSide;
      }
      else
      {
        doVerificationScan(intersectionInImage, directionOfLine1 * -1.0, directionOfLine2, lineSize, side2, cameraMatrix, image);
        doVerificationScan(intersectionInImage, directionOfLine1, directionOfLine2, lineSize, side4, cameraMatrix, image); 
      }
      if (bothSidesOfLine2)
      {
        side1 = LinesPercept::lineOnThisSide;
        side3 = LinesPercept::lineOnThisSide;
      }
      else
      {
        doVerificationScan(intersectionInImage, directionOfLine2, directionOfLine1, lineSize, side1, cameraMatrix, image);
        doVerificationScan(intersectionInImage, directionOfLine2 * -1.0, directionOfLine1, lineSize, side3, cameraMatrix, image);
      }
    }
    
    /*
    // don't take crossings with stupid characteristics
    if (
        (  side1 == LinesPercept::lineOnThisSide
        && side2 == LinesPercept::lineOnThisSide
        && side3 == LinesPercept::lineOnThisSide
        && side4 == LinesPercept::lineOnThisSide)
        ||
        (  side1 == LinesPercept::noLineOnThisSide
        && side2 == LinesPercept::noLineOnThisSide
        && side3 == LinesPercept::noLineOnThisSide
        && side4 == LinesPercept::noLineOnThisSide)
       )
    {
      return;
    }
    */
    
    // add percept
    if (linesPercept.numberOfLineCrossings<linesPercept.maxNumberOfLineCrossings)
    {
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].locationOnField = intersectionOnField;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].locationInImage = Vector2<int>((int)intersectionInImage.x,(int)intersectionInImage.y); 
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleOnField = angleOnField;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleInImage1 = angleOfLine1;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleInImage2 = angleOfLine2;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side1 = side1;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side2 = side2;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side3 = side3;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side4 = side4;
      linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].outOfImage = false;
      linesPercept.numberOfLineCrossings++;          
    }
  }

  // add drawings...
#ifndef NODEBUGDRAWINGS
  Vector2<double> intersectionOnFieldGlobal = Geometry::relative2FieldCoord(robotPose,intersectionOnField.x,intersectionOnField.y);
  CIRCLE(lineCrossingsField,intersectionOnFieldGlobal.x,intersectionOnFieldGlobal.y,60,3,Drawings::ps_solid,Drawings::red);
  //OUTPUT(idText,text,intersectionOnField.x << " " << intersectionOnField.y << " " << intersectionOnFieldGlobal.x << " " << intersectionOnFieldGlobal.y);
  double angleOnFieldGlobal = angleOnField + robotPose.rotation;

  // draw characteristics of the sides: white->lineOnThisSide, black->noLineOnThisSide, light_gray->dontKnow
  LINE( // side1
    lineCrossingsField,
    intersectionOnFieldGlobal.x,
    intersectionOnFieldGlobal.y,
    intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal)),
    intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal)),
    5,
    Drawings::ps_solid,
    (side1==LinesPercept::lineOnThisSide) ? Drawings::white : ((side1==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
  );
  LINE( // side2
    lineCrossingsField,
    intersectionOnFieldGlobal.x,
    intersectionOnFieldGlobal.y,
    intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal+pi_2)),
    intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal+pi_2)),
    5,
    Drawings::ps_solid,
    (side2==LinesPercept::lineOnThisSide) ? Drawings::white : ((side2==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
  );
  LINE( // side3
    lineCrossingsField,
    intersectionOnFieldGlobal.x,
    intersectionOnFieldGlobal.y,
    intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal+pi)),
    intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal+pi)),
    5,
    Drawings::ps_solid,
    (side3==LinesPercept::lineOnThisSide) ? Drawings::white : ((side3==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
  );
  LINE( // side4
    lineCrossingsField,
    intersectionOnFieldGlobal.x,
    intersectionOnFieldGlobal.y,
    intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal+pi3_2)),
    intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal+pi3_2)),
    5,
    Drawings::ps_solid,
    (side4==LinesPercept::lineOnThisSide) ? Drawings::white : ((side4==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
  );    
#endif
  // end of drawing 
}

bool VLCLineFinder_DeterministicApproach::handleCenterCircle(const Vector2<double> & centerOnField, const CameraMatrix& cameraMatrix, const Image& image)
{
  int i;
  Vector2<int> pointOnField;
  Vector2<int> center2((int)centerOnField.x,(int)centerOnField.y);
  double distance;
  for (i=0; i<numberOfLinePoints; i++)
  {
    if (Geometry::calculatePointOnField(linePoints[i].pointOnLine.x, linePoints[i].pointOnLine.y, cameraMatrix, image.cameraInfo, pointOnField))
    {
      distance = (pointOnField-center2).abs();
      if ( distance > 80 && distance < 250 )
      {
        linePoints[i].belongsToLineNo = -2;
      }
    }    
  }
  return true;
}

bool VLCLineFinder_DeterministicApproach::handleCenterCircle(const CameraMatrix& cameraMatrix, const Image& image, const RobotPose & robotPose)
{
  int i,j;
  Vector2<double> temp1,temp2;
  Vector2<int> temp3,temp4;

  double orientationOfCircleCandidate1 = 0.0;
  //double orientationOfCircleCandidate2 = 0.0;
  bool candidateFound1 = false;
  bool candidateFound2 = false;

  double distanceToMiddleLine;
  int possibleMiddleLine = 0;
  int numberOfPointsOfPossibleMiddleLine;
  double bestDistanceToPossibleMiddleLine;

  bool possibleTangent[maxNumberOfLines*2];
  for (i=0; i<numberOfLineFragments; i++)
  {
    possibleTangent[i]=false;
  }
  Geometry::Line linesOnField[maxNumberOfLines*2];
  bool onFieldAvailable[maxNumberOfLines*2];
  Vector2<double> base,direction;
  for (i=0; i<numberOfLineFragments; i++)
  {
    onFieldAvailable[i] = calculateLineOnField(lineFragments[i].base, lineFragments[i].normal, base, direction, cameraMatrix, image);
    linesOnField[i].base = base;
    linesOnField[i].direction = direction.rotateLeft(); // normal (on the field) to line    
  }
  
  
  // first test for a near circle:
  // - lots of tangents are detected, and their normals on the field should cross in the center of the circle
  int numberOfGoodCrossings = 0;
  double deviation1 = 0.0;
  double deviation2 = 0.0;
  Vector2<double> intersection,centerCandidate1,centerCandidate2;
  double distance1,distance2,projection;
  for (i=0; i<numberOfLineFragments; i++)
  {
    if (!onFieldAvailable[i])
    { // line could not be calculated (maybe too near to the horizon, so it isn't of interest anyway)
      continue;
    }
    for (j=i+1; j<numberOfLineFragments; j++)
    {
      if (!onFieldAvailable[j])
      { // line could not be calculated (maybe too near to the horizon, so it isn't of interest anyway)
        continue;
      }
      Geometry::getIntersectionOfLines(linesOnField[i], linesOnField[j], intersection);
      distance1 = (linesOnField[i].base-intersection).abs();
      distance2 = (linesOnField[j].base-intersection).abs();

      // In a situation with a real circle, one nearly never get tangents, which are perpendicular,
      // but most likely some on the upper side and some on the lower,
      // both only varying only by up to 50° or something.
      // On the other hand, the goal arena or the outer part of the goal
      // quite often provides lines, that would be nice tangents
      // for a hypothetical center circle, and sometimes these are even
      // verified by some linefragment that could be taken for a middle line. (Look 50 lines below.)
      // BUT in these situations, the "tangents" are perpendicular on the field, and therefore easy to identify.
      projection = fabs( linesOnField[i].direction * linesOnField[j].direction );

      // perfect match would be both distances equal 180mm (=radius)  (or a little bit less, because actually it's only approximately tangent, and more a secant)
      if ( (distance1 < 260) && (distance1 > 130) && (distance2 < 260) && (distance2 > 130) ) //&& (projection > 0.4) )
      {
        if (projection < 0.4)
        {
          continue;
        }
        deviation1 += fabs(distance1-180) + fabs(distance2-180);
        if (numberOfGoodCrossings>0)
        {
          deviation2 += (centerCandidate1 - intersection).abs();
        }
        centerCandidate1 = (centerCandidate1 * (double)numberOfGoodCrossings + intersection) / (numberOfGoodCrossings+1.0);
        numberOfGoodCrossings++;
        possibleTangent[i]=true;
        possibleTangent[j]=true;
      }
    }
  }
  if (numberOfGoodCrossings>0)
  {
    deviation1 /= numberOfGoodCrossings*2;
    deviation2 /= numberOfGoodCrossings;
  }

  // was the first search successful?
  if (numberOfGoodCrossings>=1 && deviation1<100 && deviation2<200)
  {
    bestDistanceToPossibleMiddleLine = 10000.0;
    numberOfPointsOfPossibleMiddleLine = 0;
    // verify candidate by checking, if it is crossed by the middle line
    for (i=0; i<numberOfLineFragments; i++)
    {
      if (!onFieldAvailable[i])
      { // line could not be calculated (maybe too near to the horizon, so it isn't of interest anyway)
        continue;
      }
      distanceToMiddleLine = fabs( (centerCandidate1-linesOnField[i].base)*linesOnField[i].direction );
      if (distanceToMiddleLine < 60) // is the line very near to the center of the circle? (should be true for at least one line, because the centerline is always detected when the circle is seen)
      {
        // prefere lines with more points, because for these it's more likely to be a middle line
        if (lineFragments[i].numberOfPoints <= numberOfPointsOfPossibleMiddleLine) // || distanceToMiddleLine >= bestDistanceToPossibleMiddleLine
        {
          continue;
        }
        if ( Geometry::calculatePointOnField(lineFragments[i].start.x,lineFragments[i].start.y,cameraMatrix,image.cameraInfo,temp3)
            && Geometry::calculatePointOnField(lineFragments[i].end.x,lineFragments[i].end.y,cameraMatrix,image.cameraInfo,temp4) )
        {
          temp1.x = centerCandidate1.x - (double)temp3.x;
          temp1.y = centerCandidate1.y - (double)temp3.y;
          temp2.x = centerCandidate1.x - (double)temp4.x;
          temp2.y = centerCandidate1.y - (double)temp4.y;
          if (temp1 * temp2 < 0) // is the middle line detected on both sides of the center?
          {
            candidateFound1=true;
            orientationOfCircleCandidate1 = pi_2 + atan2(linesOnField[i].direction.y,linesOnField[i].direction.x);
            bestDistanceToPossibleMiddleLine = distanceToMiddleLine;
            possibleMiddleLine = i;
            numberOfPointsOfPossibleMiddleLine = lineFragments[i].numberOfPoints;
          }
        }
      }
      /*
      else if (distanceToMiddleLine < 180 && lineFragments[i].numberOfPoints <= numberOfPointsOfPossibleMiddleLine) // would not be enough to verify a candidate, but if already verified, then project the canidate to this middle line
      {
        if ( Geometry::calculatePointOnField(lineFragments[i].start.x,lineFragments[i].start.y,cameraMatrix,image.cameraInfo,temp3)
            && Geometry::calculatePointOnField(lineFragments[i].end.x,lineFragments[i].end.y,cameraMatrix,image.cameraInfo,temp4) )
        {
          temp1.x = centerCandidate1.x - (double)temp3.x;
          temp1.y = centerCandidate1.y - (double)temp3.y;
          temp2.x = centerCandidate1.x - (double)temp4.x;
          temp2.y = centerCandidate1.y - (double)temp4.y;
          if (temp1 * temp2 < 0) // is the middle line detected on both sides of the center?
          {
            orientationOfCircleCandidate1 = pi_2 + atan2(linesOnField[i].direction.y,linesOnField[i].direction.x);
            bestDistanceToPossibleMiddleLine = distanceToMiddleLine;
            possibleMiddleLine = i;
            numberOfPointsOfPossibleMiddleLine = lineFragments[i].numberOfPoints;
          }
        }
      }
      */
    }
  }

  
  // project the candidate to the associated middle line
  if (candidateFound1 && bestDistanceToPossibleMiddleLine > 1.0) // to prevent division by zero and to much unnecessary work
  {
    temp1 = centerCandidate1 - linesOnField[possibleMiddleLine].base;
    temp2 = linesOnField[possibleMiddleLine].direction;
    centerCandidate1 -= temp2*(temp1*temp2);
  }
  


  /*
  // now test for a far away circle:
  // - here 2 parallel tangents can be observed, that should have a distance of 2*radius on the field
  // - the center indicated by these 2 lines should additionally be crossed by another line, the centerline

  int numberOfGoodPossibleCandidates = 0;
  Vector2<double> possibleCandidate;
  if (!candidateFound1)
  {
    for (i=0; i<numberOfLineFragments; i++)
    {
      if (!onFieldAvailable[i])
      { // line could not be calculated (maybe too near to the horizon, so it isn't of interest anyway)
        continue;
      }
      for (j=i+1; j<numberOfLineFragments; j++)
      {
        if (!onFieldAvailable[j])
        { // line could not be calculated (maybe too near to the horizon, so it isn't of interest anyway)
          continue;
        }

        // check if the 2 lines are nearly parallel and exactly 2*radius away from each other
        if ( fabs( linesOnField[i].direction * linesOnField[j].direction ) > 0.9 )
        {
          direction = (linesOnField[i].direction + linesOnField[j].direction).normalize();
          distance1 = (linesOnField[i].base - linesOnField[j].base)*direction;
          if ( (distance1 < 380) && (distance1 > 300) )
          {
            possibleCandidate = (linesOnField[i].base + linesOnField[j].base)/2.0;
            deviation1 += fabs(distance1-360);
            if (numberOfGoodPossibleCandidates>0)
            {
              deviation2 += (centerCandidate2 - possibleCandidate).abs();
            }
            centerCandidate2 = (centerCandidate2 * (double)numberOfGoodPossibleCandidates + possibleCandidate) / (numberOfGoodPossibleCandidates+1.0);
            numberOfGoodPossibleCandidates++;
            possibleTangent[i]=true;
            possibleTangent[j]=true;
          }

        }
      }
    }
  }
  if (numberOfGoodPossibleCandidates>0)
  {
    deviation1 /= numberOfGoodPossibleCandidates;
    deviation2 /= numberOfGoodPossibleCandidates;
  }

  // was the second search successful?
  if (numberOfGoodPossibleCandidates>=1 && deviation1<200 && deviation2<300)
  {
    // verify candidate by checking, if it is crossed by the middle line
    for (i=0; i<numberOfLineFragments; i++)
    {
      if (!onFieldAvailable[i])
      { // line could not be calculated (maybe too near to the horizon, so it isn't of interest anyway)
        continue;
      }
      if (fabs( (centerCandidate2-linesOnField[i].base)*linesOnField[i].direction ) < 100) // is the line very near to the center of the circle? (should be true for at least one line, because the centerline is always detected when the circle is seen)
      {
        candidateFound2=true;
        orientationOfCircleCandidate2 = pi_2 + atan2(linesOnField[i].direction.y,linesOnField[i].direction.x);
      }
    }
  }
  */
  

  // exclude the tangents from further calculation of the lines
  Vector2<int> circleCandidateInImage,temp;
  Vector2<double> circleOnFieldGlobal;
  if (candidateFound1)
  {
    for (i=0; i<numberOfLineFragments; i++)
    {
      distance1 = (centerCandidate1-linesOnField[i].base).abs();
      if ( possibleTangent[i] || (distance1<280 && distance1>120) )
      {
        lineFragments[i].lineFragmentAlreadyConsidered = true;
      }
    }

    // it is still right to exclude tangents, because the next check also filters 
    // out correct center circles with the wrong associated middle line.

    fromOwnHandling.x = (int)centerCandidate1.x;
    fromOwnHandling.y = (int)centerCandidate1.y;
    centerCircleOrientation = orientationOfCircleCandidate1;

    // one last check if it is really the correct center circle with the correct orientation
    foundByOwnHandling = doVerificationScanForCircle(fromOwnHandling,centerCircleOrientation,cameraMatrix,image);

    if (foundByOwnHandling)
    {
      double orientationOnField = centerCircleOrientation + robotPose.rotation;

      circleOnFieldGlobal = Geometry::relative2FieldCoord(robotPose,centerCandidate1.x,centerCandidate1.y);

      lastSeenCircleGlobal = circleOnFieldGlobal;
      lastSeenCircleOrientationGlobal = orientationOnField;
      framesSinceLastSeen = 0;
      
      CIRCLE(circlePoints_Field,circleOnFieldGlobal.x,circleOnFieldGlobal.y,180,3,Drawings::ps_solid,Drawings::skyblue);
      LINE(circlePoints_Field,(circleOnFieldGlobal.x+180*cos(orientationOnField)),(circleOnFieldGlobal.y+180*sin(orientationOnField)),(circleOnFieldGlobal.x-180*cos(orientationOnField)),(circleOnFieldGlobal.y-180*sin(orientationOnField)),3,Drawings::ps_solid,Drawings::skyblue);
      //OUTPUT(idText,text, "MSHLineFinder: " << "centerCandidate1 found!");
      temp.x = (int)centerCandidate1.x;
      temp.y = (int)centerCandidate1.y;
      Geometry::calculatePointInImage(temp,cameraMatrix,image.cameraInfo,circleCandidateInImage);
      CIRCLE(circlePoints_image,circleCandidateInImage.x,circleCandidateInImage.y,3,1,Drawings::ps_solid,Drawings::skyblue);

      // for validity tests
      //OUTPUT(idText,text, "MSHLineFinder: " <<  robotPose.translation.x << " " << robotPose.translation.y << " " << circleOnFieldGlobal.x << " " << circleOnFieldGlobal.y);

      // test of orientation validity
      // OUTPUT(idText,text, "MSHLineFinder: " <<  centerCircleOrientation + robotPose.rotation );
    }
    
  }
  /*
  else if(false && candidateFound2) // skip (doesn't work good enough, yet)
  {
    for (i=0; i<numberOfLineFragments; i++)
    {
      distance2 = (centerCandidate2-linesOnField[i].base).abs();
      if ( possibleTangent[i] || (distance2<280 && distance2>120) )
      {
        lineFragments[i].lineFragmentAlreadyConsidered = true;
      }
    }

    fromOwnHandling.x = (int)centerCandidate2.x;
    fromOwnHandling.y = (int)centerCandidate2.y;
    foundByOwnHandling = true;
    centerCircleOrientation = orientationOfCircleCandidate2;

    circleOnFieldGlobal = Geometry::relative2FieldCoord(robotPose,centerCandidate2.x,centerCandidate2.y);
    
    CIRCLE(circlePoints_Field,circleOnFieldGlobal.x,circleOnFieldGlobal.y,180,3,Drawings::ps_solid,Drawings::skyblue);
    //OUTPUT(idText,text, "MSHLineFinder: " << "centerCandidate2 found!");
    temp.x = (int)centerCandidate2.x;
    temp.y = (int)centerCandidate2.y;
    Geometry::calculatePointInImage(temp,cameraMatrix,image.cameraInfo,circleCandidateInImage);
    CIRCLE(circlePoints_image,circleCandidateInImage.x,circleCandidateInImage.y,3,1,Drawings::ps_solid,Drawings::skyblue);    
  }
  */

  
  if (!candidateFound1 && framesSinceLastSeen<60)
  {
    Drawings::Color c;
    if (framesSinceLastSeen<2)
    {
      c = Drawings::white;
    }
    else if (framesSinceLastSeen<10)
    {
      c = Drawings::light_gray;
    }
    else if (framesSinceLastSeen<30)
    {
      c = Drawings::gray;
    }
    else
    {
      c = Drawings::dark_gray;
    }
    CIRCLE(circlePoints_Field,lastSeenCircleGlobal.x,lastSeenCircleGlobal.y,180,3,Drawings::ps_solid,c);
    LINE(circlePoints_Field,(lastSeenCircleGlobal.x+180*cos(lastSeenCircleOrientationGlobal)),(lastSeenCircleGlobal.y+180*sin(lastSeenCircleOrientationGlobal)),(lastSeenCircleGlobal.x-180*cos(lastSeenCircleOrientationGlobal)),(lastSeenCircleGlobal.y-180*sin(lastSeenCircleOrientationGlobal)),3,Drawings::ps_solid,c);   
    
  }

  // recognition rate test
  // OUTPUT(idText,text, "MSHLineFinder: " <<  (candidateFound1 ? 1 : 0) );

  return (candidateFound1 || candidateFound2);
}

void VLCLineFinder_DeterministicApproach::addCenterCirclePercept(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image)
{
  int i;
  if (foundByCircleFinder)
  {
    // dertermine orientation first
    double orientation = 0.0;
    bool orientationFound = false;

    Vector2<double> fromCircleFinder2((double)fromCircleFinder.x, (double)fromCircleFinder.y);

    double distance;
    double smallestDistance = 10000.0;

    Geometry::Line linesOnField[maxNumberOfLines];
    bool onFieldAvailable[maxNumberOfLines];
    Vector2<double> base,direction;
    for (i=0; i<numberOfLines; i++)
    {
      onFieldAvailable[i] = calculateLineOnField(i, base, direction, cameraMatrix, image);
      linesOnField[i].base = base;
      linesOnField[i].direction = direction.rotateLeft(); // normal (on the field) to line    
    }
    for (i=0; i<numberOfLines; i++)
    {
      distance = fabs( (fromCircleFinder2-linesOnField[i].base)*linesOnField[i].direction );
      if (distance < 80 && distance < smallestDistance) // is the line very near to the center of the circle? (should be true for at least one line, because the centerline is always detected when the circle is seen, if it isn't filtered out by accident or if the centerCircle lies at the wrong location (not my fault!))   ;-)
      {
        orientationFound=true;
        orientation = atan2(linesOnField[i].direction.y, linesOnField[i].direction.x);
        orientation += pi_2; // because "direction" was the normal to the line
      }
    }
    
    linesPercept.centerCircle.location = fromCircleFinder;
    linesPercept.centerCircle.orientation = orientation;
    linesPercept.centerCircle.orientationKnown = orientationFound;
    linesPercept.centerCircleFound = true;
  }
  else if (foundByOwnHandling)
  {
    linesPercept.centerCircle.location = fromOwnHandling;
    linesPercept.centerCircle.orientation = centerCircleOrientation;
    linesPercept.centerCircle.orientationKnown = true;
    linesPercept.centerCircleFound = true;
  }
}


int VLCLineFinder_DeterministicApproach::getNumberOfLines()
{
  return numberOfLines;
}

void VLCLineFinder_DeterministicApproach::getLine(int number, Vector2<int> & pointOnLine, Vector2<double> & normalToLine)
{
  pointOnLine.x = lines[number].base.x;
  pointOnLine.y = lines[number].base.y;
  normalToLine.x = lines[number].normal.x;
  normalToLine.y = lines[number].normal.y;
}


bool VLCLineFinder_DeterministicApproach::linesPerpendicularOnField(int lineNumber1, int lineNumber2, const CameraMatrix& cameraMatrix, const Image& image)
{
  Vector2<double> justAPoint, direction1, direction2;
  if ( calculateLineOnField(lineNumber1, justAPoint, direction1, cameraMatrix, image)
    && calculateLineOnField(lineNumber2, justAPoint, direction2, cameraMatrix, image) )
  {
    if (fabs(direction1*direction2) < 0.5) // directions include an angle of approximately 90° (60°..120°)
    {
      return true;
    }
  }
  return false;
}

bool VLCLineFinder_DeterministicApproach::calculateLineOnField(int lineNumber, Vector2<double> & base, Vector2<double> & direction, const CameraMatrix& cameraMatrix, const Image& image)
{
  Vector2<int> pointOnField1, pointOnField2, directionOnField;
  if ( Geometry::calculatePointOnField(lines[lineNumber].base.x, lines[lineNumber].base.y, cameraMatrix, image.cameraInfo, pointOnField1)
    && Geometry::calculatePointOnField((int)(lines[lineNumber].base.x+50*lines[lineNumber].normal.y), (int)(lines[lineNumber].base.y-50*lines[lineNumber].normal.x), cameraMatrix, image.cameraInfo, pointOnField2))
  {
    directionOnField = pointOnField2-pointOnField1;
    base.x = (double)pointOnField1.x;
    base.y = (double)pointOnField1.y;
    direction.x = (double)directionOnField.x;
    direction.y = (double)directionOnField.y;
    direction.normalize();  
    return true;
  }
  return false;
}


bool VLCLineFinder_DeterministicApproach::calculateLineOnField(const Vector2<int> & baseInImage, const Vector2<double> & normalInImage, Vector2<double> & baseOnField, Vector2<double> & directionOnField, const CameraMatrix& cameraMatrix, const Image& image)
{
  Vector2<int> pointOnField1, pointOnField2, directionOnFieldTemp;
  if ( Geometry::calculatePointOnField(baseInImage.x, baseInImage.y, cameraMatrix, image.cameraInfo, pointOnField1)
    && Geometry::calculatePointOnField((int)(baseInImage.x+50*normalInImage.y), (int)(baseInImage.y-50*normalInImage.x), cameraMatrix, image.cameraInfo, pointOnField2))
  {
    directionOnFieldTemp = pointOnField2-pointOnField1;
    baseOnField.x = (double)pointOnField1.x;
    baseOnField.y = (double)pointOnField1.y;
    directionOnField.x = (double)directionOnFieldTemp.x;
    directionOnField.y = (double)directionOnFieldTemp.y;
    directionOnField.normalize();
    return true;
  }
  return false;
}

void VLCLineFinder_DeterministicApproach::doVerificationScan(const Vector2<double> & crossingPoint, const Vector2<double> & directionToScanAt, const Vector2<double> & scanningDirection, int lineSize, LinesPercept::CrossingCharacteristic & result, const CameraMatrix& cameraMatrix, const Image& image)
{
  // preparations
  int i,j;
  double scanningDistance = max(6.0, lineSize*1.5);
  double projection = fabs( sin( atan2(directionToScanAt.y,directionToScanAt.x) - atan2(scanningDirection.y,scanningDirection.x) ) );
  if (projection > 0) // should never be otherwise
  {
    scanningDistance /= projection;
  }
  double directionAngle = atan2(scanningDirection.y,scanningDirection.x);

  int numberOfScanLines = 2;
  double distanceBetweenScanLines = 2.0;
  bool greenFound[2]; // numberOfScanLines
  bool whiteFound[2]; // numberOfScanLines
  bool robotColorFound[2]; // numberOfScanLines
  Vector2<int> actual;
  unsigned char y,u,v;
  colorClass color;
  Vector2<double> scanningStart;
  Vector2<int> scanningStart2;

  for (i=0; i<numberOfScanLines; i++)
  {
    scanningStart = crossingPoint + directionToScanAt * (scanningDistance + i*distanceBetweenScanLines) - scanningDirection * scanningDistance;
    scanningStart2.x = (int)scanningStart.x;
    scanningStart2.y = (int)scanningStart.y;
    BresenhamLineScan bls(scanningStart2, directionAngle, image.cameraInfo);
    actual = scanningStart2;

    greenFound[i]=false;
    whiteFound[i]=false;
    robotColorFound[i]=false;

    // begin scanning
    for (j=0; j<= (int)(2.0*scanningDistance); j++)
    {
      if ( actual.x < 0 || actual.y < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y >= image.cameraInfo.resolutionHeight )
      {
        break;
      }      
      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]);
      color = COLOR_CLASS(y, u, v, (*bestColorTable));

      switch (color)
      {
        case green:
          greenFound[i]=true;
          break;
        case white:
          whiteFound[i]=true;
          break;
        case red:
        case blue:
          robotColorFound[i]=true;
        default:
          break;
      }

      bls.getNext(actual);
    }
    LINE(imageProcessor_lines,scanningStart2.x,scanningStart2.y,actual.x,actual.y,1,Drawings::ps_solid,Drawings::gray);
  }

  // analyse scanning results (perhaps later in a more sophisticated way)
  bool robotInTheWay = false;
  bool greenSeen = false;
  bool whiteSeen = false;
  bool whiteAlwaysSeen = true;
  for (i=0; i<numberOfScanLines; i++)
  {
    robotInTheWay = robotInTheWay || robotColorFound[i];
    greenSeen = greenSeen || greenFound[i];
    whiteSeen = whiteSeen || whiteFound[i];
    whiteAlwaysSeen = whiteAlwaysSeen && whiteFound[i];
  }
  if (robotInTheWay)
  {
    result = LinesPercept::dontKnow;
  }
  else
  {
    if ( lineSize > 4 ) // there should be a quite big line, so most scan lines should have seen white
    {
      if (!whiteAlwaysSeen)
      {
        if (greenSeen)
        {
          result = LinesPercept::noLineOnThisSide;
        }
        else
        {
          result = LinesPercept::dontKnow;
        }
      }
      else // white was seen
      {
        if (greenSeen)
        {
          result = LinesPercept::lineOnThisSide;
        }
        else
        {
          result = LinesPercept::dontKnow;
        }
      }
    }
    else // the line might be quite small, so a scan line might not see white just by chance/bad colortable/blur
    {
      if (!whiteSeen)
      {
        if (greenSeen)
        {
          result = LinesPercept::noLineOnThisSide;
        }
        else
        {
          result = LinesPercept::dontKnow;
        }
      }
      else // white was seen
      {
        if (greenSeen)
        {
          result = LinesPercept::lineOnThisSide;
        }
        else
        {
          result = LinesPercept::dontKnow;
        }
      }
    }
  }  
}

bool VLCLineFinder_DeterministicApproach::doVerificationScanForCircle(
  const Vector2<int> & circleOnFieldRelative,
  double orientation,
  const CameraMatrix& cameraMatrix,
  const Image& image)
{
  int i,j;

  const int numberOfScanlines = 4;

  bool imageBorderFound[numberOfScanlines], circleLineFound[numberOfScanlines], robotColorFound[numberOfScanlines];
  
  double scanAngle[numberOfScanlines];
  scanAngle[0] = orientation + 0.33333 * pi;
  scanAngle[1] = orientation + 0.66666 * pi;
  scanAngle[2] = orientation - 0.33333 * pi;
  scanAngle[3] = orientation - 0.66666 * pi;
  Vector2<double> scanDirection[numberOfScanlines];
  Vector2<int> scanningStart[numberOfScanlines],scanningEnd[numberOfScanlines];
  Vector2<int> scanningStartOnField[numberOfScanlines],scanningEndOnField[numberOfScanlines];
  double scanDistance;

  for (i=0; i<numberOfScanlines; i++)
  {
    imageBorderFound[i] = circleLineFound[i] = robotColorFound[i] = false;

    scanDirection[i].x = cos(scanAngle[i]);
    scanDirection[i].y = sin(scanAngle[i]);
    scanAngle[i] = normalize(scanAngle[i]);
    if (scanAngle[i] > -1.0 * pi_2 && scanAngle[i] < pi_2)
    {
      scanDistance = 500.0; // otherwise the verification scan is too horizon dependant
    }
    else
    {
      scanDistance = 300.0;
    }
    scanningStartOnField[i].x = int(circleOnFieldRelative.x + 50.0*scanDirection[i].x);
    scanningStartOnField[i].y = int(circleOnFieldRelative.y + 50.0*scanDirection[i].y);
    scanningEndOnField[i].x = int(circleOnFieldRelative.x + scanDistance*scanDirection[i].x);
    scanningEndOnField[i].y = int(circleOnFieldRelative.y + scanDistance*scanDirection[i].y);
    Geometry::calculatePointInImage(scanningStartOnField[i],cameraMatrix,image.cameraInfo,scanningStart[i]);
    Geometry::calculatePointInImage(scanningEndOnField[i],cameraMatrix,image.cameraInfo,scanningEnd[i]);
  
    Vector2<int> actual = scanningStart[i];
    unsigned char y,u,v;
    colorClass color;

    BresenhamLineScan bls(scanningStart[i], scanningEnd[i]);

    bool firstGreen = false;
    bool whiteFound = false;

    int pixelsToScan = max(15,bls.numberOfPixels); // better for circles that are not very near
    // begin scanning
    for (j=0; j<=pixelsToScan; j++)
    {
      if ( actual.x < 0 || actual.y < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y >= image.cameraInfo.resolutionHeight )
      {
        imageBorderFound[i] = true;
        break;
      }      
      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]);
      color = COLOR_CLASS(y, u, v, (*bestColorTable));

      switch (color)
      {
        case green:
          if (whiteFound)
          {
            // green again
            circleLineFound[i] = true;
          }
          else
          {
            firstGreen = true;
          }
          break;
        case white:
          if (firstGreen)
          {
            whiteFound=true;
          }
          break;
        case red:
        case blue:
          robotColorFound[i]=true;
        default:
          break;
      }

      bls.getNext(actual);
    }
    LINE(circlePoints_image,scanningStart[i].x,scanningStart[i].y,actual.x,actual.y,1,Drawings::ps_solid,Drawings::gray);
  }

  // put the results together
  // (If a scan hit a border, than it is assumed that there may be a circle line, 
  // because most false positive situations are resolved with scanning 'up',
  // while many circles are only seen half, and thus scans 'down' will nearly always hit the border of the image.) 
  // TODO: Include robotColor somehow...
  bool result =    (imageBorderFound[0] || circleLineFound[0])
                && (imageBorderFound[1] || circleLineFound[1])
                && (imageBorderFound[2] || circleLineFound[2])
                && (imageBorderFound[3] || circleLineFound[3]);

  //OUTPUT(idText,text, "MSHLineFinder: " << "CircleVerification returns: " << result);
  
  if (result)
  {
    circleCount++;
  }

  return result;  
}

---