Modules/SelfLocator/GT2005SelfLocator/GT2005SelfLocator.cpp

Go to the documentation of this file.

#include "GT2005SelfLocator.h"
#include "Tools/Math/Common.h"
  
#ifdef _WIN32
#pragma warning(disable:4355) // VC++: "this" in initializer list is ok
#endif

/*
  Particle-filter based Self-Locator based on the GT2004 SelfLocator


  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 methods
      -sample.setPerceptProbability replaces setProbability
        -enum PerceptType to avoid strange LineType enum usage for setProbability
      -usage of line crossings percepts
      -middle circle as flag
      -possible crash fixed: no flag update for particle position inside a flag (MR)
      -adjustable parameters moved into separate class (MR)
      -added percept and odometry update probabilities (MR)

    @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
    @author <A href=mailto:c_rohde@web.de>Carsten Rohde</A>
    @author <A href=mailto:judith-winter@web.de>Judith Winter</A>
    @author Max Risler
*/

const double GT2005SelfLocator::QUASI_ZERO = 0.000001;

/*---------------------------- class GT2005SelfLocator methods ------------------------------*/


GT2005SelfLocator::GT2005SelfLocator(const SelfLocatorInterfaces& interfaces)
: SelfLocator(interfaces), sampleTemplateGenerator(parameters)
{
  timeStamp = 0;
  testSample = 0;
  testSampleIndex = 0;
  speed = 0;

  resetSamples();

  timeStamp = 0;

  calculateTrust();

  OUTPUT(idText, text, "GT2005 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 GT2005SelfLocator::motionUpdate(const Pose2D& odometry, const Pose2D& camera, bool noise)
{
  double transNoise = noise ? parameters.translationNoise : 0;
  double rotNoise   = noise ? parameters.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   = max(dist*parameters.movedDistWeight,    
                           angle*parameters.movedAngleWeight);  

  /* precalculate translational error that has to be adapted to all samples */
  double transXError = max( fabs(transX*parameters.majorDirTransWeight),
                             fabs(transY*parameters.minorDirTransWeight));  
  double transYError = max( fabs(transY*parameters.majorDirTransWeight),
                             fabs(transX*parameters.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++)
  {
    GT2005SelfLocatorSample& s = sampleSet[i];

    if (s.isUsingOdometry)
      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) * max(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;               
  }
    
}

// for the moment this is just inserted into the line crossings percept value
void GT2005SelfLocator::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(parameters.headHeightEstimation,pointDist);
  // seen angle to the linepoint in y-direction 
  double observedYAngle = atan2((double)point.y,(double)point.x);

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

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

  // update all samples..
  for(int i = 0; i < SAMPLES_MAX; ++i)
    if (random() < parameters.updateProbCenterCircle)
    {
      GT2005SelfLocatorSample& s = sampleSet[i];
      // the absolute position of the point if it was seen from the samples position
      Pose2D pointFromSample = s + lastSeenCenterCircle; 
      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(parameters.headHeightEstimation,v.abs()),
            yModel = atan2(v.y,v.x);

      double orientationDiff;
      if (centerCircle.orientationKnown)
      {
        double orientation = fabs(normalize(pointFromSample.rotation));
        orientationDiff = pi_2 - orientation;
      }
      else
        orientationDiff = 0;

      double val = gaussian((zModel - observedZAngle) * centerCircleZAngleTrust) * 
                  gaussian((yModel - observedYAngle) * centerCircleYAngleTrust) *
                  gaussian(orientationDiff * centerCircleOrientationAngleTrust);
      // 
      
      s.setPerceptProbability(GT2005SelfLocatorSample::lineCrossing, val, parameters);                              
      if (i==testSampleIndex)
      {
        Drawings::Color color = getSampleColor(val);
        NCIRCLE("gt05-sl:testSample", 0, 0, 40, 1, Drawings::ps_solid, color);
        NCIRCLE("gt05-sl:testSample", pointFromSample.translation.x, pointFromSample.translation.y, 40, 1, Drawings::ps_solid, color);
        NLINE("gt05-sl:testSample", 0, 0, pointFromSample.translation.x, pointFromSample.translation.y, 1, Drawings::ps_solid, color);
      }
    }
}

void GT2005SelfLocator::updateByCrossing(const LinesPercept::LineCrossingPoint& crossingPoint)
{
  // COMPLEX_DRAWING(selfLocator,draw(point,type));  
  Vector2<int> point = crossingPoint.locationOnField;

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

  // crossing filtering
  Vector2<double> testCrossing = lineCrossingsTable.getClosestPoint(pointFromPose.translation, GT2005LineCrossingsTable::falseCrossing, 0);
  Vector2<double> testDiff = testCrossing - pointFromPose.translation;
  if (testDiff.abs()<300)
  {
    NCIRCLE("gt05-sl:lastCrossing",pointFromPose.translation.x,pointFromPose.translation.y,40,40,Drawings::ps_solid,Drawings::yellow);
    return;
  }

  //debug stuff
  Vector2<double> vMin;
  vMin = lineCrossingsTable.getClosestPoint(pointFromPose, crossingPoint, lastSeenCrossingClass);
  lastSeenCrossing = pointFromPose.translation;
  if(vMin.x != GT2005LineCrossingsTable::NO_POINT_DISTANCE)
  {
    lastModelCrossing = vMin;
  }
  else
  {
    lastModelCrossing = pointFromPose.translation;
    // If a crossing cannot be matched it must be a wrong percept since all possible correct crossings
    // are included in the tables. Therefore we do not update with this crossing.
    return;
  }

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

  // update all samples..
  for(int i = 0; i < SAMPLES_MAX; ++i)
    if (random() < parameters.updateProbCrossing)
    {
      GT2005SelfLocatorSample& 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;
      GT2005LineCrossingsTable::CrossingClass crossingClass;
      vMin = lineCrossingsTable.getClosestPoint(pointFromSample, crossingPoint, crossingClass);

      double val;
      // if we got a closest point..
      if(vMin.x != GT2005LineCrossingsTable::NO_POINT_DISTANCE)
      {
        // 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(parameters.headHeightEstimation,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)GT2005SelfLocatorSample::perceptTypeYFieldLine : type, 
                                gaussian(normedSimilarity*7));
                                */

        val = gaussian((zModel - observedZAngle) * crossingZAngleTrust) * 
                    gaussian((yModel - observedYAngle) * crossingYAngleTrust);
        // if (val>0.3) OUTPUT(idText,text,"crossings value" << val);
        s.setPerceptProbability(GT2005SelfLocatorSample::lineCrossing, val, parameters);
        if (i==testSampleIndex)
        {
          Drawings::Color color = getSampleColor(val);
          NCIRCLE("gt05-sl:testSample", vMin.x, vMin.y, 40, 1, Drawings::ps_solid, color);
          NCIRCLE("gt05-sl:testSample", pointFromSample.translation.x, pointFromSample.translation.y, 40, 1, Drawings::ps_solid, color);
          NLINE("gt05-sl:testSample", vMin.x, vMin.y, pointFromSample.translation.x, pointFromSample.translation.y, 1, Drawings::ps_solid, color);
        }
      }
    }

}

// TODO: document, rewrite
void GT2005SelfLocator::updateByPoint(const LinesPercept::LinePoint& point,GT2005SelfLocatorSample::PerceptType type, LinesPercept::LineType observationTableIndex)
{
  // COMPLEX_DRAWING(selfLocator,draw(point,type));  

  double pointDist = point.abs();

         // seen angle to the linepoint in z-direction (height) assuming the robots height is paramHeight
  double observedZAngle = atan2(parameters.headHeightEstimation,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)
  {
    GT2005SelfLocatorSample& 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 = (type == GT2005SelfLocatorSample::xFieldLine || type==GT2005SelfLocatorSample::yFieldLine) && 
              (fabs(pointFromSample.getAngle()) < pi / 4 || 
               fabs(pointFromSample.getAngle()) > 3 * pi / 4);
    // we find the closest point in the corresponding observationTable
    vMin = observationTable[observationTableIndex].getClosestPoint(pointFromSample);

    // 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(parameters.headHeightEstimation,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)GT2005SelfLocatorSample::perceptTypeYFieldLine : type, 
                              gaussian(normedSimilarity*7));
                              */


      double val = gaussian((zModel - observedZAngle) * linePointZAngleTrust) * 
                   gaussian((yModel - observedYAngle) * linePointYAngleTrust);
      s.setPerceptProbability(isY ? GT2005SelfLocatorSample::yFieldLine : type, val, parameters);
      if (i==testSampleIndex)
      {
        Drawings::Color color = getSampleColor(val);
        NCIRCLE("gt05-sl:testSample", vMin.x, vMin.y, 40, 1, Drawings::ps_solid, color);
        NCIRCLE("gt05-sl:testSample", pointFromSample.translation.x, pointFromSample.translation.y, 40, 1, Drawings::ps_solid, color);
        NLINE("gt05-sl:testSample", vMin.x, vMin.y, pointFromSample.translation.x, pointFromSample.translation.y, 1, Drawings::ps_solid, color);
      }
    }
    else
    {
      s.setPerceptProbability(isY ? GT2005SelfLocatorSample::yFieldLine : type, QUASI_ZERO, parameters);
      if (i==testSampleIndex)
      {
        Drawings::Color color = getSampleColor(0);
        NCIRCLE("gt05-sl:testSample", pointFromSample.translation.x, pointFromSample.translation.y, 40, 1, Drawings::ps_solid, color);
      }
    }
  }
}


void GT2005SelfLocator::lineObservationUpdate(const LinesPercept& linesPercept)
{
    int i;
   
    // updating by center circle
    if (linesPercept.centerCircleFound && parameters.centerCircleWeight > 0){
      updateByCenterCircle(linesPercept.centerCircle);
    }
    

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

    for(int j = 0; j < linesPercept.points[LinesPercept::border].numberOfPoints && j < parameters.maxBorderpoints; ++j)
    {
      updateByPoint(linesPercept.points[LinesPercept::border].pointsOnField[rand() % linesPercept.points[LinesPercept::border].numberOfPoints],
        GT2005SelfLocatorSample::border, 
        LinesPercept::border
      );
      observationUpdateDone = true;
    }

    for(int j = 0; j < linesPercept.points[LinesPercept::field].numberOfPoints && j < parameters.maxLinepoints; ++j)
    {
      updateByPoint(linesPercept.points[LinesPercept::field].pointsOnField[rand() % linesPercept.points[LinesPercept::field].numberOfPoints],
        GT2005SelfLocatorSample::xFieldLine, 
        LinesPercept::field
      );
      observationUpdateDone = true;
    }
        
    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 < parameters.maxGoalpoints; ++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;
        LinesPercept::LineType observationTableIndex;
        if(getPlayer().getTeamColor() == Player::blue)
          // switch observation table index for blue as tables are calculated for red players only
          observationTableIndex = (select == LinesPercept::yellowGoal) ? LinesPercept::skyblueGoal : LinesPercept::yellowGoal;
        else
          observationTableIndex = select;
        updateByPoint(linesPercept.points[select].pointsOnField[rand() % linesPercept.points[select].numberOfPoints],
          GT2005SelfLocatorSample::goal,
          observationTableIndex
        );
        observationUpdateDone = true;
      }   
}


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

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

      // if particle position is inside a flag similarity cannot be determined
      if (flagPos.translation.abs() > flagRadius)
      {
        // 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...
        double val = gaussian(similarity * flagYAngleTrust);
        s.setPerceptProbability(GT2005SelfLocatorSample::flag, val, parameters);
        if (i==testSampleIndex)
        {
          Drawings::Color color = getSampleColor(val);
          NCIRCLE("gt05-sl:testSample", flagFieldPosition.x, flagFieldPosition.y, 40, 1, Drawings::ps_solid, color);
        }
      }
      else
      {
        s.setPerceptProbability(GT2005SelfLocatorSample::flag, QUASI_ZERO, parameters);
        if (i==testSampleIndex)
        {
          Drawings::Color color = getSampleColor(0);
          NCIRCLE("gt05-sl:testSample", flagFieldPosition.x, flagFieldPosition.y, 40, 1, Drawings::ps_solid, color);
        }
      }
    }
}


// look at updateByFlag
// goalPost is a Vector pointing on the used goalpost
// TODO: maybe introduce new PerceptType GoalPost instead of using "Flag"
void GT2005SelfLocator::updateByGoalPost(const Vector2<double>& goalPost,
                                         double measuredBearing)
{ 
  observationUpdateDone = true;
  int i;
  for(i = 0; i < SAMPLES_MAX; ++i)
    if (random() < parameters.updateProbGoalPost)
    {
      GT2005SelfLocatorSample& 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;

      double val = gaussian(similarity * goalYAngleTrust);
      s.setPerceptProbability(GT2005SelfLocatorSample::flag, val, parameters);
      if (i==testSampleIndex)
      {
        Drawings::Color color = getSampleColor(val);
        NCIRCLE("gt05-sl:testSample", goalPost.x, goalPost.y, 40, 1, Drawings::ps_solid, color);
      }
    }
}



/*
*  update observation information by all flags and goalposts seen
*/
void GT2005SelfLocator::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 GT2005SelfLocator::updateVariancesBySpeed(double speed)
{
  linePointZAngleMotionDependentVariance = parameters.linePointZAngleVariance * (1 + speed * parameters.linePointZAngleMotionDependency);
  linePointZAngleTrust = parameters.linePointWeight / (linePointZAngleMotionDependentVariance + QUASI_ZERO );

}

void GT2005SelfLocator::calculateTrust()
{
  linePointZAngleTrust = 
    parameters.linePointWeight / ( parameters.linePointZAngleVariance + QUASI_ZERO ),
  linePointYAngleTrust = 
    parameters.linePointWeight / ( parameters.linePointYAngleVariance + QUASI_ZERO ),

  crossingZAngleTrust = 
    parameters.crossingWeight / ( parameters.crossingZAngleVariance + QUASI_ZERO ),
  crossingYAngleTrust =
    parameters.crossingWeight / ( parameters.crossingYAngleVariance + QUASI_ZERO ),

  centerCircleZAngleTrust = 
    parameters.centerCircleWeight / ( parameters.centerCircleZAngleVariance + QUASI_ZERO ),
  centerCircleYAngleTrust =
    parameters.centerCircleWeight / ( parameters.centerCircleYAngleVariance + QUASI_ZERO ),
  centerCircleOrientationAngleTrust =
    parameters.centerCircleWeight / ( parameters.centerCircleOrientationAngleVariance + QUASI_ZERO ),


  //flagZAngleTrust =
  //  parameters.flagWeight / ( parameters.flagZAngleVariance + QUASI_ZERO ),
  flagYAngleTrust = 
    parameters.flagWeight / ( parameters.flagYAngleVariance + QUASI_ZERO ),

  goalZAngleTrust = 
    parameters.goalWeight / ( parameters.goalZAngleVariance + QUASI_ZERO ),
  goalYAngleTrust = 
    parameters.goalWeight / ( parameters.goalYAngleVariance + QUASI_ZERO );
}

void GT2005SelfLocator::execute()
{
  NDECLARE_DEBUGDRAWING( "gt05-sl:sampleSet" , "drawingOnField" , "shows the current sample set");
  NDECLARE_DEBUGDRAWING( "gt05-sl:templates" , "drawingOnField" , "shows the current template positions for new samples");
  NDECLARE_DEBUGDRAWING( "gt05-sl:flags" , "drawingOnField" , "shows the currently buffered flags for template calculation");
  NDECLARE_DEBUGDRAWING( "gt05-sl:testSample" , "drawingOnField" , "shows the currently selected test sample");
  NDECLARE_DEBUGDRAWING( "gt05-sl:lastSeenCenterCircle", "drawingOnField", "shows the last seen center circle");
  NDECLARE_DEBUGDRAWING( "gt05-sl:lastCrossing", "drawingOnField", "shows the last seen line crossing");
  NDECLARE_DEBUGDRAWING( "gt05-sl:covarianceEllipse", "drawingOnField", "shows the ellipse describing the sample distribution covariance matrix");

  MODIFY( "gt05-sl:parameters", parameters );

  DEBUG_RESPONSE( "gt05-sl:recalculate trust values",
    calculateTrust();
  );

  DEBUG_RESPONSE( "gt05-sl:resetSamples",
    resetSamples();
  );

  DEBUG_RESPONSE( "gt05-sl:resetSamplesWhenPenalized",
    resetSamplesWhenPenalized();
  );
  
  DEBUG_RESPONSE( "gt05-sl:resetSamplesToZero",
    resetSamples(Pose2D(0, Vector2<double>(0,0)));
  );

  DEBUG_RESPONSE( "gt05-sl:resetSamplesToCurrentRobotPose",
    resetSamples(robotPose);
  );

  MODIFY( "gt05-sl:testSampleIndex", testSampleIndex);

  testSample = &sampleSet[testSampleIndex];

  MODIFY( "gt05-sl:testSample", *testSample);


  if (parameters.resetSamplesWhenPenalized && gameControlData.getRobot().penalty != GameControlData::notPenalized)
  {
    // robot is penalized
    resetSamplesWhenPenalized();
    return;
  }

  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());

  
  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 || parameters.noiseWithoutObservationUpdate);

  // age sample probabilities
  for(int i = 0; i < SAMPLES_MAX; ++i)
    sampleSet[i].scaleProbabilities(parameters.agingFactor);

  observationUpdateDone = false;

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

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

  // OUTPUT(idText,text,validity);

  draw();

  sampleSet.link(selfLocatorSamples);

  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, 1.0 else
// returns the product over all average percepttype probabilities
 
double GT2005SelfLocator::calcAveragePerceptTypeProbabilities()
{
  int count[GT2005SelfLocatorSample::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<GT2005SelfLocatorSample::numberOfPerceptTypes; k++){
    count[k] = 0;
    averagePerceptTypeProb[k] = 0;
  }

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

  for (i=0;i<GT2005SelfLocatorSample::numberOfPerceptTypes;i++)
  {
    if (count[i]>0) averagePerceptTypeProb[i] /= count[i];
    else averagePerceptTypeProb[i] = 1.0;
    // OUTPUT(idText,text,"avg type "<< i << "  "<<averagePerceptTypeProb[i]);
    //if (/*i!=GT2005SelfLocatorSample::lineCrossing &&*/) 
    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 GT2005SelfLocator::resample()
{
  // this fills averagePerceptTypeProb[] and returns the whole averageProbability
  double averageProbability = 
    calcAveragePerceptTypeProbabilities();
  // OUTPUT(idText,text,averageProbability);

  //MODIFY( "gt05-sl:average percept probabilities", averagePerceptTypeProb);
  
  // calculate probability for all samples from their perceptType probabilities
  int i;
  for(i = 0; i < SAMPLES_MAX; ++i){
    sampleSet[i].updateProbability(averagePerceptTypeProb, parameters);
    // sampleSet[i].updateProbability();
  }

  // if (testSample!=0) OUTPUT(idText,text,"last Samples prob: "<< testSample->probability << " average = " << averageProbability);
  // swap sample arrays
  GT2005SelfLocatorSample* 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;
  int resamplingSize;
  if (parameters.applyConstantResampling)
  {
    prob2Index = (double) SAMPLES_MAX * (1.0 - parameters.constantResamplingRate) / probSum;
    resamplingSize = (int)( (double)SAMPLES_MAX * (1.0 - parameters.constantResamplingRate) );
  }
  else
  {
    prob2Index = (double) SAMPLES_MAX / probSum;
    resamplingSize = SAMPLES_MAX;
  }

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

  testSampleIndex = -1;

  // 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 < resamplingSize && j < indexSum - 0.5)
    {
      sampleSet[j++] = oldSet[i];
      if (testSampleIndex == -1 && testSample == &oldSet[i]) 
      {
        testSample = &sampleSet[j-1];
        testSampleIndex = j-1;
      }
      // reset odometry flag for further copies of the sample
      oldSet[i].isUsingOdometry = (random() < (collisionPercept.getCollisionAggregate() ? parameters.odometryProbCollision : parameters.odometryProbNoCollision));
    }
  }

  if (parameters.applyConstantResampling)
  {
    // fill up remaining samples with new poses
    for(; j < SAMPLES_MAX; ++j)
    {
      sampleSet[j] = sampleTemplateGenerator.getTemplate();
      sampleSet[j].isUsingOdometry = (random() < (collisionPercept.getCollisionAggregate() ? parameters.odometryProbCollision : parameters.odometryProbNoCollision));
      if (testSampleIndex == -1) 
      {
        testSample = &sampleSet[j];
        testSampleIndex = j;
      }
    }
  }
  else
  {
    // determine which samples we will replace randomly by 
    // template samples
    for(i = 0; i < SAMPLES_MAX; i++)
    {
      GT2005SelfLocatorSample& 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();
        s.isUsingOdometry = (random() < (collisionPercept.getCollisionAggregate() ? parameters.odometryProbCollision : parameters.odometryProbNoCollision));
        if (testSampleIndex == -1) 
        {
          testSample = &s;
          testSampleIndex = i;
        }
      }
      if(random() * averageProbability > s.getProbability())
      {
        s = GT2005SelfLocatorSample(field.randomPose());
        s.isUsingOdometry = (random() < (collisionPercept.getCollisionAggregate() ? parameters.odometryProbCollision : parameters.odometryProbNoCollision));
        if (testSampleIndex == -1) 
        {
          testSample = &s;
          testSampleIndex = i;
        }
      }
    }
    if (testSampleIndex == -1) 
    {
      testSample = &sampleSet[0];
      testSampleIndex = 0;
    }
  }
}



double GT2005SelfLocator::calcDistributionValidityByStandardDeviation()
{ //data reset starts
  averageX = 0;
  averageY = 0;
  averageRot = 0;
  stdDevRot = 0;
  
  //double eigenVal0, eigenVal1, eigenSwap;
  eigenVal0 = eigenVal1 = eigenSwap = 0;
  eigenVec0 = Vector2<double>(0,0);
  eigenVec1 = Vector2<double>(0,0);

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

  int i = 0;

  Vector2<double> angle(0,0);
  double pSum = 0;
  //data reset ends

  for (i=0;i<SAMPLES_MAX;i++)
  {
    if(sampleSet[i].isValid())
    {
      averageX += sampleSet[i].translation.x * sampleSet[i].getProbability();
      averageY += sampleSet[i].translation.y * sampleSet[i].getProbability();
      angle.x += cos(sampleSet[i].rotation) * sampleSet[i].getProbability();
      angle.y += sin(sampleSet[i].rotation) * sampleSet[i].getProbability();
      pSum += sampleSet[i].getProbability();
    }
  }

  //calculate the average particle values
  averageX /= pSum;
  averageY /= pSum;
  averageRot = atan2(angle.y,angle.x);

  // Covariance calculation

  for (i = 0; i < SAMPLES_MAX; i++)
  {
    if(sampleSet[i].isValid())
    {
      double xErr = (sampleSet[i].translation.x - averageX);
      double yErr = (sampleSet[i].translation.y - averageY);
      double rErr = normalize(sampleSet[i].rotation - averageRot);
      varianceCovarianceMatrix[0][0] += sampleSet[i].getProbability() * xErr * xErr;
      varianceCovarianceMatrix[0][1] += sampleSet[i].getProbability() * xErr * yErr;
      varianceCovarianceMatrix[1][1] += sampleSet[i].getProbability() * yErr * yErr;
      stdDevRot += sampleSet[i].getProbability() * rErr * rErr;
    }
  }
  
  varianceCovarianceMatrix[0][0] /= pSum;
  varianceCovarianceMatrix[0][1] /= pSum;
  varianceCovarianceMatrix[1][1] /= pSum;
  stdDevRot /= pSum;

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

  // eigenvalues
  double p_2,q;
  p_2 = (varianceCovarianceMatrix[0][0] + varianceCovarianceMatrix[1][1])/2;
  q = varianceCovarianceMatrix[0][0] * varianceCovarianceMatrix[1][1] - 
      varianceCovarianceMatrix[0][1] * varianceCovarianceMatrix[0][1];

  eigenVal0 = p_2 + sqrt(fabs(p_2*p_2 - q));
  eigenVal1 = p_2 - sqrt(fabs(p_2*p_2 - q));

  //eigenVal0 must be bigger than eigenVal1
  if (eigenVal1 > eigenVal0)      
  {
    eigenSwap = eigenVal0;
    eigenVal0 = eigenVal1;
    eigenVal1 = eigenSwap;
  }

  // first eigenvector  
  double amount;  //vector amount
 
  eigenVec0.y = ((varianceCovarianceMatrix[0][0] - eigenVal0) * varianceCovarianceMatrix[0][1]) /
                 (-varianceCovarianceMatrix[1][1] + eigenVal0 + varianceCovarianceMatrix[0][1]*varianceCovarianceMatrix[0][1]);
  eigenVec0.x = (-varianceCovarianceMatrix[1][1] + eigenVal0) * eigenVec0.y / varianceCovarianceMatrix[0][1];

  amount = sqrt(eigenVec0.x*eigenVec0.x + eigenVec0.y*eigenVec0.y);
  
  //normalize
  if (amount != 0)
  {
    eigenVec0.x  /= amount;
    eigenVec0.y  /= amount;
  }

  //calculate the 2nd eigenvector
  eigenVec1.y = ((varianceCovarianceMatrix[0][0] - eigenVal1) * varianceCovarianceMatrix[0][1]) /
                 (-varianceCovarianceMatrix[1][1] + eigenVal1 + varianceCovarianceMatrix[0][1]*varianceCovarianceMatrix[0][1]);
  eigenVec1.x = (-varianceCovarianceMatrix[1][1] + eigenVal1) * eigenVec1.y / varianceCovarianceMatrix[0][1];

  amount = sqrt(eigenVec1.x*eigenVec1.x + eigenVec1.y*eigenVec1.y);

  //normalize
  if (amount != 0)
  {
    eigenVec1.x  /= amount;
    eigenVec1.y  /= amount;
  }
  
  //both eigenvectors should have positive x-coordinates
  if (eigenVec0.x < 0) 
  {
    eigenVec0 *= -1;
  }

  if (eigenVec1.x < 0) 
  {
    eigenVec1 *= -1;
  }

  eigenVal0 = sqrt(eigenVal0); //eigenVal is now the new std-deviation for position
  eigenVal1 = sqrt(eigenVal1);
  stdDevRot = sqrt(stdDevRot);

  eigenVec0 *= eigenVal0;
  eigenVec1 *= eigenVal1;
  double validity = max(eigenVal0,eigenVal1);
  robotPose.setPositionVariance(validity*validity);

  robotPose.directionOfGreatestUncertaintyExists = (eigenVal0 >= 1.7 * eigenVal1);
  robotPose.greatestUncertainty.translation.x = averageX;
  robotPose.greatestUncertainty.translation.y = averageY;
  robotPose.greatestUncertainty.rotation = atan2(eigenVec0.y, eigenVec0.x);
  
  validity = gaussian(validity/500);
  validity = min(validity,gaussian(stdDevRot/0.5));  //validity is min of position and angle validity
  DEBUG_RESPONSE("gt05-sl:display eigenvalues", OUTPUT(idText,text,"gt05-sl " << eigenVal0 <<  "   " << eigenVal1 << "  " << validity););

  NCOMPLEX_DRAWING("gt05-sl:covarianceEllipse",
  
    NLINE("gt05-sl:covarianceEllipse",averageX,averageY,averageX+300*cos(averageRot),averageY+300*sin(averageRot),10,Drawings::ps_solid,Drawings::black);
    NLINE("gt05-sl:covarianceEllipse",averageX,
      averageY,
      averageX+600*cos(averageRot+stdDevRot),
      averageY+600*sin(averageRot+stdDevRot),
      10,Drawings::ps_solid,Drawings::red);
    NLINE("gt05-sl:covarianceEllipse",averageX,
      averageY,
      averageX+600*cos(averageRot-stdDevRot),
      averageY+600*sin(averageRot-stdDevRot),
      10,Drawings::ps_solid,Drawings::red);

    double alpha = atan2(eigenVec0.y,eigenVec0.x);
    double a = eigenVal0;
    double b = eigenVal1;
    double x1 = 0;
    double x2 = 0;
    double y1 = 0;
    double y2 = 0;

    int step = (int)(a/10);
    if (!step)
    {
      step = 1;
    }
       
    for (int xc = 0; (xc + step) < a; xc += step)
    {         
      x1 = xc;
      y1 = (sqrt(1-sqr(x1/a))* b);
      x2 = xc + step;
      y2 = (sqrt(1-sqr(x2/a))* b);


      NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(x1) + averageX - sin(alpha)*(y1), 
        sin(alpha)*(x1) + cos(alpha)*(y1) + averageY, 
        cos(alpha)*(x2) + averageX - sin(alpha)*(y2), 
        sin(alpha)*(x2) + cos(alpha)*(y2) + averageY, 
        4, Drawings::ps_solid,
        Drawings::red);
      NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(x1) + averageX - sin(alpha)*(-y1), 
        sin(alpha)*(x1) + cos(alpha)*(-y1) + averageY, 
        cos(alpha)*(x2) + averageX - sin(alpha)*(-y2), 
        sin(alpha)*(x2) + cos(alpha)*(-y2) + averageY, 
        4, Drawings::ps_solid,
        Drawings::red);
      NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(-x1) + averageX - sin(alpha)*(y1), 
        sin(alpha)*(-x1) + cos(alpha)*(y1) + averageY, 
        cos(alpha)*(-x2) + averageX - sin(alpha)*(y2), 
        sin(alpha)*(-x2) + cos(alpha)*(y2) + averageY, 
        4, Drawings::ps_solid,
        Drawings::red);
      NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(-x1) + averageX - sin(alpha)*(-y1), 
        sin(alpha)*(-x1) + cos(alpha)*(-y1) + averageY, 
        cos(alpha)*(-x2) + averageX - sin(alpha)*(-y2), 
        sin(alpha)*(-x2) + cos(alpha)*(-y2) + averageY, 
        4, Drawings::ps_solid,
        Drawings::red);
    }

    x1 = a;
    y1 = 0;

    NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(x1) + averageX - sin(alpha)*(y1), 
      sin(alpha)*(x1) + cos(alpha)*(y1) + averageY, 
      cos(alpha)*(x2) + averageX - sin(alpha)*(y2), 
      sin(alpha)*(x2) + cos(alpha)*(y2) + averageY, 
      4, Drawings::ps_solid,
      Drawings::red);
    NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(x1) + averageX - sin(alpha)*(-y1), 
      sin(alpha)*(x1) + cos(alpha)*(-y1) + averageY, 
      cos(alpha)*(x2) + averageX - sin(alpha)*(-y2), 
      sin(alpha)*(x2) + cos(alpha)*(-y2) + averageY, 
      4, Drawings::ps_solid,
      Drawings::red);
    NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(-x1) + averageX - sin(alpha)*(y1), 
      sin(alpha)*(-x1) + cos(alpha)*(y1) + averageY, 
      cos(alpha)*(-x2) + averageX - sin(alpha)*(y2), 
      sin(alpha)*(-x2) + cos(alpha)*(y2) + averageY, 
      4, Drawings::ps_solid,
      Drawings::red);
    NLINE("gt05-sl:covarianceEllipse",  cos(alpha)*(-x1) + averageX - sin(alpha)*(-y1), 
      sin(alpha)*(-x1) + cos(alpha)*(-y1) + averageY, 
      cos(alpha)*(-x2) + averageX - sin(alpha)*(-y2), 
      sin(alpha)*(-x2) + cos(alpha)*(-y2) + averageY, 
      4, Drawings::ps_solid,
      Drawings::red);
  );


  // for now, dont use the covariances
  return validity;//gaussian( sqrt(varianceCovarianceMatrix[0][0] + varianceCovarianceMatrix[1][1] + averageRotDeviation*averageRotDeviation) / 300);
  // using the maximum eigenvalue seems to work fine, too;
  //eigenvalue is sqrt(eigenvalue) now

}


double GT2005SelfLocator::calcDistributionValidityByEntropy()
{
  int gridX, gridY, gridR;
  gridX = 5;
  gridY = 5;
  gridR = 5;

  int grid[15][15][15];
  double entropy = 0;

  double x = 6000;
  double y = 3600;
  int transx = 0;
  int transy = 0;
  int rot = 0;
  int i;

  for(i = 0; i < (gridX * 3); ++i)
    for(int j = 0; j < (gridY * 3); ++j)
      for(int k = 0; k < (gridR * 3); ++k)
      {
        grid[i][j][k] = 0;
      }


  for(i = 0; i < SAMPLES_MAX; ++i)
  {
    transx = (int)((sampleSet[i].translation.x + 1.5*x) / x / 3 * gridX);
    transy = (int)((sampleSet[i].translation.y + 1.5*y) / y / 3 * gridY);
    rot = (int)((sampleSet[i].rotation + pi) / (2*pi) * gridR);

    if(transx < 0) {transx = 0;}
    if(transy < 0) {transy = 0;}
    while(rot < 0) {rot = rot + gridR;}

    if(transx > gridX) {transx = (gridX-1);}
    if(transy > gridY) {transy = (gridY-1);}
    while(rot >= gridR) {rot = (rot - gridR);}

    grid[transx][transy][rot]++;
  }

  for(i = 0; i < gridX; ++i)
    for(int j = 0; j < gridY; ++j)
      for(int k = 0; k < gridR; ++k)
      {
        if (grid[i][j][k])
        {
          entropy -= ((double)grid[i][j][k]/SAMPLES_MAX)*log((double)grid[i][j][k]/SAMPLES_MAX);
        }
      }
  entropy /= log(2.0);
    
  return  gaussian(entropy);
}

RobotPose GT2005SelfLocator::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(GT2005SelfLocatorSample* 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
    if (parameters.clipRobotPose)
    {
      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 GT2005SelfLocator::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++)
  {
    GT2005SelfLocatorSample& sample = sampleSet[i];

    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)
      {
        robotPoseCollection.poses[i] = calcPoseFromSubCube(poseSpace, xMax[i],yMax[i],rMax[i]);
        CIRCLE(selfLocatorField,
               robotPoseCollection.poses[i].translation.x,
               robotPoseCollection.poses[i].translation.y,
               50,50,Drawings::ps_solid,Drawings::yellowOrange);
      } else 
      {
        robotPoseCollection.poses[i].setValidity(-1);
      }
    }
    pose = robotPoseCollection.poses[0].getPose();
    validity = robotPoseCollection.poses[0].getValidity();

    //calc validity by covariance
    DEBUG_RESPONSE("gt05-sl:validity by covariance",
      validity = calcDistributionValidityByStandardDeviation();
    );
    //or calc validity by Entropy
    DEBUG_RESPONSE("gt05-sl:validity by entropy",
      validity = calcDistributionValidityByEntropy();
    );


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

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

bool GT2005SelfLocator::handleMessage(InMessage& message)
{/*
  switch(message.getMessageID())
  {
    case idMSHSelfLocator:
    {
      GenericDebugData d;
      message.bin >> d;
      GT2005SelfLocatorSample::paramProbUpLimit   = d.data[0];
      GT2005SelfLocatorSample::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;
}

void GT2005SelfLocator::resetSamples()
{
  for(int i = 0; i < SAMPLES_MAX; i++)
  {
    sampleSet[i].reset();
    (Pose2D&) sampleSet[i] = field.randomPose();
  }
}

void GT2005SelfLocator::resetSamples(const Pose2D &pose)
{
  for(int i = 0; i < SAMPLES_MAX; i++)
  {
    sampleSet[i].reset();
    (Pose2D&) sampleSet[i] = pose;
  }
}

void GT2005SelfLocator::resetSamplesWhenPenalized()
{
  // erase samples
  int i;
  Pose2D t;
  double randomFactor = 3;

  // place 25% samples at left center spot
  t.translation.x = 0;
  t.translation.y = yPosLeftSideline;
  t.rotation = -pi_2;
  for(i = 0; i < SAMPLES_MAX / 4; ++i)
  {
    sampleSet[i].reset();
    (Pose2D&) sampleSet[i] = Pose2D::random(Range<double>(t.translation.x - randomFactor * 50,
                                                t.translation.x + randomFactor * 50),
                                  Range<double>(t.translation.y - randomFactor * 50,
                                                t.translation.y + randomFactor * 50),
                                  Range<double>(t.getAngle() - randomFactor * 0.1,
                                                t.getAngle() + randomFactor * 0.1));
  }
  // place 25% samples at rightt center spot
  t.translation.y = yPosRightSideline;
  t.rotation = pi_2;
  for(; i < SAMPLES_MAX / 2; ++i)
  {
    sampleSet[i].reset();
    (Pose2D&) sampleSet[i] = Pose2D::random(Range<double>(t.translation.x - randomFactor * 50,
                                                t.translation.x + randomFactor * 50),
                                  Range<double>(t.translation.y - randomFactor * 50,
                                                t.translation.y + randomFactor * 50),
                                  Range<double>(t.getAngle() - randomFactor * 0.1,
                                                t.getAngle() + randomFactor * 0.1));
  }

  for(; i < SAMPLES_MAX; ++i)
  {
    sampleSet[i].reset();
    (Pose2D&) sampleSet[i] = field.randomPose();
  }
}


///////////////////////////////////////////////////////////////////////
// Debug drawing

void GT2005SelfLocator::draw() const
{
  NCOMPLEX_DRAWING ( "gt05-sl:sampleSet",
    // draw sample set
    for(int i = 0; i < SAMPLES_MAX; ++i)
      draw(sampleSet[i], getSampleColor(sampleSet[i].probability));
  );

  NCOMPLEX_DRAWING( "gt05-sl:testSample",
    drawTestSample(*testSample, getSampleColor(testSample->probability));
  );

  NCOMPLEX_DRAWING ( "gt05-sl:lastSeenCenterCircle", 
  {
    //debug stuff
    Pose2D pointFromPose = robotPose + lastSeenCenterCircle;
    NCIRCLE( "gt05-sl:lastSeenCenterCircle", 
      pointFromPose.translation.x, pointFromPose.translation.y,
      60,30,Drawings::ps_solid,Drawings::white);
    Pose2D p1 = pointFromPose + Pose2D(0,60);
    Pose2D p2 = pointFromPose + Pose2D(0,-60);
    NLINE("gt05-sl:lastSeenCenterCircle",
        p1.translation.x,
        p1.translation.y,
        p2.translation.x,
        p2.translation.y,
        1,
        Drawings::ps_solid,
        Drawings::white);
  });

  NCOMPLEX_DRAWING( "gt05-sl:lastCrossing",
  {
    int color = Drawings::black;
    switch(lastSeenCrossingClass)
    {
      case GT2005LineCrossingsTable::virtualCrossing:
        color = Drawings::blue;
        break;
      case GT2005LineCrossingsTable::lCrossing:
        color = Drawings::yellow;
        break;
      case GT2005LineCrossingsTable::tCrossing:
        color = Drawings::red;
        break;
      case GT2005LineCrossingsTable::xCrossing:
        color = Drawings::pink;
        break;
    }
    NCIRCLE("gt05-sl:lastCrossing",lastModelCrossing.x,lastModelCrossing.y,40,40,Drawings::ps_solid,color);
    NCIRCLE("gt05-sl:lastCrossing",lastSeenCrossing.x,lastSeenCrossing.y,30,30,Drawings::ps_solid,Drawings::red);
    NLINE("gt05-sl:lastCrossing",lastSeenCrossing.x,lastSeenCrossing.y,lastModelCrossing.x,lastModelCrossing.y,10,Drawings::ps_solid,Drawings::blue);
  });

  DEBUG_RESPONSE("gt05-sl:display field tables",
    Field field;
    field.draw(Drawings::white, LinesPercept::field);
    field.draw(Drawings::red, LinesPercept::border);
    field.draw(Drawings::yellow, LinesPercept::yellowGoal);
    field.draw(Drawings::skyblue, LinesPercept::skyblueGoal);
  );

  DEBUG_RESPONSE("gt05-sl:display field lines observation table, 0 deg",
    observationTable[LinesPercept::field].draw(0);
  );
  DEBUG_RESPONSE("gt05-sl:display field lines observation table, 90 deg",
    observationTable[LinesPercept::field].draw(pi_2);
  );
  DEBUG_RESPONSE("gt05-sl:display field lines observation table, 180 deg",
    observationTable[LinesPercept::field].draw(pi);
  );
  DEBUG_RESPONSE("gt05-sl:display field lines observation table, -90 deg",
    observationTable[LinesPercept::field].draw(-pi_2);
  );

  DEBUG_RESPONSE("gt05-sl:display yellow goal lines observation table, 0 deg",
    observationTable[LinesPercept::yellowGoal].draw(0);
  );
  DEBUG_RESPONSE("gt05-sl:display yellow goal lines observation table, 90 deg",
    observationTable[LinesPercept::yellowGoal].draw(pi_2);
  );
  DEBUG_RESPONSE("gt05-sl:display yellow goal lines observation table, 180 deg",
    observationTable[LinesPercept::yellowGoal].draw(pi);
  );
  DEBUG_RESPONSE("gt05-sl:display yellow goal lines observation table, -90 deg",
    observationTable[LinesPercept::yellowGoal].draw(-pi_2);
  );

  DEBUG_RESPONSE("gt05-sl:display carpet border observation table, 0 deg",
    observationTable[LinesPercept::border].draw(0);
  );
  DEBUG_RESPONSE("gt05-sl:display carpet border observation table, 90 deg",
    observationTable[LinesPercept::border].draw(pi_2);
  );
  DEBUG_RESPONSE("gt05-sl:display carpet border observation table, 180 deg",
    observationTable[LinesPercept::border].draw(pi);
  );
  DEBUG_RESPONSE("gt05-sl:display carpet border observation table, -90 deg",
    observationTable[LinesPercept::border].draw(-pi_2);
  );

}

Drawings::Color GT2005SelfLocator::getSampleColor(double probability) const
{ 
  if (probability == 2 || probability <= QUASI_ZERO)
    return Drawings::black;
  else if (probability < 0.1)
    return Drawings::blue;
  else if (probability < 0.2)
    return Drawings::yellow;
  else if (probability < 0.5)
    return Drawings::orange;
  else if (probability < 0.8)
    return Drawings::red;
  else if (probability < 0.95)
    return Drawings::pink;
  else
    return Drawings::white;
}

void GT2005SelfLocator::draw(const Pose2D& pose,Drawings::Color color) const
{ 
  Pose2D p = pose + Pose2D(-100,0);
  NLINE("gt05-sl:sampleSet",
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  p = pose + Pose2D(-40,-40);
  NLINE("gt05-sl:sampleSet",
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  p = pose + Pose2D(-40,40);
  NLINE("gt05-sl:sampleSet",
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
}

void GT2005SelfLocator::drawTestSample(const Pose2D& pose,Drawings::Color color) const
{ 
  Pose2D p = pose + Pose2D(-100,0);
  NLINE("gt05-sl:testSample",
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  p = pose + Pose2D(-40,-40);
  NLINE("gt05-sl:testSample",
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  p = pose + Pose2D(-40,40);
  NLINE("gt05-sl:testSample",
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
}

---