Modules/SelfLocator/GT2004SelfLocator.cpp

Go to the documentation of this file.

/**
 * @file Modules/SelfLocator/GT2004SelfLocator.cpp
 * 
 * This file contains a class for self-localization based on the Monte Carlo approach 
 * using goals, landmarks, and field lines.
 *
 * @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
 */

#include "Platform/GTAssert.h"
#include "GT2004SelfLocator.h"
#include "Platform/SystemCall.h"
#include "Tools/FieldDimensions.h"
#include "Tools/Player.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Streams/InStreams.h"
#include "Tools/Debugging/GenericDebugData.h"

double GT2004SelfLocator::paramUp = 0.01, // step size for increasing qualities
       GT2004SelfLocator::paramDown = 0.005, // step size for decreasing qualities
       GT2004SelfLocator::paramDelay = 3, // factor
       GT2004SelfLocator::paramHeight = 150, // hypothetic height of head
       GT2004SelfLocator::paramZ = 10, // sharpness of Gauss for rotational errors 
       GT2004SelfLocator::paramY = 2; // sharpness of Gauss for translational errors

GT2004SelfLocator::Sample::Sample() 
{
  for(int i = 0; i < numberOfQualities; ++i)
    quality[i] = 0.5;
  probability = 2;
}

GT2004SelfLocator::Sample::Sample(const Pose2D& pose,const double* quality)
: PoseSample(pose)
{
  for(int i = 0; i < numberOfQualities; ++i)
    this->quality[i] = 2;
  probability = 2;
}

void GT2004SelfLocator::Sample::updateQuality(const double* average)
{
  probability = 1;
  for(int i = 0; i < numberOfQualities; ++i)
    probability *= quality[i] == 2 ? max(average[i] - paramDelay * paramUp, 0.000001) : quality[i];
}

void GT2004SelfLocator::Sample::setProbability(LinesPercept::LineType type,double value)
{
  double& q = quality[type > LinesPercept::yellowGoal ? type - 1 : type];
  if(type > LinesPercept::yellowGoal)
  {
    if(q == 2)
      q = value;
    else if(value < q)
      if(value < q - 0.05)
        q -= 0.05;
      else
        q = value;
    else 
      if(value > q + 0.1)
        q += 0.1;
      else
        q = value;
  }
  else
  {
    if(q == 2)
      q = max(value - paramDelay * paramUp, 0.000001);
    else if(value < q)
      if(value < q - paramDown)
        q -= paramDown;
      else
        q = value;
    else 
      if(value > q + paramUp)
        q += paramUp;
      else
        q = value;
  }
}

GT2004SelfLocator::GT2004SelfLocator(const SelfLocatorInterfaces& interfaces)
: SelfLocator(interfaces)
{
  int i;
  for(i = 0; i < Sample::numberOfQualities; ++i)
    average[i] = 0.5;
  for(i = 0; i < SAMPLES_MAX; ++i)
    (Pose2D&) sampleSet[i] = field.randomPose();
  numberOfTypes = 0;
  numOfFlags = 0;
  sensorUpdated = false;
  timeStamp = 0;
  speed = 0;
}

void GT2004SelfLocator::execute()
{
  NDECLARE_DEBUGDRAWING( "gt04-sl" , "drawingOnField" , "GT2004SelfLocator debug drawings");
  const int maxPercepts = 3;
/*  observationTable[1].draw();
  DEBUG_DRAWING_FINISHED(selfLocatorField); 
  return;*/
  robotPose.setFrameNumber(landmarksPercept.frameNumber);
  robotPose.setTimestamp(SystemCall::getCurrentSystemTime());

  Pose2D odometry = odometryData - lastOdometry;
  lastOdometry = odometryData;
  if(SystemCall::getTimeSince(timeStamp) > 500)
  {
    speed = (odometryData - lastOdometry2).translation.abs() / SystemCall::getTimeSince(timeStamp) * 1000.0;
    lastOdometry2 = odometryData;
    timeStamp = SystemCall::getCurrentSystemTime();
  }
  paramZ = 10 - 9 * speed / 400;

  // First step: action update
  updateByOdometry(odometry,
                   Pose2D(landmarksPercept.cameraOffset.x,
                          landmarksPercept.cameraOffset.y),
                          sensorUpdated);

  // Second step: observation update
  numberOfTypes = 0;
  sensorUpdated = false;
  if(cameraMatrix.isValid)
  {
    int i;
    for(i = 0; i < 2; ++i)
      for(int j = 0; j < linesPercept.points[i].numberOfPoints && j < maxPercepts; ++j)
      {
        updateByPoint(linesPercept.points[i].pointsOnField[rand() % linesPercept.points[i].numberOfPoints],
                      LinesPercept::LineType(i));
        sensorUpdated = true;
      }

    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;
        updateByPoint(linesPercept.points[select].pointsOnField[rand() % linesPercept.points[select].numberOfPoints],
                      select);
        sensorUpdated = true;
      }
    for(i = 0; i < landmarksPercept.numberOfFlags; ++i)
    {
      const Flag& flag = landmarksPercept.flags[i];
      if(!flag.isOnBorder(flag.x.max))
        updateByFlag(flag.position,
                     LEFT_SIDE_OF_FLAG,
                     flag.x.max);
      if(!flag.isOnBorder(flag.x.min))
        updateByFlag(flag.position,
                     RIGHT_SIDE_OF_FLAG,
                     flag.x.min);
    }
    for(i = 0; i < landmarksPercept.numberOfGoals; ++i)
    {
      const Goal& goal = landmarksPercept.goals[i];
      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(sensorUpdated)
    {
      // generate templates for calculated sampleSet
      generatePoseTemplates(landmarksPercept,odometry);
      resample();
    }
  }

  Pose2D pose;
  double validity;
  calcPose(pose,validity);
  robotPose.setPose(pose);
  robotPose.setValidity(validity);
  sampleSet.link(selfLocatorSamples);

  //COMPLEX_DRAWING(selfLocator,drawEllipse());  


  DEBUG_DRAWING_FINISHED(selfLocatorField); 
  DEBUG_DRAWING_FINISHED(selfLocator); 

  landmarksState.update(landmarksPercept);
}

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

void GT2004SelfLocator::updateByOdometry(const Pose2D& odometry,
                                         const Pose2D& camera,
                                         bool noise)
{     
  double transNoise = noise ? 200 : 0,
         rotNoise = noise ? 0.5 : 0;    
  double dist = odometry.translation.abs();    
  double angle = fabs(odometry.getAngle());
  double f;
  
  for(int i = 0; i < SAMPLES_MAX; ++i)
  {
    Sample& s = sampleSet[i];
    s += odometry;
    if(s.probability == 2)
      f = 0;
    else
      f = (1 - s.probability) * (1 - s.probability);
    double rotError = ((random() * 2 - 1) * fmax(fmax(dist * 0.002,angle * 0.2),rotNoise * f));
    Vector2<double> transError((random() * 2 - 1) * max(fmax(fabs(odometry.translation.x) * 0.1,
                                                             fabs(odometry.translation.y) * 0.02),
                                                        transNoise * f),
                               (random() * 2 - 1) * max(fmax(fabs(odometry.translation.y) * 0.1,
                                                             fabs(odometry.translation.x) * 0.02),
                                                        transNoise * f));
    s += Pose2D(rotError,transError);
    s.camera = s + camera;
  } 
}

void GT2004SelfLocator::updateByPoint(const LinesPercept::LinePoint& point,LinesPercept::LineType type)
{
  NCOMPLEX_DRAWING("gt04-sl", draw(point,type));  
  
  int index = type;
  if(getPlayer().getTeamColor() == Player::blue && type >= LinesPercept::yellowGoal) 
    index = LinesPercept::yellowGoal + LinesPercept::skyblueGoal - type;
  double d = point.abs();
  double zObs = atan2(paramHeight,d),
         yObs = atan2((double)point.y,(double)point.x);
  Pose2D point2(point.angle, Vector2<double>(point.x, point.y));
  for(int i = 0; i < SAMPLES_MAX; ++i)
  {
    Sample& s = sampleSet[i];
    Pose2D p = s + point2;
    Vector2<double> vMin;
    bool isY = index == LinesPercept::field && (fabs(p.getAngle()) < pi / 4 || fabs(p.getAngle()) > 3 * pi / 4);
    if(isY)
      vMin = observationTable[LinesPercept::numberOfLineTypes].getClosestPoint(p.translation);
    else
      vMin = observationTable[index].getClosestPoint(p.translation);
    if(vMin.x != 1000000)
    {
      Vector2<double> diff = vMin - p.translation;
      Vector2<double> v = (Pose2D(vMin) - s).translation;
      double zModel = atan2(paramHeight,v.abs()),
             yModel = atan2(v.y,v.x);
      s.setProbability(isY ? LinesPercept::LineType(LinesPercept::numberOfLineTypes + 1) : type, sigmoid((zModel - zObs) * paramZ) * sigmoid((yModel - yObs) * paramY));
    }
    else
      s.setProbability(type,0.000001);
  }
}

void GT2004SelfLocator::updateByFlag(const Vector2<double>& flag,
                                     FlagSides sideOfFlag,
                                     double measuredBearing)
{
  sensorUpdated = true;
  for(int i = 0; i < SAMPLES_MAX; ++i)
  {
    Sample& s = sampleSet[i];
    Pose2D p(flag.x,flag.y);
    p -= s.camera;
    if (p.translation.abs() > flagRadius)
    {
      double assumedBearing = atan2(p.translation.y,p.translation.x) + sideOfFlag * asin(flagRadius / p.translation.abs());
      double similarity = fabs((assumedBearing - measuredBearing)) / pi;
      if(similarity > 1)
        similarity = 2.0 - similarity;
      s.setProbability(LinesPercept::numberOfLineTypes,sigmoid(similarity * 7));
    }
    else
      s.setProbability(LinesPercept::numberOfLineTypes,0.000001);
  }
}

void GT2004SelfLocator::updateByGoalPost(const Vector2<double>& goalPost,
                                         double measuredBearing)
{ 
  sensorUpdated = true;
  for(int i = 0; i < SAMPLES_MAX; ++i)
  {
    Sample& 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;
    s.setProbability(LinesPercept::numberOfLineTypes,sigmoid(similarity * 7));
  }
}

void GT2004SelfLocator::resample()
{
  int i,j,
      count[Sample::numberOfQualities];

  for(i = 0; i < Sample::numberOfQualities; ++i)
  {
    average[i] = 0;
    count[i] = 0;
  }

  for(i = 0; i < SAMPLES_MAX; ++i)
  {
    Sample& s = sampleSet[i];
    for(j = 0; j < Sample::numberOfQualities; ++j)
      if(s.quality[j] != 2)
      {
        average[j] += s.quality[j];
        ++count[j];
      }
  }

  double average2 = 1;
  for(i = 0; i < Sample::numberOfQualities; ++i)
  {
    if(count[i])
      average[i] /= count[i];
    else
      average[i] = 0.5;
    average2 *= average[i];
  }

  // move the quality of all samples towards the probability
  for(i = 0; i < SAMPLES_MAX; ++i)
    sampleSet[i].updateQuality(average);
    
  // swap sample arrays
  Sample* oldSet = sampleSet.swap();

  double probSum = 0;
  for(i = 0; i < SAMPLES_MAX; ++i)
    probSum += oldSet[i].getQuality();
  double prob2Index = (double) SAMPLES_MAX / probSum;

  double indexSum = 0;
  j = 0;
  for(i = 0; i < SAMPLES_MAX; ++i)
  {
    indexSum += oldSet[i].getQuality() * prob2Index;
    while(j < SAMPLES_MAX && j < indexSum - 0.5)
      sampleSet[j++] = oldSet[i];
  }

  for(i = 0; i < SAMPLES_MAX; ++i)
  {
    Sample& s = sampleSet[i];
    // if quality is too low, select a new random pose for the sample
    if(numOfTemplates && random() * average2 > s.getQuality())
      s = getTemplate();
    else
    {
      NCOMPLEX_DRAWING("gt04-sl",draw(s));
    }
  }
}
  
void GT2004SelfLocator::calcPose(Pose2D& pose,double& validity)
{
  Cell cells[GRID_MAX][GRID_MAX][GRID_MAX];
  double width = field.x.getSize(),
         height = field.y.getSize();

  // attach the samples to the corresponding grid cells
  for(int i = 0; i < SAMPLES_MAX; ++i)
  {
    Sample& p = sampleSet[i];
    if(p.isValid())
    {
      int r = static_cast<int>((p.getAngle() / pi2 + 0.5) * GRID_MAX),
          y = static_cast<int>((p.translation.y / height + 0.5) * GRID_MAX),
          x = static_cast<int>((p.translation.x / width + 0.5) * GRID_MAX);
      if(x < 0)
        x = 0;
      else if(x >= GRID_MAX)
        x = GRID_MAX-1;
      if(y < 0)
        y = 0;
      else if(y >= GRID_MAX)
        y = GRID_MAX-1;
      if(r < 0)
        r = GRID_MAX-1;
      else if(r >= GRID_MAX)
        r = 0;
      Cell& c = cells[r][y][x];
      p.next = c.first;
      c.first = &p;
      ++c.count;
    }
  }  
  
  // determine the 2x2x2 sub-cube that contains most samples
  int rMax = 0,
      yMax = 0,
      xMax = 0,
      countMax = 0,
      r;
  for(r = 0; r < GRID_MAX; ++r)
  {
    int rNext = (r + 1) % GRID_MAX;
    for(int y = 0; y < GRID_MAX - 1; ++y)
      for(int x = 0; x < GRID_MAX - 1; ++x)
      {
        int count = cells[r][y][x].count +
                    cells[r][y][x+1].count +
                    cells[r][y+1][x].count +
                    cells[r][y+1][x+1].count +
                    cells[rNext][y][x].count +
                    cells[rNext][y][x+1].count +
                    cells[rNext][y+1][x].count +
                    cells[rNext][y+1][x+1].count;
        if(count > countMax)
        {
          countMax = count;
          rMax = r;
          yMax = y;
          xMax = x;
        }
      }
  }

  if(countMax > 0)
  {
    // determine the average pose of all samples in the winner sub-cube
    double xSum = 0,
                  ySum = 0,
                  cosSum = 0,
                  sinSum = 0,
                  qualitySum = 0;
    for(r = 0; r < 2; ++r)
    {
      int r2 = (rMax + r) % GRID_MAX;
      for(int y = 0; y < 2; ++y)
        for(int x = 0; x < 2; ++x)
        {
          for(Sample* p = cells[r2][yMax + y][xMax + x].first; p; p = p->next)
            if(p->isValid())
            {
              xSum += p->translation.x * p->getQuality();
              ySum += p->translation.y * p->getQuality();
              cosSum += p->getCos() * p->getQuality();
              sinSum += p->getSin() * p->getQuality();
              qualitySum += p->getQuality();
            }
        }
    }  
    if(qualitySum)
    {
      pose = Pose2D(atan2(sinSum,cosSum),
                            Vector2<double>(xSum / qualitySum,ySum / qualitySum));
      validity = qualitySum / SAMPLES_MAX;
      double diff = field.clip(pose.translation);
      if(diff > 1)
        validity /= sqrt(diff);
      return;
    }
  }
  validity = 0;
}

///////////////////////////////////////////////////////////////////////
// New samples, i.e. random and templates

bool GT2004SelfLocator::poseFromBearings(double dir0,double dir1,double dir2,
                                         const Vector2<double>& mark0,
                                         const Vector2<double>& mark1,
                                         const Vector2<double>& mark2,
                                         const Vector2<double>& cameraOffset,
                                         Pose2D& resultingPose) const 
{
  double w1 = dir1 - dir0,
         w2 = dir2 - dir1;
  if(sin(w1) != 0 && sin(w2) != 0)
  {
    Vector2<double> p = mark2 - mark1;
    double a2 = p.abs();
    p = (Pose2D(mark0) - Pose2D(atan2(p.y,p.x),mark1)).translation;
    double a1 = p.abs();
    if(a1 > 0 && a2 > 0)
    {
      double w3 = pi2 - w1 - w2 - atan2(p.y,p.x);
      double t = a1 * sin(w2) / (a2 * sin(w1)) + cos(w3);
      if(t != 0)
      {
        double w = atan(sin(w3) / t);
        double b = a1 * sin(w) / sin(w1);
        w = pi - w1 - w;
        p = mark0 - mark1;
        resultingPose = Pose2D(atan2(p.y,p.x) - w,mark1)
                        + Pose2D(Vector2<double>(b,0));
        p = (Pose2D(mark1) - resultingPose).translation;
        resultingPose += Pose2D(atan2(p.y,p.x) - dir1);
        resultingPose += Pose2D(Vector2<double>(-cameraOffset.x,
                                                -cameraOffset.y));
        return true;
      }
    }
  }
  return false;
}

int GT2004SelfLocator::poseFromBearingsAndDistance(
  double dir0,double dir1,double dist,
  const Vector2<double>& mark0,
  const Vector2<double>& mark1,
  const Vector2<double>& cameraOffset,
  Pose2D& resultingPose1,
  Pose2D& resultingPose2) const
{
  double alpha = dir0 - dir1;
  Vector2<double> diff = mark0 - mark1;
  double dir10 = atan2(diff.y,diff.x);
  double f = dist * sin(alpha) / diff.abs();
  if(fabs(f) <= 1)
  {
    double gamma = asin(f),
           beta = pi - alpha - gamma;
    resultingPose1 = Pose2D(dir10 + beta,mark1) + 
                     Pose2D(pi - dir1,Vector2<double>(dist,0)) +
                     Pose2D(Vector2<double>(-cameraOffset.x,
                            -cameraOffset.y));
    if(fabs(alpha) < pi_2 || fabs(alpha) > 3 * pi_2)
    {
      if(gamma > 0)
        gamma = pi - gamma;
      else
        gamma = -pi - gamma;
      beta = pi - alpha - gamma;
      resultingPose2 = Pose2D(dir10 + beta,mark1) + 
                       Pose2D(pi - dir1,Vector2<double>(dist,0)) +
                       Pose2D(Vector2<double>(-cameraOffset.x,
                              -cameraOffset.y));
      return 2;
    }
    else
      return 1;
  }
  else
    return 0;
}

bool GT2004SelfLocator::getBearing(const LandmarksPercept& landmarksPercept,int i,
                                   Vector2<double>& mark,
                                   double& dir,double& dist) const
{
  if(i < numOfFlags)
  {
    const Flag& flag = flags[i];
    mark = flag.position;
    dir = flag.angle;
    dist = flag.distance;
    return true;
  }
  else
  {
    i -= numOfFlags;
    const Goal& goal = landmarksPercept.goals[i >> 1];
    if((i & 1) == 0 && !goal.isOnBorder(goal.x.min))
    {
      mark = goal.rightPost;
      dir = goal.x.min;
      dist = goal.distance;
      return true;
    }
    else if((i & 1) == 1 && !goal.isOnBorder(goal.x.max))
    {
      mark = goal.leftPost;
      dir = goal.x.max;
      dist = goal.distance;
      return true;
    }
    else
      return false;
  }
}

void GT2004SelfLocator::addFlag(const Flag& flag)
{
  if(!flag.isOnBorder(flag.x.min) && !flag.isOnBorder(flag.x.max))
  {
    int i;
    for(i = 0; i < numOfFlags && flags[i].type != flag.type; ++i)
      ;
    if(i < numOfFlags)
      --numOfFlags; // remove one
    if(i < FLAGS_MAX)
      ++i; // still room at the end
    while(--i > 0)
      flags[i] = flags[i - 1];
    flags[0] = flag;
    if(numOfFlags < FLAGS_MAX)
      ++numOfFlags; // one was added
  }
}

void GT2004SelfLocator::generatePoseTemplates(const LandmarksPercept& landmarksPercept,
                                              const Pose2D& odometry)
{
  randomFactor = 0;
  int i;
  for(i = 0; i < numOfFlags; ++i)
  {
    Flag& flag = flags[i];
    Vector2<double> p = (Pose2D(flag.angle) 
      + Pose2D(Vector2<double>(flag.distance,0)) - odometry).translation;
    flag.angle = atan2(p.y,p.x);
    flag.distance = p.abs();
  }
  for(i = 0; i < landmarksPercept.numberOfFlags; ++i)
    addFlag(landmarksPercept.flags[i]);

  numOfTemplates = 0;
  nextTemplate = 0;
  double dir0,
         dir1,
         dir2;
  double dist0,
         dist1;
  Vector2<double> mark0,
                  mark1,
                  mark2,
                  cameraOffset(landmarksPercept.cameraOffset.x,
                               landmarksPercept.cameraOffset.y);
  int n = numOfFlags + 2 * landmarksPercept.numberOfGoals;
  for(i = 0; i < n - 2; ++i)
    if(getBearing(landmarksPercept,i,mark0,dir0,dist0))
      for(int j = i + 1; j < n - 1; ++j)
        if(getBearing(landmarksPercept,j,mark1,dir1,dist1))
        {
          if(numOfTemplates >= SAMPLES_MAX - 4)
            return;
          numOfTemplates += poseFromBearingsAndDistance(dir0,dir1,dist1,mark0,mark1,cameraOffset,
                                                        templates[numOfTemplates],templates[numOfTemplates + 1]);
          numOfTemplates += poseFromBearingsAndDistance(dir1,dir0,dist0,mark1,mark0,cameraOffset,
                                                        templates[numOfTemplates],templates[numOfTemplates + 1]);
          for(int k = j + 1; k < n; ++k)
            if(getBearing(landmarksPercept,k,mark2,dir2,dist0))
              if(poseFromBearings(dir0,dir1,dir2,mark0,mark1,mark2,cameraOffset,
                                  templates[numOfTemplates]))
                if(++numOfTemplates == SAMPLES_MAX)
                  return;
        }
  for(i = 0; i < numberOfTypes; ++i)
  {
    LinesPercept::LineType type = types[i];
    for(int j = 0; j < linesPercept.points[type].numberOfPoints; ++j)
    {
      const Vector2<int>& point = linesPercept.points[type].pointsOnField[j];
      int index = type;
      if(getPlayer().getTeamColor() == Player::blue && type >= LinesPercept::yellowGoal) 
        index = LinesPercept::yellowGoal + LinesPercept::skyblueGoal - type;
      templates[numOfTemplates++] = templateTable[index - LinesPercept::yellowGoal].sample(point.abs()) + Pose2D(-atan2((double)point.y,(double)point.x));
      if(numOfTemplates == SAMPLES_MAX)
        return;
    }
  }
}

GT2004SelfLocator::Sample GT2004SelfLocator::getTemplate()
{
  if(nextTemplate >= numOfTemplates)
  {
    nextTemplate = 0;
    ++randomFactor;
  }
  while(nextTemplate < numOfTemplates)
  {
    const Pose2D& t = templates[nextTemplate++];
    if(field.isInside(t.translation))
    {
      return Sample(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)),
                    average);
    }
  }
  return Sample(field.randomPose(),average);
}

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

void GT2004SelfLocator::draw(const Sample& pose) const
{ 
  Drawings::Color color;

  if (pose.probability == 2 || pose.probability == 0)
    color = Drawings::black;
  else if (pose.probability < 0.1)
    color = Drawings::blue;
  else if (pose.probability < 0.2)
    color = Drawings::yellow;
  else if (pose.probability < 0.5)
    color = Drawings::orange;
  else if (pose.probability < 0.8)
    color = Drawings::red;
  else 
    color = Drawings::pink;

  Pose2D p = pose + Pose2D(-100,0);
  NLINE("gt04-sl",
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  p = pose + Pose2D(-40,-40);
  NLINE("gt04-sl",
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  p = pose + Pose2D(-40,40);
  NLINE("gt04-sl",
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
}

void GT2004SelfLocator::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);
  NCIRCLE("gt04-sl",p.x,p.y,3,1,Drawings::ps_solid,colors[type]);
}

void GT2004SelfLocator::drawEllipse()
{
  //DEBUG Daniel>
  
  double mx,my,sxx,sxy,syy;       // means and covariance variables
  mx = my = sxx = sxy = syy = 0;
  double l1, l2, ev1x, ev1y, ev2x, ev2y;    // EigenValues and EigenVectors
  l1 = l2 = ev1x = ev1y = ev2x = ev2y = 0;

  int i ;
  // Mean calculation
  for (i = 0; i < selfLocatorSamples.getNumberOfSamples(); i++)
  {
  mx += selfLocatorSamples[i].translation.x;
  my += selfLocatorSamples[i].translation.y;
  }
  mx /= selfLocatorSamples.getNumberOfSamples();
  my /= selfLocatorSamples.getNumberOfSamples();

  // Covariance calculation
  for (i = 0; i < selfLocatorSamples.getNumberOfSamples(); i++)
  {
  sxx += (selfLocatorSamples[i].translation.x - mx) * (selfLocatorSamples[i].translation.x - mx);
  sxy += (selfLocatorSamples[i].translation.x - mx) * (selfLocatorSamples[i].translation.y - my);
  syy += (selfLocatorSamples[i].translation.y - my) * (selfLocatorSamples[i].translation.y - my);
  }

  sxx /= selfLocatorSamples.getNumberOfSamples();
  sxy /= selfLocatorSamples.getNumberOfSamples();
  syy /= selfLocatorSamples.getNumberOfSamples();

  l1 = (sxx + syy)/2 + sqrt((sxx + syy)/2*(sxx + syy)/2 - sxx * syy + sxy * sxy);
  l2 = (sxx + syy)/2 - sqrt((sxx + syy)/2*(sxx + syy)/2 - sxx * syy + sxy * sxy);

  // Calculate the Eigenvectors
    double amount;  //vector amount
  ev1x =  (syy - l1 - sxy) / (sxx - l1 - sxy);
  amount = sqrt(ev1x*ev1x + 1);
  
  ev1x /= amount;
  ev1y  = 1.0 / amount;

  ev2x =  (syy - l2 - sxy) / (sxx - l2 - sxy);
  amount = sqrt(ev2x*ev2x + 1);
  
  ev2x /= amount;
  ev2y  = 1.0 / amount;

  double alpha = atan2(ev1y,ev1x);

  double a = sqrt(l1*1);
  double b = sqrt(l2*1);
  double x = 0;
  double y = 0;
    int step = (int)(a/200);
      if (!step)
      {
        step = 1;
      }
      
      for (int xc = 0; xc < a; xc += step)
      {         
        x = xc;
        y = (sqrt(1-sqr(x/a))* b);

    NDOT("gt04-sl",  cos(alpha)*(x + mx) - sin(alpha)*(y + my), 
                 sin(alpha)*(x + mx) + cos(alpha)*(y + my), 
                 2, Drawings::red);
    NDOT("gt04-sl",  cos(alpha)*(x + mx) - sin(alpha)*(-y + my), 
                 sin(alpha)*(x + mx) + cos(alpha)*(-y + my), 
                 2, Drawings::red);
    NDOT("gt04-sl",  cos(alpha)*(-x + mx) - sin(alpha)*(y + my), 
                 sin(alpha)*(-x + mx) + cos(alpha)*(y + my), 
                 2, Drawings::red);
    NDOT("gt04-sl",  cos(alpha)*(-x + mx) - sin(alpha)*(-y + my), 
                 sin(alpha)*(-x + mx) + cos(alpha)*(-y + my), 
                 2, Drawings::red);

  }
    //OUTPUT(idText, text, "Mitte: " << mx << ";" << my << " l1: " << l1 << " l2: " << l2 << " phi " << alpha * 180 / 3.1415 << " Grad");
}


bool GT2004SelfLocator::handleMessage(InMessage& message)
{
  switch(message.getMessageID())
  {
    case idLinesSelfLocatorParameters:
      GenericDebugData d;
      message.bin >> d;
      paramUp = d.data[0];
      paramDown = d.data[1];
      paramDelay = d.data[2];
      paramHeight = d.data[3];
      paramZ = d.data[4];
      paramY = d.data[5];
      return true;
  }
  return false;
}

---