Modules/SelfLocator/GT2005StableSelfLocator/GT2005StableSelfLocator.cpp

Go to the documentation of this file.

#include "GT2005StableSelfLocator.h"

#include "Tools/Math/Common.h"
  
/*
  todo
    template generation by line types is not yet done properly
    needs change in class structure first
    implement encapsuled motion and observation model

    negative information update not working properly, now commented-out

    changing number of percept types: change probability calculation in sample
                                      change average calculation in the self locator

    DEBUG_RESPONSE output, comments adapt to gt2004
    declare some functions inline

    diffs to gt04
      -execute split up in new mehtods
      -sample.setPerceptProbability replaces setProbability
        -enum PerceptType to avoid strange LineType enum usage for setProbability
      -usage of line crossings percepts
      -negative information (not yet working, commented-out)
      -middle circle as flag (not yet working)
      -head height extracted from camera matrix
*/

double GT2005StableSelfLocator::translationNoise = 200,
       GT2005StableSelfLocator::rotationNoise = 0.5,
       GT2005StableSelfLocator::movedDistWeight = 0.002,
       GT2005StableSelfLocator::movedAngleWeight = 0.2,
       GT2005StableSelfLocator::majorDirTransWeight = 0.1,
       GT2005StableSelfLocator::minorDirTransWeight = 0.02,

       GT2005StableSelfLocator::headHeight = 150,


// observation update parameters
// trust = weight / (variance)
// motiondependent variance = variance * (speed*dependency + 1) as variance estimated for a standing robot

// at the moment we take sqrt variance instead of variance..
 
       GT2005StableSelfLocator::quasiZero = 0.000001, 
       GT2005StableSelfLocator::maximumTrust = 100,

       GT2005StableSelfLocator::linePointZAngleVariance = 1.949,
       GT2005StableSelfLocator::linePointZAngleMotionDependentVariance = 1.949,
       GT2005StableSelfLocator::linePointYAngleVariance = 9.745,
       GT2005StableSelfLocator::linePointYAngleMotionDependentVariance = 9.745,
       GT2005StableSelfLocator::linePointZAngleMotionDependency = 3/400,
       GT2005StableSelfLocator::linePointYAngleMotionDependency = 3/400,

       GT2005StableSelfLocator::crossingZAngleVariance = 1.949,
       GT2005StableSelfLocator::crossingZAngleMotionDependentVariance = 1.949,
       GT2005StableSelfLocator::crossingYAngleVariance = 9.745,
       GT2005StableSelfLocator::crossingYAngleMotionDependentVariance = 9.745,
       GT2005StableSelfLocator::crossingZAngleMotionDependency = 1/400,
       GT2005StableSelfLocator::crossingYAngleMotionDependency = 1/400,
       GT2005StableSelfLocator::centerCircleZAngleVariance = 1.949,
       GT2005StableSelfLocator::centerCircleZAngleMotionDependentVariance = 1.949,
       GT2005StableSelfLocator::centerCircleYAngleVariance = 9.745,
       GT2005StableSelfLocator::centerCircleYAngleMotionDependentVariance = 9.745,
       GT2005StableSelfLocator::centerCircleOrientationAngleVariance = 9.745,
       GT2005StableSelfLocator::centerCircleOrientationAngleMotionDependentVariance = 9.745,
       GT2005StableSelfLocator::centerCircleZAngleMotionDependency = 1/400,
       GT2005StableSelfLocator::centerCircleYAngleMotionDependency = 1/400,
       GT2005StableSelfLocator::centerCircleOrientationAngleMotionDependency = 1/400,

       GT2005StableSelfLocator::flagZAngleVariance = 1.949,
       GT2005StableSelfLocator::flagZAngleMotionDependentVariance = 1.949,
       GT2005StableSelfLocator::flagYAngleVariance = 9.745,
       GT2005StableSelfLocator::flagYAngleMotionDependentVariance = 9.745,
       GT2005StableSelfLocator::flagZAngleMotionDependency = 1/400,
       GT2005StableSelfLocator::flagYAngleMotionDependency = 1/400,
          
       GT2005StableSelfLocator::goalZAngleVariance = 1.949,
       GT2005StableSelfLocator::goalZAngleMotionDependentVariance = 1.949,
       GT2005StableSelfLocator::goalYAngleVariance = 9.745,
       GT2005StableSelfLocator::goalYAngleMotionDependentVariance = 9.745,
       GT2005StableSelfLocator::goalZAngleMotionDependency = 1/400,
       GT2005StableSelfLocator::goalYAngleMotionDependency = 1/400,

       GT2005StableSelfLocator::linePointWeight = 10,//19.49,
       GT2005StableSelfLocator::linePointZAngleTrust = 
       GT2005StableSelfLocator::linePointWeight / ( GT2005StableSelfLocator::linePointZAngleVariance + GT2005StableSelfLocator::quasiZero ),
       GT2005StableSelfLocator::linePointYAngleTrust = 
       GT2005StableSelfLocator::linePointWeight / ( GT2005StableSelfLocator::linePointYAngleVariance + GT2005StableSelfLocator::quasiZero ),

       GT2005StableSelfLocator::crossingWeight = 7,// 9.745,
       GT2005StableSelfLocator::crossingZAngleTrust = 
       GT2005StableSelfLocator::crossingWeight / ( GT2005StableSelfLocator::crossingZAngleVariance + GT2005StableSelfLocator::quasiZero ),
       GT2005StableSelfLocator::crossingYAngleTrust =
       GT2005StableSelfLocator::crossingWeight / ( GT2005StableSelfLocator::crossingYAngleVariance + GT2005StableSelfLocator::quasiZero ),

       GT2005StableSelfLocator::centerCircleWeight = 0,// 9.745,
       GT2005StableSelfLocator::centerCircleZAngleTrust = 
       GT2005StableSelfLocator::centerCircleWeight / ( GT2005StableSelfLocator::centerCircleZAngleVariance + GT2005StableSelfLocator::quasiZero ),
       GT2005StableSelfLocator::centerCircleYAngleTrust =
       GT2005StableSelfLocator::centerCircleWeight / ( GT2005StableSelfLocator::centerCircleYAngleVariance + GT2005StableSelfLocator::quasiZero ),
       GT2005StableSelfLocator::centerCircleOrientationAngleTrust =
       GT2005StableSelfLocator::centerCircleWeight / ( GT2005StableSelfLocator::centerCircleOrientationAngleVariance + GT2005StableSelfLocator::quasiZero ),

       GT2005StableSelfLocator::flagWeight = 13,//13.643,
       GT2005StableSelfLocator::flagZAngleTrust =
       GT2005StableSelfLocator::flagWeight / ( GT2005StableSelfLocator::flagZAngleVariance + GT2005StableSelfLocator::quasiZero ),
       GT2005StableSelfLocator::flagYAngleTrust = 
       GT2005StableSelfLocator::flagWeight / ( GT2005StableSelfLocator::flagYAngleVariance + GT2005StableSelfLocator::quasiZero ),

          
       GT2005StableSelfLocator::goalWeight = 13,//13.643,
       GT2005StableSelfLocator::goalZAngleTrust = 
       GT2005StableSelfLocator::goalWeight / ( GT2005StableSelfLocator::goalZAngleVariance + GT2005StableSelfLocator::quasiZero ),
       GT2005StableSelfLocator::goalYAngleTrust = 
       GT2005StableSelfLocator::goalWeight / ( GT2005StableSelfLocator::goalYAngleVariance + GT2005StableSelfLocator::quasiZero );

static inline double fmax(double a, double b)
{
  return a > b ? a : b;
}



/*---------------------------- class GT2005StableSelfLocatorII methods ------------------------------*/


GT2005StableSelfLocator::GT2005StableSelfLocator(const SelfLocatorInterfaces& interfaces)
: SelfLocator(interfaces)
{
  numberOfTypes = 0;
  timeStamp = 0;
  testSample = 0;
  speed = 0;
  Vector2<double> nullvec(0,0);
  lastModelCrossing = nullvec;
  lastSeenCrossing = nullvec;


  for(int i = 0; i < SAMPLES_MAX; i++)
    (Pose2D&) sampleSet[i] = field.randomPose();

  timeStamp = 0;
  sampleTemplateGenerator = GT2005StableSampleTemplateGenerator();
  lineCrossingsTable = GT2005StableLineCrossingsTable();
  odometryPose = field.randomPose();
  odometryPoseResetted = false;

  winningValidity = 0; secondaryValidity = 0;

  OUTPUT(idText, text, "GT2005Stable SelfLocator initialized");

  OUTPUT(idText,text,"line Z trust: " << linePointZAngleTrust);
  OUTPUT(idText,text,"line Y trust: " << linePointYAngleTrust);
  OUTPUT(idText,text,"crossings Z trust: " << crossingZAngleTrust);
  OUTPUT(idText,text,"crossings Y trust: " << crossingYAngleTrust);
  OUTPUT(idText,text,"flags Z trust: " << flagZAngleTrust);
  OUTPUT(idText,text,"flags Y trust: " << flagYAngleTrust);
  OUTPUT(idText,text,"goal Z trust: " << goalZAngleTrust);
  OUTPUT(idText,text,"goal Y trust: " << goalYAngleTrust);

  #ifdef LARGE_FIELD
    OUTPUT(idText, text, "old field");
  #endif
#ifdef HUGE_FIELD
    OUTPUT(idText, text, "new field");
#endif
}


void GT2005StableSelfLocator::motionUpdate(const Pose2D& odometry, const Pose2D& camera, bool noise)
{
  double transNoise = noise ? translationNoise : 0;
  double rotNoise   = noise ? rotationNoise : 0;
  double transX     = odometry.translation.x;
  double transY     = odometry.translation.y;
  double dist     = odometry.translation.abs();
  double angle    = fabs(odometry.getAngle());

  /* precalculate rotational error that has to be adapted to all samples */
  double rotError   = fmax(dist*movedDistWeight,    
                           angle*movedAngleWeight); 

  /* precalculate translational error that has to be adapted to all samples */
  double transXError = fmax( fabs(transX*majorDirTransWeight),
                             fabs(transY*minorDirTransWeight)); 
  double transYError = fmax( fabs(transY*majorDirTransWeight),
                             fabs(transX*minorDirTransWeight)); 

  double improbability;

  /* for each sample:
    -move by odometry
    -apply motion dependent error 
    -apply probability dependent error
    */ 
  int i;
  for (i = 0; i < SAMPLES_MAX; i++)
  {
    GT2005StableSelfLocatorSample& s = sampleSet[i];
    s += odometry;

    if (s.probability>1) improbability = 0;           // for uninitialized or faulty samples we can't calc noise
    else improbability = (1-s.probability)*(1-s.probability);

    // improbability = 1;

    rotError = (random()*2-1) * fmax(rotError,rotNoise*improbability);

    /* the translational error vector*/
    double sampleTransXError = max(transXError,transNoise*improbability);
    double sampleTransYError = max(transYError,transNoise*improbability);
    Vector2<double> transErrorVec( (random()*2-1)*sampleTransXError,
                                   (random()*2-1)*sampleTransYError );

    /* update the sample */
    s += Pose2D(rotError, transErrorVec);
    s.camera = s + camera;               
  }
    
}


GT2005StableLineCrossingsTable::CrossingClass GT2005StableSelfLocator::getCrossingClassification(const LinesPercept::LineCrossingPoint& point)
{
  if (point.outOfImage)
    return GT2005StableLineCrossingsTable::unknownCrossing;


  int numberOfWhite = (point.side1 == LinesPercept::lineOnThisSide ? 1 : 0) +
                      (point.side2 == LinesPercept::lineOnThisSide ? 1 : 0) +
                      (point.side3 == LinesPercept::lineOnThisSide ? 1 : 0) +
                      (point.side4 == LinesPercept::lineOnThisSide ? 1 : 0);

  int numberOfGreen = (point.side1 == LinesPercept::noLineOnThisSide ? 1 : 0) +
                      (point.side2 == LinesPercept::noLineOnThisSide ? 1 : 0) +
                      (point.side3 == LinesPercept::noLineOnThisSide ? 1 : 0) +
                      (point.side4 == LinesPercept::noLineOnThisSide ? 1 : 0);

  if (numberOfWhite == 2 && numberOfGreen == 2)
  {
    if ( (point.side1==LinesPercept::lineOnThisSide && point.side3==LinesPercept::lineOnThisSide &&
          point.side2==LinesPercept::noLineOnThisSide && point.side4==LinesPercept::noLineOnThisSide) ||
         (point.side1==LinesPercept::noLineOnThisSide && point.side3==LinesPercept::noLineOnThisSide &&
          point.side2==LinesPercept::lineOnThisSide && point.side4==LinesPercept::lineOnThisSide) )
      return GT2005StableLineCrossingsTable::virtualCrossing;
    else 
      return GT2005StableLineCrossingsTable::lCrossing;
  } else
    if (numberOfWhite == 3) 
      return GT2005StableLineCrossingsTable::tCrossing;
    else 
      return GT2005StableLineCrossingsTable::unknownCrossing;
}


// for the moment this is just inserted into the line crossings percept value
void GT2005StableSelfLocator::updateByCenterCircle(const LinesPercept::CenterCircle& centerCircle)
{
  Vector2<int> point = centerCircle.location;
  double pointDist = point.abs();
 
         // seen angle to the linepoint in z-direction (height) assuming the robots height is paramHeight
  double observedZAngle = atan2(headHeight,pointDist),  
         // seen angle to the linepoint in y-direction 
         observedYAngle = atan2((double)point.y,(double)point.x);


  // pose describing where the linepoint is seen from the robot and under which angle
  Pose2D point2(centerCircle.orientation,(double)point.x,(double)point.y);
  Pose2D pointFromPose = robotPose + point2;

  //debug stuff
  lastSeenCenterCircle = pointFromPose;
  // OUTPUT(idText,text,"seen angle: " << centerCircle.orientation << " field angle: " << pointFromPose.rotation);

  // update all samples..
  for(int i = 0; i < SAMPLES_MAX; ++i)
  {
    GT2005StableSelfLocatorSample& s = sampleSet[i];
    // the absolute position of the point if it was seen from the samples position
    Pose2D pointFromSample = s + point2; 
    Vector2<double> vMin(0,0);

    Vector2<double> v = (Pose2D(vMin) - s).translation;

    // the z and y directed angle derived from the closest point
    double zModel = atan2(headHeight,v.abs()),
           yModel = atan2(v.y,v.x);

    double orientationDiff1 = fabs(fromDegrees(90) - pointFromSample.rotation);
    double orientationDiff2 = fabs(fromDegrees(270) - pointFromSample.rotation);
    double orientationDiff;

    if (centerCircle.orientationKnown)
    {
      orientationDiff = orientationDiff1 < orientationDiff2 ? orientationDiff1 : orientationDiff2;
    } else
    {
      orientationDiff = 0;
    }

    double val = sigmoid((zModel - observedZAngle) * centerCircleZAngleTrust) * 
                 sigmoid((yModel - observedYAngle) * centerCircleYAngleTrust) *
                 sigmoid(orientationDiff * centerCircleOrientationAngleTrust);
    // 
    
    s.setPerceptProbability(GT2005StableSelfLocatorSample::lineCrossing , val);                              
  }
}

void GT2005StableSelfLocator::updateByCrossing(const LinesPercept::LineCrossingPoint& crossingPoint)
{
  // COMPLEX_DRAWING(selfLocator,draw(point,type));  
  Vector2<int> point = crossingPoint.locationOnField;
  double pointDist = point.abs();
  GT2005StableLineCrossingsTable::CrossingClass crossingClass = getCrossingClassification(crossingPoint);

         // seen angle to the linepoint in z-direction (height) assuming the robots height is paramHeight
  double observedZAngle = atan2(headHeight,pointDist),  
         // seen angle to the linepoint in y-direction 
         observedYAngle = atan2((double)point.y,(double)point.x);


  // pose describing where the linepoint is seen from the robot and under which angle
  Pose2D point2(point);
  Pose2D pointFromPose = robotPose + point2;

  // crossing filtering
  Vector2<double> testCrossing = lineCrossingsTable.getClassifiedClosestPoint(pointFromPose.translation,GT2005StableLineCrossingsTable::falseCrossing);
  Vector2<double> testDiff = testCrossing - pointFromPose.translation;
  if (testDiff.abs()<300)
  {
    CIRCLE(selfLocatorField,pointFromPose.translation.x,pointFromPose.translation.y,40,40,Drawings::ps_solid,Drawings::yellow);
    return;
  }

  //debug stuff
        lastSeenCrossing = pointFromPose.translation;
        Vector2<double> vm = lineCrossingsTable.getClassifiedClosestPoint(pointFromPose.translation,crossingClass);
        lastModelCrossing = vm;
        lastSeenCrossingClass = crossingClass;

  // update all samples..
  for(int i = 0; i < SAMPLES_MAX; ++i)
  {
    GT2005StableSelfLocatorSample& s = sampleSet[i];
    // the absolute position of the point if it was seen from the samples position
    Pose2D pointFromSample = s + point2; 
    // stores the vector pointing to the nearest point of the given type
    Vector2<double> vMin;
    vMin = lineCrossingsTable.getClassifiedClosestPoint(pointFromSample.translation,crossingClass);

    // if we got a closest point..
    if(vMin.x != 1000000)
    {
      // Vector2<double> diff = vMin - pointFromSample.translation;
      Vector2<double> v = (Pose2D(vMin) - s).translation;
      // the z and y directed angle derived from the closest point
      double zModel = atan2(headHeight,v.abs()),
             yModel = atan2(v.y,v.x);

      // testing this way this equals the similarity function of flags and goals
      /*
      double normedSimilarity = fabs((zModel - observedZAngle)*0.8) + fabs((yModel - observedYAngle)*0.2);
      s.setPerceptProbability(isY ? (LinesPercept::LineType)GT2005StableSelfLocatorSample::perceptTypeYFieldLine : type, 
                              sigmoid(normedSimilarity*7));
                              */

      double val = sigmoid((zModel - observedZAngle) * crossingZAngleTrust) * 
                   sigmoid((yModel - observedYAngle) * crossingYAngleTrust);
      // if (val>0.3) OUTPUT(idText,text,"crossings value" << val);
      s.setPerceptProbability(GT2005StableSelfLocatorSample::lineCrossing , val);
                              
    }
    else
      s.setPerceptProbability(GT2005StableSelfLocatorSample::lineCrossing,0.000001);
  }

}

// TODO: document, rewrite
void GT2005StableSelfLocator::updateByPoint(const LinesPercept::LinePoint& point,GT2005StableSelfLocatorSample::PerceptType type)
{
  // COMPLEX_DRAWING(selfLocator,draw(point,type));  
  
  GT2005StableSelfLocatorSample::PerceptType index = type;

  // as tables are built for the red team we have to swap indices for the goals
  // if(teamColorBlue && (type >= LinesPercept::yellowGoal) 
  //   index = LinesPercept::yellowGoal + LinesPercept::skyblueGoal - type;
  if (teamColorBlue)
  {
    if (type==GT2005StableSelfLocatorSample::skyblueGoal)
      index = GT2005StableSelfLocatorSample::yellowGoal;
    else if (type==GT2005StableSelfLocatorSample::yellowGoal)
      index = GT2005StableSelfLocatorSample::skyblueGoal;
  }

  double pointDist = point.abs();

         // seen angle to the linepoint in z-direction (height) assuming the robots height is paramHeight
  double observedZAngle = atan2(headHeight,pointDist),  
         // seen angle to the linepoint in y-direction 
         observedYAngle = atan2((double)point.y,(double)point.x);


  // pose describing where the linepoint is seen from the robot and under which angle
  Pose2D point2(point.angle, Vector2<double>(point.x, point.y));

  // update all samples..
  for(int i = 0; i < SAMPLES_MAX; ++i)
  {
    GT2005StableSelfLocatorSample& s = sampleSet[i];
    // the absolute position of the point if it was seen from the samples position
    Pose2D pointFromSample = s + point2; 
    

    // stores the vector pointing to the nearest point of the given type
    Vector2<double> vMin;

    // is this a line running in y direction?
    bool isY = (index == GT2005StableSelfLocatorSample::xFieldLine || index==GT2005StableSelfLocatorSample::yFieldLine) && 
              (fabs(pointFromSample.getAngle()) < pi / 4 || 
               fabs(pointFromSample.getAngle()) > 3 * pi / 4);
    if(isY)
      // y-directed lines are stored here
      vMin = observationTable[LinesPercept::numberOfLineTypes].getClosestPoint(pointFromSample.translation);
    else
      // else we find the closest point in the corresponding observationTable
      switch(index)
      {
        case GT2005StableSelfLocatorSample::xFieldLine:
        case GT2005StableSelfLocatorSample::yFieldLine:
          vMin = observationTable[LinesPercept::field].getClosestPoint(pointFromSample.translation);
          break;
        //case GT2005StableSelfLocatorSample::border:
        //  vMin = observationTable[LinesPercept::border].getClosestPoint(pointFromSample.translation);
        //  break;
        case GT2005StableSelfLocatorSample::yellowGoal:
          vMin = observationTable[LinesPercept::yellowGoal].getClosestPoint(pointFromSample.translation);
          break;
        case GT2005StableSelfLocatorSample::skyblueGoal:
          vMin = observationTable[LinesPercept::skyblueGoal].getClosestPoint(pointFromSample.translation);
          break;
        case GT2005StableSelfLocatorSample::lineCrossing:
          OUTPUT(idText,text,"please use updateByCrossing for line crossings");
          return; // vMin = lineCrossingsTable.getClosestPoint(pointFromSample.translation,GT2005StableLineCrossingsTable::lineCrossing);
          break;
      }

    // if we got a closest point..
    if(vMin.x != 1000000)
    {
      // Vector2<double> diff = vMin - pointFromSample.translation;
      Vector2<double> v = (Pose2D(vMin) - s).translation;
      // the z and y directed angle derived from the closest point
      double zModel = atan2(headHeight,v.abs()),
             yModel = atan2(v.y,v.x);

      // testing this way this equals the similarity function of flags and goals
      /*
      double normedSimilarity = fabs((zModel - observedZAngle)*0.8) + fabs((yModel - observedYAngle)*0.2);
      s.setPerceptProbability(isY ? (LinesPercept::LineType)GT2005StableSelfLocatorSample::perceptTypeYFieldLine : type, 
                              sigmoid(normedSimilarity*7));
                              */


      s.setPerceptProbability(isY ? GT2005StableSelfLocatorSample::yFieldLine : type, 
                              sigmoid((zModel - observedZAngle) * linePointZAngleTrust) * 
                              sigmoid((yModel - observedYAngle) * linePointYAngleTrust));
                              
    }
    else
      s.setPerceptProbability(type,0.000001);
  }
}


void GT2005StableSelfLocator::lineObservationUpdate(const LinesPercept& linesPercept)
{
    const int maxPercepts = 5; // maximum number of percepts used per linetype
    int i;
   
    // updating by center circle
    /*
    if (linesPercept.centerCircleFound){
      updateByCenterCircle(linesPercept.centerCircle);
    }
    */

    // debug drawings
    CIRCLE(selfLocatorField,lastSeenCenterCircle.translation.x,lastSeenCenterCircle.translation.y,60,30,Drawings::ps_solid,Drawings::white);
    COMPLEX_DRAWING(selfLocatorField,draw(lastSeenCenterCircle,Drawings::white));      

    int color = Drawings::black;
    switch(lastSeenCrossingClass)
    {
      case GT2005StableLineCrossingsTable::unknownCrossing:
        color = Drawings::black;
        break;
      case GT2005StableLineCrossingsTable::virtualCrossing:
        color = Drawings::blue;
        break;
      case GT2005StableLineCrossingsTable::lCrossing:
        color = Drawings::yellow;
        break;
      case GT2005StableLineCrossingsTable::tCrossing:
        color = Drawings::red;
        break;
    }
    CIRCLE(selfLocatorField,lastModelCrossing.x,lastModelCrossing.y,40,40,Drawings::ps_solid,color);
    CIRCLE(selfLocatorField,lastSeenCrossing.x,lastSeenCrossing.y,30,30,Drawings::ps_solid,Drawings::red);
    LINE(selfLocatorField,lastSeenCrossing.x,lastSeenCrossing.y,lastModelCrossing.x,lastModelCrossing.y,10,Drawings::ps_solid,Drawings::blue);


    // this wil be replaced by a nicer solution in the LinesPercept
    // OUTPUT(idText,text,"crossings " << linesPercept.numberOfLineCrossings);
    for(i=0;i<linesPercept.numberOfLineCrossings && i<3;i++)
    {
      updateByCrossing(linesPercept.lineCrossings[i]);
      observationUpdateDone = true;
    }


    ///COPIED FROM UNSTABLE VERSION!!!///
    for(i = 0; i < 2; ++i)
      for(int j = 0; j < linesPercept.points[i].numberOfPoints && j < maxPercepts; ++j)
      {
        switch(i)
        {
          case LinesPercept::field:
            updateByPoint(linesPercept.points[i].pointsOnField[rand() % linesPercept.points[i].numberOfPoints],GT2005StableSelfLocatorSample::xFieldLine);
            break;
          //case LinesPercept::border:
          //  updateByPoint(linesPercept.points[i].pointsOnField[rand() % linesPercept.points[i].numberOfPoints],GT2005StableSelfLocatorSample::border);
          //  break;

        }
        observationUpdateDone = true;
      }


    //for(i = 0; i < 2; ++i)
    //  for(int j = 0; j < linesPercept.numberOfPoints[i] && j < maxPercepts; ++j)
    //  {
    //    switch(i)
    //    {
    //      case LinesPercept::field:
    //        updateByPoint(linesPercept.points[i][rand() % linesPercept.numberOfPoints[i]],GT2005StableSelfLocatorSample::xFieldLine);
    //        break;
    //      case LinesPercept::border:
    //        updateByPoint(linesPercept.points[i][rand() % linesPercept.numberOfPoints[i]],GT2005StableSelfLocatorSample::border);
    //        break;

    //    }
    //    observationUpdateDone = true;
    //  }
    
      // for template generation - will find another solution
    //if(linesPercept.numberOfPoints[LinesPercept::yellowGoal])
    //  types[numberOfTypes++] = LinesPercept::yellowGoal;
    //if(linesPercept.numberOfPoints[LinesPercept::skyblueGoal])
    //  types[numberOfTypes++] = LinesPercept::skyblueGoal;
    if(linesPercept.points[LinesPercept::yellowGoal].numberOfPoints)
      types[numberOfTypes++] = LinesPercept::yellowGoal;
    if(linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints)
      types[numberOfTypes++] = LinesPercept::skyblueGoal;
    

    if(linesPercept.points[LinesPercept::yellowGoal].numberOfPoints ||
      linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints)
    //if(linesPercept.numberOfPoints[LinesPercept::yellowGoal] ||
    //   linesPercept.numberOfPoints[LinesPercept::skyblueGoal])
      for(int j = 0; j < linesPercept.points[LinesPercept::yellowGoal].numberOfPoints +
                         linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints && j < maxPercepts; ++j)
      //for(int j = 0; j < linesPercept.numberOfPoints[LinesPercept::yellowGoal] +
      //                   linesPercept.numberOfPoints[LinesPercept::skyblueGoal] && j < maxPercepts; ++j)
      { // This works if only one goal or both were seen
        LinesPercept::LineType select = (random() + 1e-6) * linesPercept.points[LinesPercept::yellowGoal].numberOfPoints >
                                        linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints
                                        ? LinesPercept::yellowGoal
                                        : LinesPercept::skyblueGoal;

        switch(select)
        {
          case LinesPercept::yellowGoal:
            updateByPoint(linesPercept.points[select].pointsOnField[rand() % linesPercept.points[select].numberOfPoints],GT2005StableSelfLocatorSample::yellowGoal);
            break;
          case LinesPercept::skyblueGoal:
            updateByPoint(linesPercept.points[select].pointsOnField[rand() % linesPercept.points[select].numberOfPoints],GT2005StableSelfLocatorSample::skyblueGoal);
            break;
        }


        //LinesPercept::LineType select = (random() + 1e-6) * linesPercept.numberOfPoints[LinesPercept::yellowGoal] >
        //                                linesPercept.numberOfPoints[LinesPercept::skyblueGoal]
        //                                ? LinesPercept::yellowGoal
        //                                : LinesPercept::skyblueGoal;

        //switch(select)
        //{
        //  case LinesPercept::yellowGoal:
        //    updateByPoint(linesPercept.points[select][rand() % linesPercept.numberOfPoints[select]],GT2005StableSelfLocatorSample::yellowGoal);
        //    break;
        //  case LinesPercept::skyblueGoal:
        //    updateByPoint(linesPercept.points[select][rand() % linesPercept.numberOfPoints[select]],GT2005StableSelfLocatorSample::skyblueGoal);
        //    break;
        //}
        observationUpdateDone = true;
      }   
}



void GT2005StableSelfLocator::updateByNotSeenFlag(Flag::FlagType type){
  switch(type)
  {
    case Flag::pinkAboveSkyblue:
      break;
    case Flag::pinkAboveYellow:
      break;
    case Flag::skyblueAbovePink:
      break;
    case Flag::yellowAbovePink:
      break;
  }
}

//void GT2005StableSelfLocator::updateByNotSeenGoal(colorClass goalColor){
//  Vector2<int> limit1(0,0);
//  Vector2<int> limit2(0,0);
//
//  // this is implemented for the blue team
//  if (!teamColorBlue)
//  {
//    goalColor = goalColor==skyblue ? yellow : skyblue;
//  }
//
//  switch(goalColor)
//  {
//    case skyblue:
//      limit1.x = xPosOwnGoalpost;
//      limit2.x = xPosOwnGoalpost;
//      limit1.y = yPosLeftGoal;
//      limit2.y = yPosRightGoal;
//      break;
//    case yellow:
//      limit1.x = xPosOpponentGoalpost;
//      limit2.x = xPosOpponentGoalpost;
//      limit1.y = yPosLeftGoal;
//      limit2.y = yPosRightGoal;
//      break;
//    default:
//      return;
//  }
//
//  int i;
//  Vector2<int> imagePos1, imagePos2;
//  for(i=0;i<SAMPLES_MAX;i++)
//  {
//    GT2005StableSelfLocatorSample& s = sampleSet[i];
//
//    Pose2D limit1FromSample(limit1.x,limit1.y);
//    limit1FromSample -= s.camera;
//
//    Pose2D limit2FromSample(limit2.x,limit2.y);
//    limit2FromSample -= s.camera;
//
//    Vector2<int> limit1Pos((int)limit1FromSample.translation.x,(int)limit1FromSample.translation.y);
//    Vector2<int> limit2Pos((int)limit2FromSample.translation.x,(int)limit2FromSample.translation.y);
//
//    if ((limit1Pos.abs()<2000 || limit2Pos.abs()<2000) )
//    {
//      Geometry::calculatePointInImage(limit1Pos,cameraMatrix,cameraInfo,imagePos1);
//      Geometry::calculatePointInImage(limit2Pos,cameraMatrix,cameraInfo,imagePos2);
//
//      // scan line between imagePos1 and imagePos2 for obstacles
//
//      // not seen goal at least partially inside virtual image?
//      if (imagePos1.x>0 && imagePos2.x>0 &&
//          imagePos1.x<cameraInfo.resolutionWidth && imagePos2.x<cameraInfo.resolutionWidth &&
//          imagePos1.y>0 && imagePos2.y>0 &&
//          imagePos1.y<cameraInfo.resolutionHeight && imagePos2.y<cameraInfo.resolutionHeight)
//      {
//        // decrease probability
//        // hack: only one goal is regarded so we dont have to care about which goal to take
//        CIRCLE(selfLocatorField,s.translation.x,s.translation.y,40,10,Drawings::ps_solid,Drawings::yellow);
//        s.setPerceptProbability(GT2005StableSelfLocatorSample::skyblueGoal,0.5);
//      }
//    }
//  }
//
//}

// TODO: maybe use vertical angles too?
// make similarity factor configurable
void GT2005StableSelfLocator::updateByFlag(const Vector2<double>& flagFieldPosition,
                                     FlagSides sideOfFlag,
                                     double measuredBearing)
{
  observationUpdateDone = true;
  int i;
  for(i = 0; i < SAMPLES_MAX; ++i)
  {
    GT2005StableSelfLocatorSample& s = sampleSet[i];

     // flagPos: position of the flag relative to sample s
    Pose2D flagPos(flagFieldPosition.x,flagFieldPosition.y);
    flagPos -= s.camera;

     // horizontal angle between flag and camera coordinate system
    double assumedBearing = atan2(flagPos.translation.y,flagPos.translation.x) + sideOfFlag * asin(flagRadius / flagPos.translation.abs());

     // similarity in numbers of pi - handy as we need a similarity between 0 and 1
    double similarity = fabs((assumedBearing - measuredBearing)) / pi;

    // use symmetry for full circle
    if(similarity > 1)
      similarity = 2.0 - similarity;

    // now we have a similarity measure between 0..1
    // make similarity * 7 configurable...
    if (similarity>0.01){
      // COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::red));
      if (testSample == &s){
        COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::blue));
      }
    }
    s.setPerceptProbability(GT2005StableSelfLocatorSample::flag,sigmoid(similarity * flagYAngleTrust));
  }
}


// look at updateByFlag
// goalPost is a Vector pointing on the used goalpost
// TODO: maybe introduce new PerceptType GoalPost instead of using "Flag"
void GT2005StableSelfLocator::updateByGoalPost(const Vector2<double>& goalPost,
                                         double measuredBearing)
{ 
  observationUpdateDone = true;
  int i;
  for(i = 0; i < SAMPLES_MAX; ++i)
  {
    GT2005StableSelfLocatorSample& s = sampleSet[i];
    Pose2D p(goalPost.x,goalPost.y);
    p -= s.camera;
    double assumedBearing = atan2(p.translation.y,p.translation.x);
    double similarity = fabs((assumedBearing - measuredBearing)) / pi;
    if(similarity > 1)
      similarity = 2 - similarity;
  
  // cast to LineType is needed for compatibility with the rest of GT04, maybe change
  // method parameters later
    s.setPerceptProbability(GT2005StableSelfLocatorSample::flag,sigmoid(similarity * goalYAngleTrust));
  }
}



/*
*  update observation information by all flags and goalposts seen
*/
void GT2005StableSelfLocator::landmarksObservationUpdate(const LandmarksPercept& landmarksPercept)
{
   int i;
   for(i = 0; i < landmarksPercept.numberOfFlags; i++)
    {
      // DEBUG_RESPONSE("selfLocator:showObservationUpdateDetails", OUTPUT(idText, text, "SLou: flag found"));
      const Flag& flag = landmarksPercept.flags[i];

    /* if the leftmost position of the flag is not on the robots image border.. */
      if(!flag.isOnBorder(flag.x.max))
        updateByFlag(flag.position,
                     LEFT_SIDE_OF_FLAG,
                     flag.x.max);
    /* if the rightmost position of the flag is not on the robots image border.. */
      if(!flag.isOnBorder(flag.x.min))
        updateByFlag(flag.position,
                     RIGHT_SIDE_OF_FLAG,
                     flag.x.min);

    /* for flag on border no update is done! */
    }

    /* same thing for goals */
    bool skyblueGoalSeen = false;
    bool yellowGoalSeen = false;
    for (i = 0; i < landmarksPercept.numberOfGoals; i++)
    {
      // DEBUG_RESPONSE("selfLocator:showObservationUpdateDetails", OUTPUT(idText, text, "SLou: goal found"));
      const Goal& goal = landmarksPercept.goals[i];
      if (goal.color == skyblue)
        skyblueGoalSeen = true;
      else if (goal.color == yellow)
        yellowGoalSeen = true;

      if(!goal.isOnBorder(goal.x.max))
        updateByGoalPost(goal.leftPost,
                         goal.x.max);
      if(!goal.isOnBorder(goal.x.min))
        updateByGoalPost(goal.rightPost,
                         goal.x.min);
      /*
      if(!goal.isOnBorder(goal.x.min) && !goal.isOnBorder(goal.x.max))
        OUTPUT(idText,text,"distance: " << goal.distance << "angle: " << goal.angle << "rot: " << goal.rotation);
        */
    }
    

    // update by not seen goals here..
    /*
    if (!skyblueGoalSeen)
      updateByNotSeenGoal(skyblue);
    if (!yellowGoalSeen)
      updateByNotSeenGoal(yellow);
      */
}



void GT2005StableSelfLocator::updateVariancesBySpeed(double speed)
{
  linePointZAngleMotionDependentVariance = linePointZAngleVariance * (1 + speed * linePointZAngleMotionDependency);
  linePointZAngleTrust = linePointWeight / (linePointZAngleMotionDependentVariance + quasiZero );
}

void GT2005StableSelfLocator::execute()
{
  teamColorBlue = (getPlayer().getTeamColor() == Player::blue);

/*
  headHeightBuffer.add(cameraMatrix.translation.z);
  headHeight = headHeightBuffer.getNumberOfEntries()>0 ? 
  headHeightBuffer.getSum() / headHeightBuffer.getNumberOfEntries() : headHeight;
  */
  // OUTPUT(idText,text,paramHeadHeight);
  // OUTPUT(idText,text,sampleSet.getNumberOfSamples());

  robotPose.setFrameNumber(landmarksPercept.frameNumber);
  robotPose.setTimestamp(SystemCall::getCurrentSystemTime());

  //distanceToBorderEstimator.execute();

  Pose2D odometryDelta = odometryData - lastOdometry;
  lastOdometry = odometryData;

  // this is about to go to the motion model
  if(SystemCall::getTimeSince(timeStamp) > 500)
  {
    speed = (odometryData - lastOdometry2).translation.abs() / SystemCall::getTimeSince(timeStamp) * 1000.0;
    lastOdometry2 = odometryData;
    timeStamp = SystemCall::getCurrentSystemTime();
  }

  // updateVariancesBySpeed(speed);

  // motion updating
  motionUpdate(odometryDelta,Pose2D(landmarksPercept.cameraOffset.x,
               landmarksPercept.cameraOffset.y), 
               observationUpdateDone);

  observationUpdateDone = false;
  numberOfTypes=0;

  // observation updating
  if (cameraMatrix.isValid)
  {
    lineObservationUpdate(linesPercept);
    landmarksObservationUpdate(landmarksPercept);

    if (observationUpdateDone)
    {
      sampleTemplateGenerator.generateTemplates(landmarksPercept,linesPercept,odometryDelta,numberOfTypes,types);
      resample();
    }
  }
  // taken from gt04..
  
  Pose2D pose;
  double validity;
  calcPose(pose,validity);
  // robotPose.setPose(pose);
  // robotPose.setValidity(validity);

  // OUTPUT(idText,text,validity);

  odometryPoseResetted = false;
  robotPose.setPose(pose);
  robotPose.setValidity(validity);

  sampleSet.link(selfLocatorSamples);


  DEBUG_DRAWING_FINISHED(selfLocatorField); 
  DEBUG_DRAWING_FINISHED(selfLocator);
  landmarksState.update(landmarksPercept);
}


// contains the part of the gt04 resampling method that calculates
// the average or each percept type probability and all samples
// averagePerceptTypeProb[i] = average for all samples if there are any, 0.5 else
// returns the product over all average percepttype probabilities
 
double GT2005StableSelfLocator::calcAveragePerceptTypeProbabilities()
{
  int count[GT2005StableSelfLocatorSample::numberOfPerceptTypes]; // number of samples per perceptType
  double probability = 1;                 //probability of all average possibilities = a1*a2*a3...

  // initialize count and averagePerceptTypeProb
  for (int k=0; k<GT2005StableSelfLocatorSample::numberOfPerceptTypes; k++){
    count[k] = 0;
    averagePerceptTypeProb[k] = 0;
  }

  int i; 
  for (i=0;i<SAMPLES_MAX;i++)
  {
    GT2005StableSelfLocatorSample& sample = sampleSet[i];
    for (int k=0; k<GT2005StableSelfLocatorSample::numberOfPerceptTypes; k++){
      if(sample.perceptProbabilities[k]<=1)
      {
        count[k]++;
        averagePerceptTypeProb[k] += sample.perceptProbabilities[k];
      }
    }
  }

  for (i=0;i<GT2005StableSelfLocatorSample::numberOfPerceptTypes;i++)
  {
    if (count[i]>0) averagePerceptTypeProb[i] /= count[i];
    else averagePerceptTypeProb[i] = 0.5;
    // OUTPUT(idText,text,"avg type "<< i << "  "<<averagePerceptTypeProb[i]);
    if (/*i!=GT2005StableSelfLocatorSample::lineCrossing &&*/ i!=GT2005StableSelfLocatorSample::skyblueGoal) probability *= averagePerceptTypeProb[i];
  }
  return probability;
}


/**
* redistibution of the sampleSet according to the probabilities of the samples
* reimplementation of the gt04 resampling method.
* TODO: template generation
*/
void GT2005StableSelfLocator::resample()
{
  // this fills averagePerceptTypeProb[] and returns the whole averageProbability
  double averageProbability = 
    calcAveragePerceptTypeProbabilities();
  // OUTPUT(idText,text,averageProbability);

  
  // calculate probability for all samples from their perceptType probabilities
  int i;
  for(i = 0; i < SAMPLES_MAX; ++i){
    sampleSet[i].updateProbability(averagePerceptTypeProb);
    // sampleSet[i].updateProbability();
  }

  // if (testSample!=0) OUTPUT(idText,text,"last Samples prob: "<< testSample->probability << " average = " << averageProbability);
  // swap sample arrays
  GT2005StableSelfLocatorSample* oldSet = sampleSet.swap();

  /* calculate the sum over the probabilities of all samples
   to determine the number of samples per 1.0 probability (prob2Index)
   this is needed to calculate how often a sample is multiplied
   -> a sample with probability 1.0 would be inserted prob2Index times
   into the new distribution  */
   double probSum = 0;
   for(i = 0; i < SAMPLES_MAX; ++i)
     probSum += oldSet[i].getProbability();
  double prob2Index = (double) SAMPLES_MAX / probSum;

  double indexSum = 0;  // number of indices we covered in the new 
                        // sample set. 
  int j = 0;

  // here starts resampling step, we copy the samples according
  // to their probability, keeping the probabilities within
  // narrow bounds
  for(i = 0; i < SAMPLES_MAX; ++i)
  {
    // calculate how far we will proceed in the new sampleset with this sample
    indexSum += oldSet[i].getProbability() * prob2Index;
    while(j < SAMPLES_MAX && j < indexSum - 0.5)
      sampleSet[j++] = oldSet[i];
  }

  // finally determine which samples we will replace randomly by 
  // template samples
  for(i = 0; i < SAMPLES_MAX; i++)
  {
    GT2005StableSelfLocatorSample& s = sampleSet[i];
    // if quality is too low, select a new random pose for the sample
    /* template generation not yet implemented*/
    // OUTPUT(idText,text,"number of templates" << sampleTemplateGenerator.getNumberOfTemplates());
    if(s.getProbability()<=1 && sampleTemplateGenerator.getNumberOfTemplates() && random() * averageProbability > s.getProbability())
    {
      s = sampleTemplateGenerator.getTemplate();
      testSample = &s;
      COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::yellow));
    }
  
  /*  
    if(random() * averageProbability > s.getProbability()){
      s = GT2005StableSelfLocatorSample(field.randomPose());
      COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::red));
    }*/
    else
    {
      COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::black));
    }
  }
}



double GT2005StableSelfLocator::calcDistributionValidity()
{
  double varianceCovarianceMatrix[3][3];
  double averageXDeviation = 0;
  double averageYDeviation = 0;
  double averageRotDeviation = 0;

  int i;
  for (i=0;i<SAMPLES_MAX;i++)
  {
    averageXDeviation += fabs( sampleSet[i].translation.x - robotPose.translation.x);
    averageYDeviation += fabs( sampleSet[i].translation.y - robotPose.translation.y);
    double rotDiff = fabs(sampleSet[i].rotation - robotPose.rotation);
    if (rotDiff > pi)
      rotDiff -= pi;
    averageRotDeviation += fabs(rotDiff);
  }

  averageXDeviation /= SAMPLES_MAX;
  averageYDeviation /= SAMPLES_MAX;
  averageRotDeviation /= SAMPLES_MAX;

  varianceCovarianceMatrix[0][0] = averageXDeviation * averageXDeviation;
  varianceCovarianceMatrix[1][1] = averageYDeviation * averageYDeviation;
  varianceCovarianceMatrix[2][2] = averageRotDeviation * averageRotDeviation;

  varianceCovarianceMatrix[0][1] = averageXDeviation * averageYDeviation;
  varianceCovarianceMatrix[0][2] = averageXDeviation * averageRotDeviation;

  varianceCovarianceMatrix[1][0] = varianceCovarianceMatrix[0][1];
  varianceCovarianceMatrix[1][2] = averageYDeviation * averageRotDeviation;

  varianceCovarianceMatrix[2][0] = varianceCovarianceMatrix[0][2];
  varianceCovarianceMatrix[2][1] = varianceCovarianceMatrix[1][2];

  // for now, dont use the covariances
  // OUTPUT(idText,text,"....>" << sigmoid(sqrt(varianceCovarianceMatrix[0][0] + varianceCovarianceMatrix[1][1] + varianceCovarianceMatrix[2][2])/300 ));
  // return sigmoid( (varianceCovarianceMatrix[0][0] + varianceCovarianceMatrix[1][1] + varianceCovarianceMatrix[2][2]) / (3*300));
  return sigmoid( sqrt(varianceCovarianceMatrix[0][0] + varianceCovarianceMatrix[1][1] + varianceCovarianceMatrix[2][2]) / 200);
}


RobotPose GT2005StableSelfLocator::calcPoseFromSubCube(Cell poseSpace[POSE_SPACE_GRID][POSE_SPACE_GRID][POSE_SPACE_GRID], int xMax, int yMax, int rMax) 
{
    // determine the average pose of all samples in the winner sub-cube
    double xSum = 0,
           ySum = 0,
           cosSum = 0,
           sinSum = 0,
           probabilitySum = 0;
       
    RobotPose result;

  // iterate over the rotation axis of the sub-cube
    for(int r = 0; r < 2; r++)
    {
    // just as before: r axis is cyclic
      int rNext = (rMax + r) % POSE_SPACE_GRID;

      // iterate over the traslation axes...
      for(int y = 0; y < 2; y++)
        for(int x = 0; x < 2; x++)
        {
          // iterate over the list of samples inside the cell
          // cell.first is 0 if the queue is empty
          // else p->next is 0 at the end of the queue  
          for(GT2005StableSelfLocatorSample* sample = poseSpace[rNext][yMax + y][xMax + x].first; sample; sample = sample->next)
            if(sample->isValid())
            {
              // sum the x and y positions of the samples weighted by probability
              // sum the sine and cosine of the samples rotation weighted by probability
              // calculate the whole sum of probabilities to calculate the weighted averages
              xSum += sample->translation.x * sample->getProbability();
              ySum += sample->translation.y * sample->getProbability();
              cosSum += sample->getCos() * sample->getProbability();
              sinSum += sample->getSin() * sample->getProbability();
              probabilitySum += sample->getProbability();
            }
        }
    }  

  // now calculate the pose as the weighted averages of x,y,r
    if(probabilitySum>0)
    {
    double weightedX = xSum / probabilitySum;
    double weightedY = ySum / probabilitySum;
    double weightedRot = atan2(sinSum,cosSum);
      result.setPose(Pose2D(weightedRot,Vector2<double>(weightedX,weightedY)) );
      result.setValidity(probabilitySum / SAMPLES_MAX);
    // clip the pose to the field
      double diff = field.clip(result.translation);
      if(diff > 1){
        // double vold = validity;
        result.setValidity(result.getValidity() / sqrt(diff));  // gt04: Why do we use the sqrt?
      }
    } 

    return result;
}

/**
* The function determines the most probable pose from the sample distribution.
* reimplementation of the gt04 calcPose method
* @param pose The pose is returned to this variable.
* @param validity The validity of the pose is returned to this variable.
*/  
void GT2005StableSelfLocator::calcPose(Pose2D& pose,double& validity)
{
  // the 3d pose-space grid
  Cell poseSpace[POSE_SPACE_GRID][POSE_SPACE_GRID][POSE_SPACE_GRID];

  double width = field.x.getSize(),
         height = field.y.getSize();

  int i;
  // attach the samples to the corresponding cells of the poseSpace
  for(i = 0; i < SAMPLES_MAX; i++)
  {
    GT2005StableSelfLocatorSample& sample = sampleSet[i];
    // COMPLEX_DRAWING(selfLocatorField,draw(sample,Drawings::black));

    if(sample.isValid())
    {
    // r: Index on the rotation axis of the cube. as angles are between
    //    [-pi..0] [0..pi] we put the 0 of the rotation axis on 0.5*POSE_SPACE_GRID
    //    r = [(angle / 2*pi) + 0.5] * POSE_SPACE_GRID
      int r = static_cast<int>((sample.getAngle() / pi2 + 0.5) * POSE_SPACE_GRID);
    //  x,y: Index on the translation axes. similar to the rotation, the 0 position
    //       is at 0.5*POSE_SPACE_GRID.   
    int y = static_cast<int>((sample.translation.y / height + 0.5) * POSE_SPACE_GRID);
      int x = static_cast<int>((sample.translation.x / width + 0.5) * POSE_SPACE_GRID);

    // translation axes are non-cyclic, so we set [x|y] < 0 to 0 and 
    // [x|y] >= POSE_SPACE_GRID to POSE_SPACE_GRID-1
      if(x < 0) 
        x = 0;
      else if(x >= POSE_SPACE_GRID) 
        x = POSE_SPACE_GRID-1;

      if(y < 0) 
        y = 0;
      else if(y >= POSE_SPACE_GRID) 
        y = POSE_SPACE_GRID-1;

    // the rotation axis is cyclic so if we run out of the interval at 
    // the one side, we are at the other...
      if(r < 0) 
        r = POSE_SPACE_GRID-1;
      else if(r >= POSE_SPACE_GRID) 
        r = 0;

    // fill the queue of samples for this cell by 
    // inserting before first sample
      Cell& cell = poseSpace[r][y][x];
    //as cell.first is preinitialized with 0, the end
    //of the queue is always marked with 0
      sample.next = cell.first;
      cell.first = &sample;
      cell.count++;
    }
  } 
  
  // determine the 2x2x2 sub-cube that contains most samples

  // the position and sample-count of the winning sub-cube
  int rMax[NUM_OF_CALCULATED_POSES],
      yMax[NUM_OF_CALCULATED_POSES],
      xMax[NUM_OF_CALCULATED_POSES],
      countMax[NUM_OF_CALCULATED_POSES];

  for (i=0;i<NUM_OF_CALCULATED_POSES;i++)
  {
    rMax[i] = 0;
    yMax[i] = 0;
    xMax[i] = 0;
    countMax[i] = 0;
  }

  // iterate wih "r"over the rotation axis...
  for(int r = 0; r < POSE_SPACE_GRID; r++)
  {
  // as this axis is cyclic, the next rotation index is:
    int rNext = (r + 1) % POSE_SPACE_GRID;

  // iterate with "x" and "y" over the translation axes
    for(int y = 0; y < POSE_SPACE_GRID - 1; y++)
      for(int x = 0; x < POSE_SPACE_GRID - 1; x++)
      {
    // get the number of all samples in the subcube
    // around the current cell [r][y][x]
        int count = poseSpace[r][y][x].count +
                    poseSpace[r][y][x+1].count +
                    poseSpace[r][y+1][x].count +
                    poseSpace[r][y+1][x+1].count +
                    poseSpace[rNext][y][x].count +
                    poseSpace[rNext][y][x+1].count +
                    poseSpace[rNext][y+1][x].count +
                    poseSpace[rNext][y+1][x+1].count;
    // if we've got a new winning cube, we have to remember
        if(count > countMax[0])
        {

          if (NUM_OF_CALCULATED_POSES>1)
          {
            for (i=NUM_OF_CALCULATED_POSES-1;i>0;i--)
            {
              rMax[i] = rMax[i-1];
              xMax[i] = xMax[i-1];
              yMax[i] = yMax[i-1];
              countMax[i] = countMax[i-1];
            }
          }
          countMax[0] = count;
          rMax[0] = r;
          yMax[0] = y;
          xMax[0] = x;
        }
      }
  }

  if(countMax[0] > 0)
  {
    for (i=0;i<NUM_OF_CALCULATED_POSES;i++)
    {
      if (countMax[i]>0)
      {
        /*candidates*/robotPoseCollection.poses[i] = calcPoseFromSubCube(poseSpace,xMax[i],yMax[i],rMax[i]);
        CIRCLE(selfLocatorField,candidates[i].translation.x,candidates[i].translation.y,50,50,Drawings::ps_solid,Drawings::yellowOrange);
      } else 
      {
        /*candidates*/robotPoseCollection.poses[i].setValidity(-1);
      }
    }
    pose = /*candidates*/robotPoseCollection.poses[0].getPose();
    // validity = results[0].getValidity();
    validity = calcDistributionValidity();

    //robotPoseCollection.poses = candidates;
    robotPoseCollection.numberOfPoses = NUM_OF_CALCULATED_POSES;
  } else {
    validity = 0;
  }

  // Vector2<double> diff = gtCamWorldState.getRobotPose().translation - pose.translation;
  // OUTPUT(idText,text,"GT2005Stable "<< diff.abs() << " " << validity);
}

bool GT2005StableSelfLocator::handleMessage(InMessage& message)
{/*
  switch(message.getMessageID())
  {
    case idGT2005SelfLocator:
    {
      GenericDebugData d;
      message.bin >> d;
      GT2005StableSelfLocatorSample::paramProbUpLimit   = d.data[0];
      GT2005StableSelfLocatorSample::paramProbDownLimit = d.data[1];
      paramObservationUpdateZAngleErrorWeight  = d.data[2];
      paramObservationUpdateYAngleErrorWeight  = d.data[3];  // ie gauss sharpness for y-angle errors
      paramObservationUpdateZAngleErrorCrossings = d.data[4]; 
      paramObservationUpdateYAngleErrorCrossings = d.data[5];
    }
    break;
  }*/
  return false;
}





///////////////////////////////////////////////////////////////////////
// Debug drawing 1:1 taken from gt04

void GT2005StableSelfLocator::draw(const Pose2D& pose,Drawings::Color color) const
{ 
  Pose2D p = pose + Pose2D(-100,0);
  LINE(selfLocatorField,
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  p = pose + Pose2D(-40,-40);
  LINE(selfLocatorField,
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  p = pose + Pose2D(-40,40);
  LINE(selfLocatorField,
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
}


void GT2005StableSelfLocator::draw(const Vector2<int>& point,LinesPercept::LineType type) const
{ 
  static const Drawings::Color colors[] =
  {
    Drawings::red,
    Drawings::green,
    Drawings::yellow,
    Drawings::skyblue
  };

  CameraInfo cameraInfo(getRobotConfiguration().getRobotDesign());

  Vector2<int> p;
  Geometry::calculatePointInImage(
    point,
    cameraMatrix, cameraInfo,
    p);
  CIRCLE(selfLocator,p.x,p.y,3,1,Drawings::ps_solid,colors[type]);
}


/*
GT2005StableSelfLocator::~GT2005StableSelfLocator(void)
{
}*/

---