Modules/ImageProcessor/VLCImageProcessor/VLCBallSpecialist.cpp

Go to the documentation of this file.

/**
* @file VLCBallSpecialist.cpp
* This file contains a class for Image Processing.
* @author <A href=mailto:juengel@informatik.hu-berlin.de>Matthias Juengel</A>
* @author Max Risler
* @author <A href=mailto:brunn@sim.tu-darmstadt.de>Ronnie Brunn</A>
* @author <A href=mailto:mkunz@sim.tu-darmstadt.de>Michael Kunz</A>
* @author <a href="mailto:walter.nistico@uni-dortmund.de">Walter Nistico</a>
*/

#include "VLCBallSpecialist.h"

#include "Tools/FieldDimensions.h"
#include "Tools/Math/MVTools.h"
#include "Tools/Math/Matrix_nxn.h"
#include "Tools/Math/Vector_n.h"
#include "Tools/Debugging/DebugDrawings.h"
#include "Tools/Debugging/Debugging.h"
#include "VLCImageProcessorTools.h"
#include "Platform/GTAssert.h"
#include "Tools/Math/Common.h"


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

void VLCBallSpecialist::searchBall
(
 const Image& image, 
 const ColorTable& colorTable,
 const CameraMatrix& cameraMatrix,
 const CameraMatrix& prevCameraMatrix,
 int x, int y,
 BallPercept& ballPercept
 )
{
  BallPointList ballPoints;
  Vector2<int> center;
  double radius;
  int countOrange = 0;
  int countAmbiguous = 0; 
  int maxOrangePerLine = 0;
  int countPixel = 0;
  CameraInfo bwCameraInfo = image.cameraInfo;
  bwCameraInfo.resolutionHeight*=2;
  bwCameraInfo.resolutionWidth*=2;
  bwCameraInfo.opticalCenter.x*=2;
  bwCameraInfo.opticalCenter.y*=2;
  bwCameraInfo.focalLength*=2;
  bwCameraInfo.focalLengthInv/=2;

  scanForBallPoints(image, bwCameraInfo, colorTable, x, y, ballPoints, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
  
  //ignore ball if not enough points are orange
  //OUTPUT (idText, text, "VLCBallSpecialist: " << countOrange * 100 / countPixel << "% orange");
  int i;
  int numberOfSoftEdgePoints = 0;
  int numberOfPointsInYellow = 0;
  
  for(i = 0; i < ballPoints.number; i++)
  {
    if (ballPoints[i].yellowIsClose && !ballPoints[i].atBorder) numberOfPointsInYellow++;
    if (!ballPoints[i].hardEdge && !ballPoints[i].atBorder) numberOfSoftEdgePoints++;
  }
  /*  OUTPUT (idText, text, "VLCBallSpecialist: " << endl << "----------------------------" << endl
  << "Ball Points: " << ballPoints.number << endl
  << "Yellow Points: " << (numberOfPointsInYellow*100)/ballPoints.number << "%" << endl
  << "SoftEdge Points: " << (numberOfSoftEdgePoints*100)/ballPoints.number << "%" << endl << endl
  << "Orange Pixel: " << (countOrange*100)/countPixel << "%" << endl
  << "Ambiguous Pixel: " << (countAmbiguous*100)/countPixel << "%" << endl
  );*/
  
  if ((countOrange > countAmbiguous) && 
    (countOrange * 6 > countPixel) &&                          // >  16.66% orange
//    (numberOfSoftEdgePoints * 5 <= ballPoints.number * 2) &&   // <= 40% with soft edge
    (numberOfPointsInYellow * 4 <= ballPoints.number * 3)      // <= 75% with Yellow close
    )
  {

  considerBallPoints(ballPoints);


    //try only points near green with hard edge first
    BallPointList testPoints;
    for(i = 0; i < ballPoints.number; i++)
    {
      if (ballPoints[i].greenIsClose && ballPoints[i].hardEdge) 
        testPoints.add(ballPoints[i]);
    }
    if (
      testPoints.number *2 >= ballPoints.number &&
      createBallPerceptLevenbergMarquardt(testPoints, center, radius) &&
      checkIfPointsAreInsideBall(ballPoints, center, radius))
    {
      addBallPercept(image, bwCameraInfo, colorTable, cameraMatrix, prevCameraMatrix, center, radius, ballPercept);
    }
    else
    {
      //now all points if not at border with hard edge
      testPoints.number = 0;
      for(i = 0; i < ballPoints.number; i++)
      {
        if (!ballPoints[i].atBorder && ballPoints[i].hardEdge) 
          testPoints.add(ballPoints[i]);
      }
      if (
        createBallPerceptLevenbergMarquardt(testPoints, center, radius) &&
        checkIfPointsAreInsideBall(ballPoints, center, radius))
      {
        addBallPercept(image, bwCameraInfo, colorTable, cameraMatrix, prevCameraMatrix, center, radius, ballPercept);
      }
      else
      {
        //now all points if not at border
        testPoints.number = 0;
        for(i = 0; i < ballPoints.number; i++)
        {
          if (!ballPoints[i].atBorder) 
            testPoints.add(ballPoints[i]);
        }
        if (
          createBallPerceptLevenbergMarquardt(testPoints, center, radius) &&
          checkIfPointsAreInsideBall(ballPoints, center, radius))
        {
          addBallPercept(image, bwCameraInfo, colorTable, cameraMatrix, prevCameraMatrix, center, radius, ballPercept);
        }
        else
        {
          //take all points if nothing else works
          if (createBallPerceptLevenbergMarquardt(ballPoints, center, radius))
          {
      calculateDeviationOfBallPoints(ballPoints, center, radius);
            addBallPercept(image, bwCameraInfo, colorTable, cameraMatrix, prevCameraMatrix, center, radius, ballPercept);
          }
        }
      }
    }
  }
  
  DEBUG_DRAWING_FINISHED(imageProcessor_ball1);
  DEBUG_DRAWING_FINISHED(imageProcessor_ball2);
}


void VLCBallSpecialist::BallPointList::add(const BallPoint& ballPoint)
{
  ASSERT(number < maxNumberOfPoints);
  ballPoints[number++] = ballPoint;
  DOT(imageProcessor_ball2, ballPoint.x / 2, ballPoint.y / 2,
    (ballPoint.hardEdge) ? Drawings::blue : Drawings::orange, 
    (ballPoint.atBorder) ? Drawings::black :
  (ballPoint.greenIsClose) ? Drawings::green :
  (ballPoint.yellowIsClose) ? Drawings::yellow :
  Drawings::white
    );
}

void VLCBallSpecialist::scanForBallPoints
(
 const Image& image,
 const CameraInfo& bwCameraInfo,
 const ColorTable& colorTable,
 int x, int y,
 BallPointList& ballPoints,
 int& countAmbiguous, 
 int& countOrange,
 int& maxOrangePerLine,
 int& countPixel
 )
{
  // search for ball variables
  BallPoint north;
  BallPoint east;
  BallPoint south;
  BallPoint west;
  
  BallPoint start;
  Vector2<int>step;
  BallPoint destination;
  
  //  int xStep;
  //  int yStep;
  
  start.x = x * 2; start.y = y * 2;
  BallPoint start2;
  
  DOT(imageProcessor_ball2, x, y, Drawings::black, Drawings::white);
  
  //find north ///////////////////////////////////////////
  step.x = 0; step.y = -1;
  findEndOfBall(image, bwCameraInfo, colorTable, start, step, north, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
  if(north.atBorder)
  {
    start2 = north - step;
    //find east
    step.x = 1; step.y = 0;
    if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    {
      ballPoints.add(destination);
    }
    //find west
    step.x = -1; step.y = 0;
    if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    {
      ballPoints.add(destination);
    }
  }
  else
  {
    ballPoints.add(north);
  }
  
  //find east ///////////////////////////////////////////
  step.x = 1; step.y = 0;
  findEndOfBall(image, bwCameraInfo, colorTable, start, step, east, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
  if(east.atBorder)
  {
    start2 = east - step;
    //find north
    step.x = 0; step.y = -1;
    if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    {
      ballPoints.add(destination);
    }
    //find south
    step.x = 0; step.y = 1;
    if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    {
      ballPoints.add(destination);
    }
  }
  else
  {
    ballPoints.add(east);
  }
  
  //find south ///////////////////////////////////////////
  step.x = 0; step.y = 1;
  findEndOfBall(image, bwCameraInfo, colorTable, start, step, south, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
  if(south.atBorder)
  {
    start2 = south - step;
    //find east
    step.x = 1; step.y = 0;
    if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    {
      ballPoints.add(destination);
    }
    //find west
    step.x = -1; step.y = 0;
    if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    {
      ballPoints.add(destination);
    }
  }
  else
  {
    ballPoints.add(south);
  }
  
  //find west ///////////////////////////////////////////
  step.x = -1; step.y = 0;
  findEndOfBall(image, bwCameraInfo, colorTable, start, step, west, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
  if(west.atBorder)
  {
    start2 = west - step;
    //find north
    step.x = 0; step.y = -1;
    if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    {
      ballPoints.add(destination);
    }
    //find south
    step.x = 0; step.y = 1;
    if(findEndOfBall(image, bwCameraInfo, colorTable, start2, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel))
    {
      ballPoints.add(destination);
    }
  }
  else
  {
    ballPoints.add(west);
  }
  
  //
  if( (south.y - north.y) > (east.x - west.x) )
  {
    if ((north.y + south.y) / 2 != start.y)
    {
      start.y = (north.y + south.y) / 2;
      //find east
      step.x = 1; step.y = 0;
      findEndOfBall(image, bwCameraInfo, colorTable, start, step, east, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
      if(!east.atBorder)
      {
        ballPoints.add(east);
      }
      //find west
      step.x = -1; step.y = 0;
      findEndOfBall(image, bwCameraInfo, colorTable, start, step, west, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
      if (!west.atBorder)
      {
        ballPoints.add(west);
      }
      //////////
      start.x = (west.x + east.x) / 2;
    }
  }
  else
  {
    if ((west.x + east.x) / 2 != start.x)
    {
      start.x = (west.x + east.x) / 2;
      //find north
      step.x = 0; step.y = -1;
      findEndOfBall(image, bwCameraInfo, colorTable, start, step, north, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
      if(!north.atBorder)
      {
        ballPoints.add(north);
      }
      //find south
      step.x = 0; step.y = 1;
      findEndOfBall(image, bwCameraInfo, colorTable, start, step, south, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
      if(!south.atBorder)
      {
        ballPoints.add(south);
      }
      //////////
      start.y = (north.y + south.y) / 2;
    }
  }
  //
  
  // find in diagonal
  
  //  for(xStep = -1; xStep <= 1; xStep += 2)
  for (step.x = -1; step.x <= 1; step.x += 2)
  {
    //for(yStep = -1; yStep <= 1; yStep += 2)
    for (step.y = -1; step.y <= 1; step.y += 2)
    {
      //step.x = xStep; step.y = yStep;
      findEndOfBall(image, bwCameraInfo, colorTable, start, step, destination, countAmbiguous, countOrange, maxOrangePerLine, countPixel);
      if (!destination.atBorder)
      {
        ballPoints.add(destination);
      }
    } //for(int yStep ...
  } //for(int xStep ...
}

bool VLCBallSpecialist::findEndOfBall
(
 const Image& image,
 const CameraInfo& bwCameraInfo,
 const ColorTable& colorTable,
 const BallPoint& start,
 const Vector2<int>& step,
 BallPoint& destination,
 int& countAmbiguous,
 int& countOrange,
 int& maxOrangePerLine,
 int& countPixel
 )
{
/*
stopColors are colors indicating the end of the ball:
green, yellow, skyblue
more than 3 pixels of a stopColor indicate the end of the ball
  */
  int stopColorCounter = 0;
  int currentOrange = 0;
  int currentAmbiguous = 0;
  int len = 0;
  int stopLen = 0;
//  bool isAmbiguous = false;
  
  colorClass currentColorClass;
  
  Vector2<int> firstStopColor = start;
  unsigned char currentOrangeSim = 0;
  Vector2<int> lastPoint = start;
  destination = start;
  destination.greenIsClose = false;
  destination.yellowIsClose = false;
  destination.atBorder = false;
  destination.hardEdge = true;
  
  bool isOrange = false;
  
  bool goOn = true;
  while(goOn)
  {
    lastPoint = destination;
    destination += step;
    /////////////
    if(destination.x < 0 || destination.x >= bwCameraInfo.resolutionWidth ||
      destination.y < 0 || destination.y >= bwCameraInfo.resolutionHeight-1) // avoid artefact at bottom of hires image
    {
      if (stopColorCounter == 0)
      {
        countPixel += len;
        destination.atBorder = true;
      }
      else
      {
        destination = firstStopColor;
        countPixel += stopLen;
        destination.atBorder = false;
      }
      goOn = false;
    }
    else
    {
      currentColorClass = 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],
        colorTable, colorCorrector);
      
      // counting all orange pixels on the scanned horz./vert./diag. lines
      len++;
      if ( currentColorClass == orange )
      {
        currentOrange++;
      }

    /*
      isAmbiguous = (currentColorClass == pink ||
        currentColorClass == yellow ||
        currentColorClass == red
        );
      
      if (isAmbiguous)
    */
    if (currentColorClass == pink || currentColorClass == yellow || currentColorClass == red)
      {
        currentAmbiguous++;
      }
      
      LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::green);
      
      if (currentColorClass == orange)
        isOrange = true;
      else
      {
        currentOrangeSim = getSimilarityToOrange(
          colorCorrector.correct(destination.x / 2,destination.y / 2, 0, image.getHighResY(destination.x,destination.y)),
          colorCorrector.correct(destination.x / 2,destination.y / 2, 1, image.image[destination.y / 2][1][destination.x / 2]),
          colorCorrector.correct(destination.x / 2,destination.y / 2, 2, image.image[destination.y / 2][2][destination.x / 2])
          );
        isOrange = currentOrangeSim > 30;
      }
      
      if(currentColorClass == green || 
        currentColorClass == yellow ||
        currentColorClass == skyblue ||
        currentColorClass == red ||
        currentColorClass == blue ||
        //        currentColorClass == pink || //added
        //removed because there is pink in the ball and I have removed pink from orange similarity
        !isOrange
        )
      {
        if (stopColorCounter == 0)
        {
          firstStopColor = destination;
          stopLen = len;
        }
        if (currentColorClass == green) destination.greenIsClose = true;
        if (currentColorClass == yellow) destination.yellowIsClose = true;
        stopColorCounter++;
        
        if (isOrange)
        {
          LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::orange);
        }
        else
        {
          LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::red);
        }
        
        if(stopColorCounter > 8) 
        {
          destination = firstStopColor;
          countPixel += stopLen;
          goOn = false;
        }
      }
      else
      {
        destination.greenIsClose = false;
        destination.yellowIsClose = false;
        stopColorCounter = 0;
      }
      
    } // else destination in range
  } //  while(goOn)
  
  destination -= step;
  
  // compute sum of all orange pixels and max-pixels-per-line
  countOrange += currentOrange;
  countAmbiguous += currentAmbiguous;
  maxOrangePerLine = max ( maxOrangePerLine, currentOrange);
  if (destination.greenIsClose) destination.yellowIsClose = false;
  
  /*  if (!destination.atBorder)
  {
  OUTPUT(idText, text, "Point(" << destination.x << "/" << destination.y << "): " << lastGoodColorOrangeSim << " -> " << currentOrangeSim);
  }*/
  if (!destination.atBorder)
  {
      unsigned char lastGoodColorOrangeSim = getSimilarityToOrange(
      colorCorrector.correct(destination.x / 2,destination.y / 2, 0, image.getHighResY(destination.x,destination.y)),
      colorCorrector.correct(destination.x / 2,destination.y / 2, 1, image.image[destination.y / 2][1][destination.x / 2]),
      colorCorrector.correct(destination.x / 2,destination.y / 2, 2, image.image[destination.y / 2][2][destination.x / 2])
      );
    destination.hardEdge = (2 * currentOrangeSim < lastGoodColorOrangeSim);
  }
  else 
    destination.hardEdge = false;
  
  if (!destination.atBorder)
  {
    if (destination.greenIsClose)
    {
      LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::blue);
    }
    else
      if (destination.yellowIsClose)
      {
        LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::skyblue);
      }
      else
      {
        LINE(imageProcessor_ball1,destination.x / 2,destination.y / 2,destination.x / 2 + 1,destination.y / 2,1,Drawings::ps_solid,Drawings::pink);
      }
  }
  return true;
}



bool VLCBallSpecialist::createBallPerceptLevenbergMarquardt
(
 const BallPointList& ballPoints,
 Vector2<int>& center,
 double& radius
 )
{
  if (ballPoints.number < 3)
    return false;
  
  double Mx = 0.0;
  double My = 0.0;
  double Mxx = 0.0;
  double Myy = 0.0;
  double Mxy = 0.0;
  double Mz = 0.0;
  double Mxz = 0.0;
  double Myz = 0.0;
  
  for (int i = 0; i < ballPoints.number; ++i)
  {
    double x = ballPoints[i].x;
    double y = ballPoints[i].y;
    double xx = x*x;
    double yy = y*y;
    double z = xx + yy;
    
    Mx += x;
    My += y;
    Mxx += xx;
    Myy += yy;
    Mxy += x*y;
    Mz += z;
    Mxz += x*z;
    Myz += y*z;
  }
  
  try
  {
    Matrix_nxn<double, 3> M;
    double Matrix[9] = 
    {
      Mxx, Mxy, Mx,
        Mxy, Myy, My,
        Mx, My, ballPoints.number
    };
    M = Matrix;
    
    Vector_n<double, 3> v;
    
    v[0] = -Mxz;
    v[1] = -Myz;
    v[2] = -Mz;
    
    Vector_n<double, 3> BCD;
    BCD = M.solve(v);
    
    center.x = (int)(-BCD[0] / 2.0);
    center.y = (int)(-BCD[1] / 2.0);
    
    double tmpWurzel = BCD[0]*BCD[0]/4.0 + BCD[1]*BCD[1]/4.0 - BCD[2];
    
    if (tmpWurzel < 0.0)
      return false;
    
    radius = sqrt(tmpWurzel);
  }
  catch (MVException)
  {
    return false;
  }
  catch (...)
  {
    OUTPUT(idText, text, "Unknown exception in VLCBallSpecialist::createBallPerceptsFromXPoints");
    return false;
  }
  
  return true;
}

bool VLCBallSpecialist::checkIfPointsAreInsideBall(const BallPointList& ballPoints, Vector2<int>& center, double radius)
{
  double d;
  int numberOfNonBorderBallPoints = 0;
  double dev = 0;
  for(int i = 0; i < ballPoints.number; i++)
  {
    d = Geometry::distance(center, ballPoints[i]);
    if (d > radius * 1.1)
    {
      return false;
    }
    if (!ballPoints[i].atBorder)
    {
      numberOfNonBorderBallPoints++;
      dev += fabs(d-radius);
    }
  }
  if (numberOfNonBorderBallPoints==0)
  {
    deviationOfBallPoints = 0;
  }
  else
  {
    deviationOfBallPoints = dev / numberOfNonBorderBallPoints;
  }
  return true;
}


void VLCBallSpecialist::calculateDeviationOfBallPoints(const BallPointList& ballPoints, Vector2<int>& center, double radius)
{
  double d;
  int numberOfNonBorderBallPoints = 0;
  double dev = 0;
  for(int i = 0; i < ballPoints.number; i++)
  {
    d = Geometry::distance(center, ballPoints[i]);
    if (!ballPoints[i].atBorder)
  {
    numberOfNonBorderBallPoints++;
    dev += fabs(d-radius);
  }
  }
  if (numberOfNonBorderBallPoints==0)
  {
    deviationOfBallPoints = 0;
  }
  else
  {
    deviationOfBallPoints = dev / numberOfNonBorderBallPoints;
  }
}

void VLCBallSpecialist::considerBallPoints(const BallPointList& ballPoints)
{
  int numberOfNearGreenPoints = 0;
  int numberOfNearYellowPoints = 0;
  int numberOfHardEdgePoints = 0;
  int numberOfNonBorderPoints = 0;
  
  for (int i=0; i<ballPoints.number; i++)
  {
    if (!ballPoints[i].atBorder)
    {
      if (ballPoints[i].hardEdge)
      {
        numberOfHardEdgePoints++;
      }
      if (ballPoints[i].greenIsClose)
      {
        numberOfNearGreenPoints++;
      }
      if (ballPoints[i].yellowIsClose)
      {
        numberOfNearYellowPoints++;
      }
      numberOfNonBorderPoints++;
    }
  }
  if (numberOfNonBorderPoints>0)
  {
    durabilityOfBallPoints=1.0;
    if (!(numberOfHardEdgePoints>=3 && numberOfNearGreenPoints*2>numberOfNonBorderPoints)) 
      durabilityOfBallPoints *= 0.8;
    if (numberOfHardEdgePoints*3<numberOfNonBorderPoints*2)   // less than 2/3 hard edge
        durabilityOfBallPoints *= 0.9;
    if (numberOfHardEdgePoints*3<numberOfNonBorderPoints)     // less than 1/3 hard edge
      durabilityOfBallPoints *= 0.9;
    if (numberOfNearGreenPoints*4<numberOfNonBorderPoints)    // less than 1/4 near green
      durabilityOfBallPoints *= 0.5;
    if (numberOfNearGreenPoints==0)                           // no points near green
      durabilityOfBallPoints *= 0.5;
    if (numberOfNearYellowPoints*2>numberOfNonBorderPoints)   // more than 1/2 near yellow
      durabilityOfBallPoints *= 0.9;
    if (numberOfNonBorderPoints<4)                            // only 1-3 non border points 
      durabilityOfBallPoints *= 0.5;
  }
  else
  {// there are only border ball points, so the ball is right in front of the camera, and the durability can stay 1.0
    durabilityOfBallPoints=1.0;
  }
  
}


double VLCBallSpecialist::calculateReliability(
  double percentOfOrange, 
  double radius, 
  Vector2<double> anglesToCenter, 
  const CameraInfo bwCameraInfo, 
  const CameraMatrix& cameraMatrix,
  const CameraMatrix& prevCameraMatrix
  )
{
  double reliability = 1.0;
  double distanceFromRadius = Geometry::getBallDistanceByAngleSize(int(2 * ballRadius), 2 * Geometry::pixelSizeToAngleSize(radius, bwCameraInfo));
  double distanceFromAngle;
  if (anglesToCenter.y < 0)
  {
    distanceFromAngle = (cameraMatrix.translation.z - ballRadius) / tan(-anglesToCenter.y); // tan() like used in the function in the ballPercept class. I think sin() should be correct, but at this small angles it should make no difference
  }
  else
  {
    distanceFromAngle = 0;
  }
  double distanceReliability = max(0.001, 1.0-fabs((distanceFromRadius-distanceFromAngle)/(distanceFromRadius+distanceFromAngle)));
  
  if (deviationOfBallPoints/radius<0.1 && durabilityOfBallPoints>0.9 && percentOfOrange>0.9 && distanceReliability>0.9)
  {
    reliability = 1.0;
  }
  else
  {
    reliability = 1.0 * durabilityOfBallPoints * percentOfOrange * distanceReliability * (1-deviationOfBallPoints/radius);
  }
  /*
  OUTPUT(idText,text,"Reliability calculation of ball percept: ");
  OUTPUT(idText,text,"deviationOfBallPoints/radius: " << deviationOfBallPoints/radius);
  OUTPUT(idText,text,"durabilityOfBallPoints: " << durabilityOfBallPoints);
  OUTPUT(idText,text,"percentOfOrange: " << percentOfOrange);
  OUTPUT(idText,text,"distanceReliability: " << distanceReliability);
  OUTPUT(idText,text,"-->reliability: " << reliability);
  */
    
  return reliability;
}



void VLCBallSpecialist::addBallPercept
(
 const Image& image,
 const CameraInfo& bwCameraInfo,
 const ColorTable& colorTable,
 const CameraMatrix& cameraMatrix,
 const CameraMatrix& prevCameraMatrix,
 const Vector2<int>& center,
 double radius,
 BallPercept& ballPercept
 )
{
  // test if ball is below horizon
  double factor = bwCameraInfo.focalLengthInv;
  Vector3<double> 
    vectorToCenter(1, (bwCameraInfo.opticalCenter.x - center.x) * factor, (bwCameraInfo.opticalCenter.y - center.y) * factor);
  Vector3<double> 
    vectorToCenterWorld = cameraMatrix.rotation * vectorToCenter;
  
  //Is the point above the horizon ? - return
  //  if(vectorToCenterWorld.z <= -5 * factor) //for perfect horizon
  if(vectorToCenterWorld.z < 1 * factor)
  {
  double percentOfOrange = 1.0; // big balls are already scanned enough. They are supposed to have nearly 100% orange inside
    if (radius <= 12.0)
    {//small ghost balls can easily appear on an orange fringe of a pink/yellow landmark
      // this because the scan follows along the fringe; this new scan starts from the 
      // hypothetical center of the "ghost" ball, which in such case lies inside the landmark
      // and hence produces a high number of "non orange" points; a true ball however,
      // should have enough orange inside
      Vector2<int> current(center), clippedCenter(center);
      int orangeCount = 0;
      int totalCount = 0;
      double scanAngle = 0;
      colorClass currentColor;
      Geometry::clipPointInsideRectange(Vector2<int>(0,0), Vector2<int>(bwCameraInfo.resolutionWidth,bwCameraInfo.resolutionHeight), clippedCenter);
      currentColor = CORRECTED_COLOR_CLASS(
      clippedCenter.x/2, clippedCenter.y/2,
      image.getHighResY(clippedCenter.x,clippedCenter.y),
      image.image[clippedCenter.y/2][1][clippedCenter.x/2],
      image.image[clippedCenter.y/2][2][clippedCenter.x/2],
      colorTable, colorCorrector
    );
    if(currentColor == orange)
      orangeCount++;
    totalCount++;
      for (int sector=0; sector<8; sector++)
      {
        BresenhamLineScan scan(clippedCenter, scanAngle, bwCameraInfo);
        scan.init();
        current = clippedCenter;
        int steps; 
        if ((sector%2)!=0)
          steps = (int)(radius/sqrt(2.0));
        else
          steps = (int)(radius);
        for (int i=0; i<steps && i<scan.numberOfPixels; i++)
        {
          scan.getNext(current);
          currentColor = CORRECTED_COLOR_CLASS(
            current.x/2, current.y/2,
            image.getHighResY(current.x,current.y),
            image.image[current.y/2][1][current.x/2],
            image.image[current.y/2][2][current.x/2],
            colorTable, colorCorrector
          );
          if(currentColor == orange)
            orangeCount++;
          totalCount++;
          DOT(imageProcessor_ball2, current.x/2, current.y/2, 
              (currentColor!=gray) ? Drawings::gray : Drawings::black, 
              ColorClasses::colorClassToDrawingsColor(currentColor));
    }
        scanAngle += pi_4;
      }
      if (orangeCount*3 < totalCount) //not enough orange to be ball
        return;
    percentOfOrange = (double)orangeCount/totalCount;
    }

  Vector2<double>angles;
    Geometry::calculateAnglesForPoint(center, cameraMatrix, prevCameraMatrix, bwCameraInfo, angles);
    double radiusAsAngle = Geometry::pixelSizeToAngleSize(radius, bwCameraInfo);
    double reliability = calculateReliability(percentOfOrange,radius,angles,bwCameraInfo,cameraMatrix,prevCameraMatrix);
    
    // ..
    BallPercept newPercept;
    newPercept.addHighRes(center,
                          angles,
                          radius,
                          radiusAsAngle, 
                          cameraMatrix.translation, 
                          cameraMatrix.isValid,
                          reliability);

    MultipleBallPerceptElement newElement;
    newElement.reliability = newPercept.reliability;
    newElement.offsetOnField = newPercept.offsetOnField;

    //set the distance error value
    newElement.calculateDistanceValue(newElement.offsetOnField);

    addMultiplePercept(newElement);
    CIRCLE(imageProcessor_multipleBallPercepts, newPercept.centerInImage.x/2, newPercept.centerInImage.y/2, newPercept.radiusInImage/2, 2, Drawings::ps_dot, Drawings::orange);
    
    //calculatePointOnField gives the position RELATIVE TO THE ROBOT...
    //Vector2<int> centerOnField;
    //if(Geometry::calculatePointOnField(newPercept.ballCenter.x, newPercept.ballCenter.y, cameraMatrix, prevCameraMatrix, bwCameraInfo,                                                                                                                                  centerOnField))
    //{
    //  CIRCLE(multipleBallPerceptsField, centerOnField.x, centerOnField.y, 30, 50,Drawings::ps_dash, Drawings::orange);
    //}
  
    ballPercept.addHighRes(center,
                          angles,
                          radius,
                          radiusAsAngle, 
                          cameraMatrix.translation, 
                          cameraMatrix.isValid,
                          reliability);
  }
}



void VLCBallSpecialist::resetMultiplePerceptsList(BallPercept& ballPercept)
{
  ballPercept.multiplePercepts.numberOfElements = 0;
  multiplePercepts.clear();
}


void VLCBallSpecialist::addMultiplePercept(const MultipleBallPerceptElement& newPercept)
{
  List<MultipleBallPerceptElement>::Pos iter = List<MultipleBallPerceptElement>::Pos();
  if( multiplePercepts.getSize() == 0)
  {
    multiplePercepts.insert(newPercept, iter);
  }
  else
  {
    bool posFound = false;
    iter = List<MultipleBallPerceptElement>::Pos(multiplePercepts.getFirst());    
    while(iter && !posFound)
    {
      const MultipleBallPerceptElement& t = multiplePercepts[iter];
      if(newPercept.reliability < t.reliability)
        ++iter;
      else
        posFound = true;
    }
    multiplePercepts.insert(newPercept,iter);
  }
}

void VLCBallSpecialist::forwardPercept(BallPercept& ballPercept)
{
  List<MultipleBallPerceptElement>::Pos iter = List<MultipleBallPerceptElement>::Pos();
  iter = List<MultipleBallPerceptElement>::Pos(multiplePercepts.getFirst());
  int grenze = multiplePercepts.getSize() > 5 ? 5 : multiplePercepts.getSize();
  for(int i = 0; i < grenze; i++)
  {
    ballPercept.multiplePercepts.add(multiplePercepts[iter]);
    iter++;
  }
}

---