Modules/SelfLocator/SlamSelfLocator/SlamSelfLocator.cpp

Go to the documentation of this file.

#include "SlamSelfLocator.h"

#include "Tools/Math/Common.h"

#include "Representations/Perception/SlamPercept.h"

#define slamPercept (*(SlamPercept *)specialPercept.slamData)
/*
  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 SlamSelfLocator::translationNoise = 50, //200,
       SlamSelfLocator::rotationNoise = 0.25, //0.5,
       SlamSelfLocator::movedDistWeight = 0.002,
       SlamSelfLocator::movedAngleWeight = 0.2,
       SlamSelfLocator::majorDirTransWeight = 0.1,
       SlamSelfLocator::minorDirTransWeight = 0.02,

       SlamSelfLocator::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..

       SlamSelfLocator::quasiZero = 0.000001, 
       SlamSelfLocator::maximumTrust = 100,

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

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

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

       SlamSelfLocator::linePointWeight = 15,//7,//19.49,
       SlamSelfLocator::linePointZAngleTrust = 
        SlamSelfLocator::linePointWeight / ( SlamSelfLocator::linePointZAngleVariance + SlamSelfLocator::quasiZero ),
       SlamSelfLocator::linePointYAngleTrust = 
        SlamSelfLocator::linePointWeight / ( SlamSelfLocator::linePointYAngleVariance + SlamSelfLocator::quasiZero ),

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

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

//       SlamSelfLocator::flagWeight = 13.643,
       SlamSelfLocator::flagWeight = 0.0,
       SlamSelfLocator::flagZAngleTrust =
        SlamSelfLocator::flagWeight / ( SlamSelfLocator::flagZAngleVariance + SlamSelfLocator::quasiZero ),
       SlamSelfLocator::flagYAngleTrust = 
        SlamSelfLocator::flagWeight / ( SlamSelfLocator::flagYAngleVariance + SlamSelfLocator::quasiZero ),

          
//       SlamSelfLocator::goalWeight = 13.643,
       SlamSelfLocator::goalWeight = 0.0,
       SlamSelfLocator::goalZAngleTrust = 
        SlamSelfLocator::goalWeight / ( SlamSelfLocator::goalZAngleVariance + SlamSelfLocator::quasiZero ),
       SlamSelfLocator::goalYAngleTrust = 
        SlamSelfLocator::goalWeight / ( SlamSelfLocator::goalYAngleVariance + SlamSelfLocator::quasiZero );



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



/*---------------------------- class SlamSelfLocator methods ------------------------------*/


SlamSelfLocator::SlamSelfLocator(const SelfLocatorInterfaces& interfaces)
: SelfLocator(interfaces)
{
  slamPercept.centerCircleInformation.timeStampOfLastSeenCenterCircle = SystemCall::getCurrentSystemTime ();

  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 = SlamSampleTemplateGenerator();
  lineCrossingsTable = SlamLineCrossingsTable();
  odometryPose = field.randomPose();
  odometryPoseResetted = false;

  OUTPUT(idText, text, "Slam 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);
}


void SlamSelfLocator::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++)
  {
    SlamSelfLocatorSample& 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;               
  }
    
}

SlamLineCrossingsTable::CrossingClass SlamSelfLocator::getCrossingClassification(const LinesPercept::LineCrossingPoint& point)
{
  if (point.outOfImage)
    return SlamLineCrossingsTable::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 SlamLineCrossingsTable::virtualCrossing;
    else 
      return SlamLineCrossingsTable::lCrossing;
  } else
    if (numberOfWhite == 3) 
      return SlamLineCrossingsTable::tCrossing;
    else 
      return SlamLineCrossingsTable::unknownCrossing;
}


void SlamSelfLocator::updateByEstimatedDirection()
{
  if (!slamPercept.viewAngleValid)
    return;

  int i;
  for(i=0;i<SAMPLES_MAX;++i)
  {
    SlamSelfLocatorSample& s = sampleSet[i];

    double diffAngle = s.getAngle () - slamPercept.viewAngle;

    while (diffAngle < -pi)
      diffAngle += pi2;

    if (diffAngle < 0)
      diffAngle = -diffAngle;

    if (diffAngle > pi_2)
    {
      //look into wrong direction
      s.translation.x = -s.translation.x;
      s.translation.y = -s.translation.y;

      s.rotation = s.rotation += pi;

      while (s.rotation > pi2)
        s.rotation -= pi2;
      NCIRCLE("SelfLocator:crossing",s.translation.x,s.translation.y,40,30,Drawings::ps_solid,Drawings::blue);
      CIRCLE(selfLocatorField,s.translation.x,s.translation.y,40,30,Drawings::ps_solid,Drawings::blue);
    };
    /*
    if (!FieldDimensions::isInsideField((int)(s.translation.x), (int)(s.translation.y)))
    {
      //outside
      s.probability *= 0.9;
    };*/

  }
}

// for the moment this is just inserted into the line crossings percept value
void SlamSelfLocator::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)
  {
    SlamSelfLocatorSample& 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(SlamSelfLocatorSample::flag , val);                              
  }
}

void SlamSelfLocator::updateByCrossing(const LinesPercept::LineCrossingPoint& crossingPoint)
{
  // COMPLEX_DRAWING(selfLocator,draw(point,type));  
  Vector2<int> point = crossingPoint.locationOnField;
  double pointDist = point.abs();
  SlamLineCrossingsTable::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,SlamLineCrossingsTable::falseCrossing);
  Vector2<double> testDiff = testCrossing - pointFromPose.translation;
  if (testDiff.abs()<300)
  {
    NCIRCLE("SelfLocator:crossings",pointFromPose.translation.x,pointFromPose.translation.y,40,40,Drawings::ps_solid,Drawings::yellow);
    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)
  {
    SlamSelfLocatorSample& 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)SlamSelfLocatorSample::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(SlamSelfLocatorSample::lineCrossing , val);
                              
    }
    else
      s.setPerceptProbability(SlamSelfLocatorSample::lineCrossing,0.000001);
  }

}

// TODO: document, rewrite
void SlamSelfLocator::updateByPoint(const LinesPercept::LinePoint& point,SlamSelfLocatorSample::PerceptType type)
{
  // COMPLEX_DRAWING(selfLocator,draw(point,type));  
  
  SlamSelfLocatorSample::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==SlamSelfLocatorSample::skyblueGoal)
      index = SlamSelfLocatorSample::yellowGoal;
    else if (type==SlamSelfLocatorSample::yellowGoal)
      index = SlamSelfLocatorSample::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)
  {
    SlamSelfLocatorSample& 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 == SlamSelfLocatorSample::xFieldLine || index==SlamSelfLocatorSample::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 SlamSelfLocatorSample::xFieldLine:
        case SlamSelfLocatorSample::yFieldLine:
          vMin = observationTable[LinesPercept::field].getClosestPoint(pointFromSample.translation);
          break;
        //case SlamSelfLocatorSample::border:
        //  vMin = observationTable[LinesPercept::border].getClosestPoint(pointFromSample.translation);
        //  break;
        case SlamSelfLocatorSample::yellowGoal:
          vMin = observationTable[LinesPercept::yellowGoal].getClosestPoint(pointFromSample.translation);
          break;
        case SlamSelfLocatorSample::skyblueGoal:
          vMin = observationTable[LinesPercept::skyblueGoal].getClosestPoint(pointFromSample.translation);
          break;
        case SlamSelfLocatorSample::lineCrossing:
          OUTPUT(idText,text,"please use updateByCrossing for line crossings");
          return; // vMin = lineCrossingsTable.getClosestPoint(pointFromSample.translation,SlamLineCrossingsTable::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)SlamSelfLocatorSample::perceptTypeYFieldLine : type, 
                              sigmoid(normedSimilarity*7));
                              */


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


void SlamSelfLocator::lineObservationUpdate(const LinesPercept& linesPercept)
{
    int maxPercepts = (slamPercept.slamState == SlamPercept::UseNewFlags ||
                       slamPercept.slamState == SlamPercept::LocalizeUsingNewFlags ) ? 5 : 3; // maximum number of percepts used per linetype
    int i;
   
    // updating by center circle
    if (linesPercept.centerCircleFound)
    {
      LinesPercept::CenterCircle centerCircle = linesPercept.centerCircle;
      double observedYAngle = atan2((double)centerCircle.location.y,(double)centerCircle.location.x);
    
      slamPercept.centerCircleInformation.centerCircleIsVisible = true;
      slamPercept.centerCircleInformation.distanceToCenterCircle = centerCircle.location.abs();
      slamPercept.centerCircleInformation.directionToCenterCircle = observedYAngle;
      slamPercept.centerCircleInformation.seenCenterCircleOrientation = linesPercept.centerCircle.orientationKnown ? linesPercept.centerCircle.orientation : 100;
      slamPercept.centerCircleInformation.timeStampOfLastSeenCenterCircle = SystemCall::getCurrentSystemTime ();
    } else
    {
      slamPercept.centerCircleInformation.centerCircleIsVisible = false;
    }

    slamPercept.centerCircleInformation.timeSinceLastSeenCenterCircle = SystemCall::getCurrentSystemTime () - slamPercept.centerCircleInformation.timeStampOfLastSeenCenterCircle;

    if (linesPercept.centerCircleFound && (slamPercept.slamState==SlamPercept::UseNewFlags || 
                                           slamPercept.slamState==SlamPercept::LocalizeUsingNewFlags))
    {
      OUTPUT(idText,text,"updating by center circle");
      updateByCenterCircle(linesPercept.centerCircle);
    }

    // debug drawings
    if (slamPercept.slamState==SlamPercept::UseNewFlags || slamPercept.slamState==SlamPercept::UseNewFlags)
    {
      NCIRCLE("SelfLocator:crossings",lastSeenCenterCircle.translation.x,lastSeenCenterCircle.translation.y,60,30,Drawings::ps_solid,Drawings::white);
      NCOMPLEX_DRAWING("SelfLocator:crossings",draw(lastSeenCenterCircle,Drawings::white));      

      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 SlamLineCrossingsTable::unknownCrossing:
        color = Drawings::black;
        break;
      case SlamLineCrossingsTable::virtualCrossing:
        color = Drawings::blue;
        break;
      case SlamLineCrossingsTable::lCrossing:
        color = Drawings::yellow;
        break;
      case SlamLineCrossingsTable::tCrossing:
        color = Drawings::red;
        break;
    }
    NCIRCLE("SelfLocator:crossings",lastModelCrossing.x,lastModelCrossing.y,40,40,Drawings::ps_solid,color);
    NCIRCLE("SelfLocator:crossings",lastSeenCrossing.x,lastSeenCrossing.y,30,30,Drawings::ps_solid,Drawings::red);
    NLINE("SelfLocator:crossings",lastSeenCrossing.x,lastSeenCrossing.y,lastModelCrossing.x,lastModelCrossing.y,10,Drawings::ps_solid,Drawings::blue);

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

    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],SlamSelfLocatorSample::xFieldLine);
            break;
          //case LinesPercept::border:
          //  updateByPoint(linesPercept.points[i].pointsOnField[rand() % linesPercept.points[i].numberOfPoints],SlamSelfLocatorSample::border);
          //  break;

        }
        observationUpdateDone = true;
      }
    
    if (slamPercept.slamState == SlamPercept::LearnNewFlags || slamPercept.slamState == SlamPercept::LocalizeWithFlags)
    {
    
    // OUTPUT(idText,text,"updating by goal lines etc");
    // for template generation - will find another solution
    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)
    for(int j = 0; j < linesPercept.points[LinesPercept::yellowGoal].numberOfPoints +
              linesPercept.points[LinesPercept::skyblueGoal].numberOfPoints && 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],SlamSelfLocatorSample::yellowGoal);
        break;
      case LinesPercept::skyblueGoal:
        updateByPoint(linesPercept.points[select].pointsOnField[rand() % linesPercept.points[select].numberOfPoints],SlamSelfLocatorSample::skyblueGoal);
        break;
      }
      observationUpdateDone = true;
    }   
    };
}



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

void SlamSelfLocator::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++)
  {
    SlamSelfLocatorSample& 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);
        NCIRCLE("SelfLocator:crossings",s.translation.x,s.translation.y,40,10,Drawings::ps_solid,Drawings::yellow);
        s.setPerceptProbability(SlamSelfLocatorSample::skyblueGoal,0.5);
      }*/
    }
  }

}

// TODO: maybe use vertical angles too?
// make similarity factor configurable
void SlamSelfLocator::updateByFlag(const Vector2<double>& flagFieldPosition,
                                     FlagSides sideOfFlag,
                                     double measuredBearing)
{
  observationUpdateDone = true;
  int i;
  for(i = 0; i < SAMPLES_MAX; ++i)
  {
    SlamSelfLocatorSample& 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){
        NCOMPLEX_DRAWING("SelfLocator:crossings",draw(s,Drawings::blue));
        COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::blue));
      }
    }
    s.setPerceptProbability(SlamSelfLocatorSample::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 SlamSelfLocator::updateByGoalPost(const Vector2<double>& goalPost,
                                         double measuredBearing)
{ 
  observationUpdateDone = true;
  int i;
  for(i = 0; i < SAMPLES_MAX; ++i)
  {
    SlamSelfLocatorSample& 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(SlamSelfLocatorSample::flag,sigmoid(similarity * goalYAngleTrust));
  }
}



/*
*  update observation information by all flags and goalposts seen
*/
void SlamSelfLocator::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);
    }

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



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

void SlamSelfLocator::execute()
{
  NDECLARE_DEBUGDRAWING ("SelfLocator:crossings", "drawingOnField", "Draws detected crossings and matching crossings on field");
  NDECLARE_DEBUGDRAWING ("SelfLocator:particles", "drawingOnField", "Draws particles on field");
  NDECLARE_DEBUGDRAWING ("SelfLocator:robotPose", "drawingOnField", "Draws robotPose on field");

  //for debug purposes
  DEBUG_RESPONSE ("Selflocator:ResetParticles",
  for(int i = 0; i < SAMPLES_MAX; i++)
    (Pose2D&) sampleSet[i] = field.randomPose();
  );

  /*
  *
  */
  switch (slamPercept.slamState)
  {
  case SlamPercept::LearnNewFlags:
  case SlamPercept::LocalizeWithFlags:
    {
      SlamSelfLocator::translationNoise = 150;

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

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

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

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

      SlamSelfLocator::centerCircleWeight = 0,// 9.745,
      SlamSelfLocator::centerCircleZAngleTrust = 
      SlamSelfLocator::centerCircleWeight / ( SlamSelfLocator::centerCircleZAngleVariance + SlamSelfLocator::quasiZero ),
      SlamSelfLocator::centerCircleYAngleTrust =
      SlamSelfLocator::centerCircleWeight / ( SlamSelfLocator::centerCircleYAngleVariance + SlamSelfLocator::quasiZero ),
      SlamSelfLocator::centerCircleOrientationAngleTrust =
      SlamSelfLocator::centerCircleWeight / ( SlamSelfLocator::centerCircleOrientationAngleVariance + SlamSelfLocator::quasiZero );
      // OUTPUT(idText,text,"all flags config");
      break;
    };
  case SlamPercept::UseNewFlags:
    {
      SlamSelfLocator::translationNoise = 50;

      SlamSelfLocator::linePointWeight = 12,//15,
      SlamSelfLocator::linePointZAngleTrust = 
      SlamSelfLocator::linePointWeight / ( SlamSelfLocator::linePointZAngleVariance + SlamSelfLocator::quasiZero ),
      SlamSelfLocator::linePointYAngleTrust = 
      SlamSelfLocator::linePointWeight / ( SlamSelfLocator::linePointYAngleVariance + SlamSelfLocator::quasiZero ),

      SlamSelfLocator::crossingWeight = 12,//15,
      SlamSelfLocator::crossingZAngleTrust = 
      SlamSelfLocator::crossingWeight / ( SlamSelfLocator::crossingZAngleVariance + SlamSelfLocator::quasiZero ),
      SlamSelfLocator::crossingYAngleTrust =
      SlamSelfLocator::crossingWeight / ( SlamSelfLocator::crossingYAngleVariance + SlamSelfLocator::quasiZero ),

      SlamSelfLocator::flagWeight = 0.0,
      SlamSelfLocator::flagZAngleTrust =
      SlamSelfLocator::flagWeight / ( SlamSelfLocator::flagZAngleVariance + SlamSelfLocator::quasiZero ),
      SlamSelfLocator::flagYAngleTrust = 
      SlamSelfLocator::flagWeight / ( SlamSelfLocator::flagYAngleVariance + SlamSelfLocator::quasiZero ),

      SlamSelfLocator::goalWeight = 0.0,
      SlamSelfLocator::goalZAngleTrust = 
      SlamSelfLocator::goalWeight / ( SlamSelfLocator::goalZAngleVariance + SlamSelfLocator::quasiZero ),
      SlamSelfLocator::goalYAngleTrust = 
      SlamSelfLocator::goalWeight / ( SlamSelfLocator::goalYAngleVariance + SlamSelfLocator::quasiZero );

      SlamSelfLocator::centerCircleWeight = 6,// 9.745,
      SlamSelfLocator::centerCircleZAngleTrust = 
      SlamSelfLocator::centerCircleWeight / ( SlamSelfLocator::centerCircleZAngleVariance + SlamSelfLocator::quasiZero ),
      SlamSelfLocator::centerCircleYAngleTrust =
      SlamSelfLocator::centerCircleWeight / ( SlamSelfLocator::centerCircleYAngleVariance + SlamSelfLocator::quasiZero ),
      SlamSelfLocator::centerCircleOrientationAngleTrust =
      SlamSelfLocator::centerCircleWeight / ( SlamSelfLocator::centerCircleOrientationAngleVariance + SlamSelfLocator::quasiZero );
      // OUTPUT(idText,text,"no landmarks config");
      break;
    };
  };

  teamColorBlue = (getPlayer().getTeamColor() == Player::blue);

  // slam challenge:
  // we will try to evaluate our Position against the mirror-symmetric
  if(slamPercept.slLearnMode == SlamPercept::evaluatePosition)
  {
    // check for all test-points, which one is nearest
    int i;
    double minDistance = 99999;
    Vector2<double> bestFieldPos;
    for(i=0;i<4;++i)
    {
      Vector3<double> diff = slamPercept.currentMeasurement - slamPercept.testPoints[i].measurement;
      if (minDistance>diff.abs())
      {
        minDistance = diff.abs();
        bestFieldPos = slamPercept.testPoints[i].fieldPos;
      }
    }

    // check if mine or the mirror-symmetric position is best
    Pose2D pose1(robotPose);
    Pose2D pose2(robotPose);
    pose2.translation.x = -pose2.translation.x;
    pose2.translation.y = -pose2.translation.y;
    pose2.rotation += pi;
    while(pose2.rotation<2*pi)
    {
      pose2.rotation -= 2*pi;
    }

    // if robot pose is nearer to the desired pose than the wrong,
    Vector2<double> diff1 = pose1.translation - bestFieldPos;
    Vector2<double> diff2 = pose2.translation - bestFieldPos;
    Vector2<double> removeTarget;
    if (diff1.abs()>diff2.abs())
    {
      removeTarget = diff1;
    } else {
      removeTarget = diff2;
    }

    // switch to direction known and always teleport symmetric particles
    double targetRadius;
    Vector2<double> targetDiff = pose1.translation - pose2.translation;
    if (targetDiff.abs()/2<300)
      targetRadius = targetDiff.abs() / 2;
    else
      targetRadius = 300;

    int k;
    for(k=0;k<SAMPLES_MAX;++k)
    {
      SlamSelfLocatorSample& s = sampleSet[k];
      Vector2<double> testDiff = s.translation - removeTarget;
        if (testDiff.abs()<targetRadius)
        {
          // teleport
          s.translation = -s.translation;
          s.rotation += pi;
          while(s.rotation>2*pi)
            s.rotation -= 2*pi;
        }
    }
/* during MERGE
  robotPoseCollection.hasDirectionKnowledge = true;
*/
  }

/*
  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);
    
    if (slamPercept.slamState == SlamPercept::LearnNewFlags || slamPercept.slamState == SlamPercept::LocalizeWithFlags)
    {
      // OUTPUT(idText,text,"updating by landmarks");
      landmarksObservationUpdate(landmarksPercept);
    }    
    
    if (observationUpdateDone)
    {
      // correct Position for slam challenge
      if (slamPercept.slamState == SlamPercept::LocalizeUsingNewFlags || slamPercept.slamState == SlamPercept::UseNewFlags)
      {
        // OUTPUT(idText,text,"updating by estimated direction");
        sampleTemplateGenerator.generateTemplates(landmarksPercept,linesPercept,odometryDelta,numberOfTypes,types);
        updateByEstimatedDirection();
      } else 
        sampleTemplateGenerator.generateTemplates(landmarksPercept,linesPercept,odometryDelta,numberOfTypes,types,true);
      resample();
    }
  }
  // taken from gt04..
  
  Pose2D pose;
  double validity;
  calcPose(pose,validity);
//  robotPose.setPose(pose);
//  robotPose.setValidity(validity);

  // OUTPUT(idText,text,validity);

  DEBUG_RESPONSE_NOT("SelfLocator:odometry only",
      odometryPoseResetted = false;
      robotPose.setPose(pose);
      robotPose.setValidity(validity);
      );
  DEBUG_RESPONSE("SelfLocator:odometry only",
      if (!odometryPoseResetted)
      {
        odometryPose = pose;
        odometryPoseResetted = true;
      } else {
        odometryPose += odometryDelta;
      }
      robotPose.setPose(odometryPose);
      robotPose.setValidity(0.5);
      );

      NARROW ("SelfLocator:robotPose", robotPose.getPose ().translation.x, robotPose.getPose ().translation.x, robotPose.getPose().getCos () * 100, robotPose.getPose().getSin () * 100, 1, Drawings::ps_solid, Drawings::red);

/* during MERGE
  switch(mshSelfLocatorRequest.selfLocatorMode)
  {
    case MSHSelfLocatorRequest::allPercepts:
      odometryPoseResetted = false;
      robotPose.setPose(pose);
      robotPose.setValidity(validity);
      break;

    case MSHSelfLocatorRequest::odometryOnly:
      if (!odometryPoseResetted)
      {
        odometryPose = pose;
        odometryPoseResetted = true;
      } else {
        odometryPose += odometryDelta;
      }
      robotPose.setPose(odometryPose);
      robotPose.setValidity(0.5);
      break;
  }
*/

  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 SlamSelfLocator::calcAveragePerceptTypeProbabilities()
{
  int count[SlamSelfLocatorSample::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<SlamSelfLocatorSample::numberOfPerceptTypes; k++){
    count[k] = 0;
    averagePerceptTypeProb[k] = 0;
  }

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

  for (i=0;i<SlamSelfLocatorSample::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!=SlamSelfLocatorSample::lineCrossing &&*/ i!=SlamSelfLocatorSample::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 SlamSelfLocator::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
  SlamSelfLocatorSample* 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++)
  {
    SlamSelfLocatorSample& 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;
      NCOMPLEX_DRAWING("SelfLocator:particles",draw(s,Drawings::yellow));
      COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::yellow));
    }
  
  /*  
    if(random() * averageProbability > s.getProbability()){
      s = SlamSelfLocatorSample(field.randomPose());
      NCOMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::red));
    }*/
    else
    {
      NCOMPLEX_DRAWING("SelfLocator:particles",draw(s,Drawings::black));
      COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::black));
    }
  }
}

double SlamSelfLocator::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);
  */
  double varianceCovarianceMatrix[2][2];
  double averageXDeviation = 0;
  double averageYDeviation = 0;
  double averageXYDeviation = 0;
  double averageRotDeviation = 0;

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


  averageXDeviation /= SAMPLES_MAX;
  averageYDeviation /= SAMPLES_MAX;
  averageXYDeviation /= SAMPLES_MAX;
  averageRotDeviation /= SAMPLES_MAX;
  // OUTPUT(idText,text,"mshiii: averagex : " << sqrt(averageXDeviation));

  varianceCovarianceMatrix[0][0] = averageXDeviation;
  varianceCovarianceMatrix[1][1] = averageYDeviation;

  varianceCovarianceMatrix[0][1] = averageXYDeviation;
  varianceCovarianceMatrix[1][0] = averageXYDeviation;

  // calculate the eigenvalues
  double p = varianceCovarianceMatrix[0][0] + varianceCovarianceMatrix[1][1];
  double q = varianceCovarianceMatrix[0][1]*varianceCovarianceMatrix[1][0] - varianceCovarianceMatrix[0][0]*varianceCovarianceMatrix[1][1];

  double t = p*p/4 - q;
  double x1 = t>=0 ? -p/2 + sqrt(t) : 0;
  double x2 = t>=0 ? -p/2 - sqrt(t) : 0;


  // OUTPUT(idText,text,"mshiii: x1 " << sqrt(fabs(x1)) << " | x2 " << sqrt(fabs(x2)));
  // 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));
  
  // scale distance by standard dev
//  double s = sqrt(fmax(fabs(x1),fabs(x2)));

/* during MERGE
  robotPoseCollection.positionVariance = s;
*/
  // OUTPUT(idText,text,s << "   " << sigmoid( sqrt(fmax(x1,x2))/200 ));
  return sigmoid( sqrt(fmax(x1,x2))/200 );
}


RobotPose SlamSelfLocator::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(SlamSelfLocatorSample* 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 SlamSelfLocator::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++)
  {
    SlamSelfLocatorSample& 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]);
        NCIRCLE("SelfLocator:particles",candidates[i].translation.x,candidates[i].translation.y,50,50,Drawings::ps_solid,Drawings::yellowOrange);
        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,"mshII "<< diff.abs() << " " << validity);
}

bool SlamSelfLocator::handleMessage(InMessage& message)
{/*
  switch(message.getMessageID())
  {
    case idSlamSelfLocator:
    {
      GenericDebugData d;
      message.bin >> d;
      SlamSelfLocatorSample::paramProbUpLimit   = d.data[0];
      SlamSelfLocatorSample::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 SlamSelfLocator::draw(const Pose2D& pose,Drawings::Color color) const
{ 
  Pose2D p = pose + Pose2D(-100,0);
  NLINE("SelfLocator:particles",
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  LINE(selfLocatorField,
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  p = pose + Pose2D(-40,-40);
  NLINE("SelfLocator:particles",
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  LINE(selfLocatorField,
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  p = pose + Pose2D(-40,40);
  NLINE("SelfLocator:particles",
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  LINE(selfLocatorField,
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
}


void SlamSelfLocator::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]);
}


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

---