Modules/ImageProcessor/SlamImageProcessor/SlamBorderFinder.cpp

Go to the documentation of this file.

/**
* @file SlamBorderFinder.h
*
* Implementation of class SlamBorderFinder
*
* @author <a href="mailto:stefan.czarnetzki@uni-dortmund.de">Stefan Czarnetzki</a>
*/


#include "SlamBorderFinder.h"

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

SlamBorderFinder::~SlamBorderFinder(void)
{
}

void SlamBorderFinder::reset()
{
  numberOfLineFragments = 0;
  numberOfLines = 0;
  numberOfLinePoints = 0;
}

void SlamBorderFinder::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 SlamBorderFinder::execute(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image, const ImageInfo& imageInfo, const RobotPose & robotPose)
{
  NDECLARE_DEBUGDRAWING ("SlamIMP:borderDirection", "drawingOnField", "shows the direction away from the border");
  

  lines = new LineFragment[maxNumberOfLines];
  lineFragments = new LineFragment[maxNumberOfLines*2];

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

  delete[] lines;
  delete[] lineFragments;

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

void SlamBorderFinder::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();
    // let the normal point in the same direction like all the normals of the points
    if (normalToLine*linePoints[linePointWithHighestWeight].normalToLine < 0)
    {
      normalToLine *= -1;
    }
    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 SlamBorderFinder::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::blue
      );*/
  }
}

void SlamBorderFinder::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::blue); 
  }
}

void SlamBorderFinder::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 
}

void SlamBorderFinder::calculateDirectionAwayFromBorder(
    LinesPercept & linesPercept,
    const Image& image,
    const ImageInfo& imageInfo,
    const CameraMatrix & cameraMatrix,
    const RobotPose & robotPose)
{
  

  int i;
  Vector2<double> temp1,temp2;

  // calculate it from the points directly
  Vector2<double> fromPoints;
  Vector2<double> nearestPoint;
  double nearestDistance = 100000;
  for (i=0; i<numberOfLinePoints; i++)
  {
    if (calculateLineOnField(linePoints[i].pointOnLine,linePoints[i].normalToLine,temp1,temp2,cameraMatrix,image))
    {
      fromPoints += temp2;
      double distance = temp1.abs();
      if (distance<nearestDistance)
      {
        nearestDistance=distance;
        nearestPoint = temp1;
      }
    }
  }
  nearestPoint = Geometry::relative2FieldCoord(robotPose,nearestPoint.x,nearestPoint.y);
  fromPoints.rotateRight();
  fromPoints.normalize();
  NARROW(
    "SlamIMP:borderDirection",
    nearestPoint.x,
    nearestPoint.y,
    (nearestPoint.x + 300*fromPoints.x),
    (nearestPoint.y + 300*fromPoints.y),
    20,
    Drawings::ps_solid,
    Drawings::light_gray
  );
  NARROW(
    "SlamIMP:borderDirection",
  robotPose.translation.x,
    robotPose.translation.y,
    (robotPose.translation.x + 300*fromPoints.x),
    (robotPose.translation.y + 300*fromPoints.y),
    20,
    Drawings::ps_solid,
    Drawings::light_gray
  );
 
  if (numberOfLineFragments>0)
  {
  // calculate it from the fragments
  Vector2<double> fromFragments;
  Vector2<double> nearestFragment;
  Vector2<double> nearestFragmentsNormal;
  nearestDistance = 100000;
  for (i=0; i<numberOfLineFragments; i++)
  {
    if (calculateLineOnField(lineFragments[i].base,lineFragments[i].normal,temp1,temp2,cameraMatrix,image))
    {
    fromFragments += temp2;
    double distance = temp1.abs();
    if (distance<nearestDistance)
    {
      nearestDistance=distance;
      nearestFragment = temp1;
      nearestFragmentsNormal = temp2;
    }
    }
  }
  nearestFragment = Geometry::relative2FieldCoord(robotPose,nearestFragment.x,nearestFragment.y);
  fromFragments.normalize();
  fromFragments.normalize();
  NARROW(
    "SlamIMP:borderDirection",
    nearestFragment.x,
    nearestFragment.y,
    (nearestFragment.x + 300*fromFragments.x),
    (nearestFragment.y + 300*fromFragments.y),
    20,
    Drawings::ps_solid,
    Drawings::blue
  );
  NARROW(
    "SlamIMP:borderDirection",
    nearestFragment.x,
    nearestFragment.y,
    (nearestFragment.x + 200*nearestFragmentsNormal.x),
    (nearestFragment.y + 200*nearestFragmentsNormal.y),
    20,
    Drawings::ps_solid,
    Drawings::skyblue
  );
  }
}

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

void SlamBorderFinder::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 SlamBorderFinder::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 SlamBorderFinder::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 SlamBorderFinder::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 SlamBorderFinder::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);

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

---