Modules/ImageProcessor/SlamImageProcessor/SlamFlagSpecialist.cpp

Go to the documentation of this file.

/**
* @file SlamFlagSpecialist.cpp
*
* This file contains a class for Image Processing.
* @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Juengel</A>
*/

#include "Tools/FieldDimensions.h"
#include "Tools/Player.h"
#include "Tools/Math/Common.h"
#include "Tools/Debugging/View.h"
#include "Modules/ImageProcessor/ImageProcessorTools/ColorCorrector.h"
#include "SlamFlagSpecialist.h"
#include "SlamImageProcessorTools.h"


SlamFlagSpecialist::SlamFlagSpecialist(const ColorCorrector& colorCorrector) :
  colorCorrector(colorCorrector)
{
}

void SlamFlagSpecialist::init(const Image& image)
{
  for(int i = 0; i < 6; i++)
  {
    numberOfBoundingBoxes[i] = 0;
  }
  INIT_DEBUG_IMAGE(imageProcessorFlags, image);
}

void SlamFlagSpecialist::searchFlags
(
 const Image& image, 
 const ColorTable& colorTable,
 const CameraMatrix& cameraMatrix,
 colorClass color,
 bool pinkIsTop,
 const Geometry::Line horizonLine,
 int x, int y
 )
{
  CameraInfo bwCameraInfo = image.cameraInfo;
  bwCameraInfo.resolutionHeight*=2;
  bwCameraInfo.resolutionWidth*=2;
  bwCameraInfo.focalLength*=2;
  bwCameraInfo.focalLengthInv/=2;
  bwCameraInfo.opticalCenter.x*=2;
  bwCameraInfo.opticalCenter.y*=2;    
  bool gridPointIsCovered = false;

  // x and y are in old resolution, so no division
  DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorFlags, x, y)
  
  int i, j;
  
  for(i = 0; i < 6; i++)
  {
    for(j = 0; j < numberOfBoundingBoxes[i]; j++)
    {
      if(x >= boundingBoxLeft[j][i] && x <= boundingBoxRight[j][i] &&
        y >= boundingBoxBottom[j][i] && y <= boundingBoxTop[j][i] )
      {
        gridPointIsCovered = true;
      }
    }// j
  }//i

  if(gridPointIsCovered) return;

  int pixelCounter = 0;
  int topPixelCounter = 0;
  int bottomPixelCounter = 0;
  
  start.x = 2*x;
  start.y = 2*y;
  
  //find up
  findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, 
    up, north, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
  //find down
  findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, 
    down, south, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
  DOT(imageProcessor_ball4, south.x/2, south.y/2, Drawings::black, Drawings::green);
  //find right
  findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, 
    right, east, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
  //find left
  findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, 
    left, west, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
  
  DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorFlags, north.x/2, north.y/2);
  DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorFlags, east.x/2, east.y/2);
  DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorFlags, south.x/2, south.y/2);
  DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorFlags, west.x/2, west.y/2);

  if(west == east)
  {
    Vector2<int> newStart((north.x + south.x)/2, (north.y + south.y)/2);
    //find right
    findEndOfFlag(image, bwCameraInfo, colorTable, newStart, horizonLine.direction, 
      right, east, color, pinkIsTop, false, horizontal, pixelCounter, topPixelCounter, bottomPixelCounter);
    //find left
    findEndOfFlag(image, bwCameraInfo, colorTable, newStart, horizonLine.direction, 
      left, west, color, pinkIsTop, false, horizontal, pixelCounter, topPixelCounter, bottomPixelCounter);
  }
  //go to center of pink
  start.x = (west.x + east.x) / 2;
  start.y = (west.y + east.y) / 2;
  DOT(imageProcessor_ball4, start.x/2, start.y/2, Drawings::black, Drawings::red);
  DOT(imageProcessor_ball4, east.x/2, east.y/2, Drawings::black, Drawings::yellow);
  DOT(imageProcessor_ball4, west.x/2, west.y/2, Drawings::black, Drawings::yellow);

  DEBUG_IMAGE_SET_PIXEL_YELLOW(imageProcessorFlags, start.x/2, start.y/2);

  //find north
  findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, 
    up, north, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
  //find south
  findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, 
    down, south, color, pinkIsTop, pinkIsTop, initial, pixelCounter, topPixelCounter, bottomPixelCounter);
  
  double distance = sqrt((double)sqr(south.x-north.x) + sqr(south.y-north.y));

  // reset counters
  pixelCounter = 0;
  topPixelCounter = 0;
  bottomPixelCounter = 0;
  
  double rightDist = 0.0;
  double leftDist = 0.0;
  int rightValidCounter = 0;
  int leftValidCounter = 0;
  int scans = 0;

  Vector2<int> temp;

  Vector2<int> lastStart;
  lastStart.x = 1000;
  lastStart.y = 1000;

  //scan horizontal lines
  Vector2<double>step;
  step.x = (south.x - north.x) / (numberOfHorizontalScans + 1.0);
  step.y = (south.y - north.y) / (numberOfHorizontalScans + 1.0);
  
  Geometry::Line leftVerticalLine;
  leftVerticalLine.base = horizonLine.base;
  leftVerticalLine.direction.x = -horizonLine.direction.y;
  leftVerticalLine.direction.y = horizonLine.direction.x;

  bool pointIsGood;
  bool startIsInTop = true;
  for(i = 1; i <= numberOfHorizontalScans; i++)
  {
    if(i==(numberOfHorizontalScans + 1)/2) 
    {
      startIsInTop = false;
      //no scan in the middle of the flag
      continue;
    }

    start.x = (int)(north.x + step.x * i);
    start.y = (int)(north.y + step.y * i); 
    if ( start == lastStart ) continue;
    lastStart = start;

    scans++;

    //find right
    pointIsGood = findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, right, temp, 
        color, pinkIsTop, startIsInTop, horizontal, pixelCounter, topPixelCounter, bottomPixelCounter);
    double rightDistance = 0.5 + Geometry::getDistanceToLine(leftVerticalLine, Vector2<double>(temp.x,temp.y));
    //~ double rightDistance = 0.5 + Geometry::getDistanceToLine(leftVerticalLine, Vector2<double>(east.x,east.y));
    rightDist += rightDistance;
    if ( pointIsGood ) rightValidCounter++;

    //find left
    pointIsGood = findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, left, temp, 
        color, pinkIsTop, startIsInTop, horizontal, pixelCounter, topPixelCounter, bottomPixelCounter);
    double leftDistance = -0.5 + Geometry::getDistanceToLine(leftVerticalLine, Vector2<double>(temp.x,temp.y));
    //~ double leftDistance = -0.5 + Geometry::getDistanceToLine(leftVerticalLine, Vector2<double>(west.x,west.y));
    leftDist += leftDistance;
    if ( pointIsGood ) leftValidCounter++;
  }
  
  rightDist = rightDist / scans;
  bool rightValid = (double)rightValidCounter >= scans / 2;
  leftDist = leftDist / scans;
  bool leftValid = (double)leftValidCounter >= scans / 2;

  distance = sqrt((double)sqr(east.x-west.x) + sqr(east.y-west.y));
  
  //scan vertical lines
  step.x = (east.x - west.x) / (numberOfVerticalScans + 1.0);
  step.y = (east.y - west.y) / (numberOfVerticalScans + 1.0);
   
  double topDist = 0.0;
  double bottomDist = 0.0;
  int topValidCounter = 0;
  int bottomValidCounter = 0;
  scans = 0;
  lastStart.x = 1000;
  lastStart.y = 1000;

  for(i = 1; i <= numberOfVerticalScans; i++)
  {
    start.x = (int)(west.x + step.x * i);
    start.y = (int)(west.y + step.y * i); 

    if ( start == lastStart ) continue;
    lastStart = start;

    scans++;

    //find top
    pointIsGood = findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, up, north,
        color, pinkIsTop, pinkIsTop, vertical, pixelCounter, topPixelCounter, bottomPixelCounter);
    topDist += 0.5 + Geometry::getDistanceToLine(horizonLine, Vector2<double>(north.x,north.y));
    if ( pointIsGood ) topValidCounter++;

    //find bottom
    pointIsGood = findEndOfFlag(image, bwCameraInfo, colorTable, start, horizonLine.direction, down, south, 
        color, pinkIsTop, pinkIsTop, vertical, pixelCounter, topPixelCounter, bottomPixelCounter);
    bottomDist += -0.5 + Geometry::getDistanceToLine(horizonLine, Vector2<double>(south.x,south.y));
    if ( pointIsGood ) bottomValidCounter++;
  }

  topDist = topDist / scans;
  bool topValid = (double)topValidCounter >= scans / 2;
  bottomDist = bottomDist / scans;
  bool bottomValid = (double)bottomValidCounter >= scans / 2;

  double topRatio = (double)topPixelCounter / (double)pixelCounter;
  double bottomRatio = (double)bottomPixelCounter / (double)pixelCounter;

  if (topRatio > 0.2 && bottomRatio > 0.2) // was 0.1
  {
    Flag::FlagType flagType = Flag::pinkAboveYellow;
    switch(color)
    {
      case yellow:  flagType = pinkIsTop ? Flag::pinkAboveYellow  : Flag::yellowAbovePink;  break;
      // case green:   flagType = pinkIsTop ? Flag::pinkAboveGreen   : Flag::greenAbovePink;   break;
      case skyblue: flagType = pinkIsTop ? Flag::pinkAboveSkyblue : Flag::skyblueAbovePink; break;
    }

    int i = numberOfBoundingBoxes[flagType];
    
    boundingBoxLeft[i][flagType] = leftDist;
    boundingBoxRight[i][flagType] = rightDist;
    boundingBoxTop[i][flagType] = topDist;
    boundingBoxBottom[i][flagType] = bottomDist;

    boundingBoxLeftValid[i][flagType] = leftValid;
    boundingBoxRightValid[i][flagType] = rightValid;
    boundingBoxTopValid[i][flagType] = topValid;
    boundingBoxBottomValid[i][flagType] = bottomValid;
  
    numberOfBoundingBoxes[flagType]++;
    if(numberOfBoundingBoxes[flagType] >= maxNumberOfBoundingBoxes)
    {
      numberOfBoundingBoxes[flagType] = maxNumberOfBoundingBoxes - 1;
    }

    DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorFlags, north.x/2, north.y/2);
    DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorFlags, east.x/2, east.y/2);
    DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorFlags, south.x/2, south.y/2);
    DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorFlags, west.x/2, west.y/2);
  }
 }

bool SlamFlagSpecialist::findEndOfFlag
(
 const Image& image,
 const CameraInfo& bwCameraInfo, 
 const ColorTable& colorTable,
 const Vector2<int> start,
 Vector2<double> horizonDirection,
 Direction directionToGo,
 Vector2<int>& destination,
 colorClass color,
 bool pinkIsTop,
 bool startIsInTop,
 DebugType type,
 int& countPixel,
 int& countTop,
 int& countBottom
 )
{
  bool valid = true;
  int counter = 0;
  int blackCounter = 0;
  int outsideCounter = 0;
  colorClass current, expected, allowed;
  colorClass topColor = pink;
  colorClass bottomColor = pink;
  colorClass startColor;
  
  if(pinkIsTop) bottomColor = color; else topColor = color;
  if(startIsInTop) startColor = topColor; else startColor = bottomColor;
  
  switch(directionToGo)
  {
    case left:
    case right:
    default:
      expected = startColor;
      allowed = startColor;
      break;
    case up:
      expected = topColor;
      allowed = bottomColor;
      break;
    case down:
      expected = bottomColor;
      allowed = topColor;
      break;
  }

  destination = start;
  //if(startColor == green) return false;

  Vector2<int> lastInsideFlag, lastDestination;
  lastInsideFlag = start;
  
  Vector2<double> direction;
  switch(directionToGo)
  {
  case up:
    direction.x = horizonDirection.y;
    direction.y = -horizonDirection.x;
    break;
  case right:
    direction = horizonDirection;
    break;
  case down:
    direction.x = -horizonDirection.y;
    direction.y = horizonDirection.x;
    break;
  case left:
    direction.x = -horizonDirection.x;
    direction.y = -horizonDirection.y;
    break;
  }
  enum {incX, decX, incY, decY} mode;
  if(direction.y < -fabs(direction.x)) mode = decY;
  else if(direction.y > fabs(direction.x)) mode = incY;
  else if(direction.x < -fabs(direction.y)) mode = decX;
  else mode = incX;
  
  Vector2<int> diff;
  
  bool goOn = true;
  while(goOn)
  {
    counter++;

    // calculate new point
    switch(mode)
    {
    case incX:
      diff.x++;
      diff.y = (int)(diff.x * direction.y / direction.x);
      break;
    case decX:
      diff.x--;
      diff.y = (int)(diff.x * direction.y / direction.x);
      break;
    case incY:
      diff.y++;
      diff.x = (int)(diff.y * direction.x / direction.y);
      break;
    case decY:
      diff.y--;
      diff.x = (int)(diff.y * direction.x / direction.y);
      break;
    }
    lastDestination = destination;
    destination = start + diff;
    
    // debug output
    if ( type == initial )
    {
      DEBUG_IMAGE_SET_PIXEL_Y(imageProcessorFlags, destination.x/2, destination.y/2, 180)
    }
    if ( type == horizontal )
    {
      DEBUG_IMAGE_SET_PIXEL_Y(imageProcessorFlags, destination.x/2, destination.y/2, 90)
    }
    if ( type == vertical )
    {
      DEBUG_IMAGE_SET_PIXEL_Y(imageProcessorFlags, destination.x/2, destination.y/2, 0)
    }

    if(destination.x < 1 || destination.x >= bwCameraInfo.resolutionWidth - 1 ||
        destination.y < 1 || destination.y >= bwCameraInfo.resolutionHeight - 2)
    {
      valid = outsideCounter > 10;
      goOn = false;
      destination = lastInsideFlag;
    }
    else
    {
      current = CORRECTED_COLOR_CLASS(
        destination.x/2,
        destination.y/2,
        image.getHighResY(destination.x,destination.y),
        image.image[destination.y/2][1][destination.x/2],
        image.image[destination.y/2][2][destination.x/2]);
      
      if(current == expected)
      {
        lastInsideFlag = destination;
        outsideCounter = 0;
      }
      else if(current != allowed)
      {
        blackCounter++;
        outsideCounter++;
        if(blackCounter > 20)
        {
          goOn = false;
          destination = lastInsideFlag;
        }
      }

      if(current == topColor) countTop++;
      else if(current == bottomColor) countBottom++;

    }// if inside image
  }//while goOn
  
  DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorFlags, destination.x/2, destination.y/2)

  countPixel += counter - outsideCounter;

  return valid;
}

void SlamFlagSpecialist::getFlagPercept
(
 const CameraMatrix& cameraMatrix, 
 const CameraMatrix& prevCameraMatrix, 
 const CameraInfo& cameraInfo, 
 const Geometry::Line horizonLine,
 LandmarksPercept& landmarksPercept
 )
{
  CameraInfo bwCameraInfo = cameraInfo;
  bwCameraInfo.resolutionHeight*=2;
  bwCameraInfo.resolutionWidth*=2;
  bwCameraInfo.focalLength*=2;
  bwCameraInfo.focalLengthInv/=2;
  bwCameraInfo.opticalCenter.x*=2;
  bwCameraInfo.opticalCenter.y*=2;  
  int flip = getPlayer().getTeamColor() == Player::blue ? -1 : 1;

  Vector2<double> verticalDirection;
  verticalDirection.x = -horizonLine.direction.y;
  verticalDirection.y = horizonLine.direction.x;

  //~ double factor = bwCameraInfo.resolutionWidth / 2 / tan(bwCameraInfo.openingAngleWidth / 2);
  double factor = bwCameraInfo.focalLength;
  for(int flagType = 0; flagType < 6; flagType++)
  {
    // find best bounding box
    for(int i = 0; i < numberOfBoundingBoxes[flagType]; i++)
    {
    }
    bestBoundingBox[flagType] = 0;

    if(numberOfBoundingBoxes[flagType] > 0)
    { 
      Vector2<double> right, left, top, bottom;

      right = horizonLine.base + 
        horizonLine.direction * boundingBoxRight[bestBoundingBox[flagType]][flagType]
        - verticalDirection * 
        (boundingBoxTop[bestBoundingBox[flagType]][flagType] + 
        boundingBoxBottom[bestBoundingBox[flagType]][flagType] 
        ) / 2;

      left = horizonLine.base + 
        horizonLine.direction * boundingBoxLeft[bestBoundingBox[flagType]][flagType]
        - verticalDirection * 
        (boundingBoxTop[bestBoundingBox[flagType]][flagType] + 
        boundingBoxBottom[bestBoundingBox[flagType]][flagType] 
        ) / 2;

      top = horizonLine.base 
        - verticalDirection * boundingBoxTop[bestBoundingBox[flagType]][flagType]
        + horizonLine.direction * 
        (boundingBoxLeft[bestBoundingBox[flagType]][flagType] + 
        boundingBoxRight[bestBoundingBox[flagType]][flagType] 
        ) / 2;

      bottom = horizonLine.base 
        - verticalDirection * boundingBoxBottom[bestBoundingBox[flagType]][flagType]
        + horizonLine.direction * 
        (boundingBoxLeft[bestBoundingBox[flagType]][flagType] + 
        boundingBoxRight[bestBoundingBox[flagType]][flagType] 
        ) / 2;

//  CANDIDATE FOR RADIAL DISTORTION CORRECTION    
      Vector3<double> vectorToLeft(factor,
                                                    bwCameraInfo.opticalCenter.x - left.x,
                                                    bwCameraInfo.opticalCenter.y - left.y);
      Vector3<double> vectorToRight(factor,
                                                    bwCameraInfo.opticalCenter.x - right.x,
                                                    bwCameraInfo.opticalCenter.y - right.y);
      Vector3<double> vectorToTop(factor,
                                                    bwCameraInfo.opticalCenter.x - top.x,
                                                    bwCameraInfo.opticalCenter.y - top.y);
      Vector3<double> vectorToBottom(factor,
                                                    bwCameraInfo.opticalCenter.x - bottom.x,
                                                    bwCameraInfo.opticalCenter.y - bottom.y);

      // note: y is the row of a large (bw) image
      Vector3<double>
        vectorToLeftWorld = Geometry::rayFromCamera(int(left.y / 2), cameraMatrix, prevCameraMatrix, vectorToLeft, cameraInfo),
        vectorToRightWorld = Geometry::rayFromCamera(int(right.y / 2), cameraMatrix, prevCameraMatrix, vectorToRight, cameraInfo),
        vectorToTopWorld = Geometry::rayFromCamera(int(top.y / 2), cameraMatrix, prevCameraMatrix, vectorToTop, cameraInfo),
        vectorToBottomWorld = Geometry::rayFromCamera(int(bottom.y / 2), cameraMatrix, prevCameraMatrix, vectorToBottom, cameraInfo);

      double 
        leftAngle = atan2(vectorToLeftWorld.y,vectorToLeftWorld.x),
        rightAngle = atan2(vectorToRightWorld.y,vectorToRightWorld.x),
        topAngle = atan2(vectorToTopWorld.z,sqrt((double)sqr(vectorToTopWorld.x) + sqr(vectorToTopWorld.y)) ),
        bottomAngle = atan2(vectorToBottomWorld.z,sqrt((double)sqr(vectorToBottomWorld.x) + sqr(vectorToBottomWorld.y)) );


      Vector2<double>flagPosition;
      
      switch (flagType)
      {
      case Flag::pinkAboveYellow:
        flagPosition.x = xPosBackFlags * flip;
        flagPosition.y = yPosRightFlags * flip;
        break;
      case Flag::pinkAboveSkyblue:
        flagPosition.x = xPosFrontFlags * flip;
        flagPosition.y = yPosRightFlags * flip;
        break;
      case Flag::yellowAbovePink:
        flagPosition.x = xPosBackFlags * flip;
        flagPosition.y = yPosLeftFlags * flip;
        break;
      case Flag::skyblueAbovePink:
        flagPosition.x = xPosFrontFlags * flip;
        flagPosition.y = yPosLeftFlags * flip;
        break;
      }
      ConditionalBoundary boundary;
      boundary.addX(leftAngle,!boundingBoxLeftValid[bestBoundingBox[flagType]][flagType]);
      boundary.addX(rightAngle,!boundingBoxRightValid[bestBoundingBox[flagType]][flagType]);
      boundary.addY(topAngle,!boundingBoxTopValid[bestBoundingBox[flagType]][flagType]);
      boundary.addY(bottomAngle,!boundingBoxBottomValid[bestBoundingBox[flagType]][flagType]);
      // calculate flag corners in image coordinates;
      Vector2<int> up((int) (top.x - bottom.x) / 4, (int) (top.y - bottom.y) / 4);
      Vector2<int> topLeft = Vector2<int>((int)left.x/2,(int)left.y/2) + up;
      Vector2<int> topRight = Vector2<int>((int)right.x/2,(int)right.y/2) + up;
      Vector2<int> bottomLeft = Vector2<int>((int)left.x/2,(int)left.y/2) - up;
      Vector2<int> bottomRight = Vector2<int>((int)right.x/2,(int)right.y/2) - up;
      landmarksPercept.addFlag((Flag::FlagType)flagType, flagPosition, boundary, topLeft, topRight, bottomLeft, bottomRight);
    } //if(numberOfBoundingBoxes[flagType]) > 0)
  }
  Vector2<double> cameraOffset(cameraMatrix.translation.x,
                                              cameraMatrix.translation.y);

  estimateOffsetForFlags(landmarksPercept, cameraOffset); 
  SEND_DEBUG_IMAGE(imageProcessorFlags);
}

void SlamFlagSpecialist::estimateOffsetForFlags
(
 LandmarksPercept& landmarksPercept,
 const Vector2<double>& cameraOffset
 )
{
  for(int i = 0;i < landmarksPercept.numberOfFlags; ++i)
  {
    Flag& flag = landmarksPercept.flags[i];

    /** @todo improve, isOnBorder(flag.x.?) not checked */
    double distance;
    double direction = flag.x.getCenter();

    if(!flag.isOnBorder(flag.y.min) && !flag.isOnBorder(flag.y.max))
    {
      if(flag.y.min != flag.y.max)
      {
        distance = flagHeight / (tan(flag.y.max) - tan(flag.y.min)) + flagRadius;
        flag.distanceValidity = 0.8;
      }
      else
      {
        distance = 4500;
        flag.distanceValidity = 0.05;
      }
    }
    else
    {
      distance = flagRadius / sin(flag.x.getSize() / 2);
      if(!flag.isOnBorder(flag.x.min) && !flag.isOnBorder(flag.x.max)) // Flag touches no vertical border
        flag.distanceValidity = 0.7;
      else
        flag.distanceValidity = 0.2;
    }
    if(!flag.isOnBorder(flag.x.min) && !flag.isOnBorder(flag.x.max)) // Flag touches no vertical border
      flag.angleValidity = 0.8;
    else
      flag.angleValidity = 0.7;
    
    Pose2D p = Pose2D(cameraOffset) + Pose2D(direction) 
                       + Pose2D(Vector2<double>(distance,0));
    flag.distance = p.translation.abs();
    flag.angle = direction;   
    if (flag.distance > 6000)
    {
      flag.distance = 6000;
      flag.distanceValidity=0; // flags far away are meassured very bad
    }
    else if (flag.distance > 3000) 
      flag.distanceValidity*=0.5; // flags medium far away are meassured medium bad
  }
}

---