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::paramR = 2, // sharpness of Gauss for rotational errors
       GT2004SelfLocator::paramTrans = 100, // maximum translational shift if quality is bad
       GT2004SelfLocator::paramRot = 0.5; // maximum rotational shift if quality is bad

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 ? average[i] - paramDelay * paramUp : 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 = value - paramDelay * paramUp;
    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),
distanceToBorderEstimator(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()
{
  const int maxPercepts = 3;
/*  observationTable[1].draw();
  DEBUG_DRAWING_FINISHED(selfLocatorField); 
  return;*/
  robotPose.setFrameNumber(landmarksPercept.frameNumber);

  distanceToBorderEstimator.execute();

  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;
/*  if(speed > 150)
    paramTrans = 0;
  else */
    paramTrans = 50 - speed / 10;

  // 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.numberOfPoints[i] && j < maxPercepts; ++j)
      {
        updateByPoint(linesPercept.points[i][rand() % linesPercept.numberOfPoints[i]],
                      LinesPercept::LineType(i));
        sensorUpdated = true;
      }

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

  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;
  } 
/*  double rotNoise = noise ? paramRot : 0,
         xError = fmax(fabs(odometry.translation.x) * 0.1,fabs(odometry.translation.y) * 0.02),
         yError = fmax(fabs(odometry.translation.y) * 0.1,fabs(odometry.translation.x) * 0.02),
         rotError = fmax(odometry.translation.abs() * 0.002,fabs(odometry.getAngle()) * 0.2);
  for(int i = 0; i < SAMPLES_MAX; ++i)
  {
    Sample& s = sampleSet[i];
    s += odometry;
    if(s.isValid())
      s += Pose2D(((random() * 2 - 1) * fmax(rotError,rotNoise * (1 - s.getQuality()))),
                  Vector2<double>((random() * 2 - 1) * xError,
                                  (random() * 2 - 1) * yError));
    s.camera = s + camera;
  } */
}

void GT2004SelfLocator::updateByPoint(const LinesPercept::LinePoint& point,LinesPercept::LineType type)
{
  COMPLEX_DRAWING(selfLocator,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 /*cameraMatrix.translation.z*/,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 /*cameraMatrix.translation.z*/,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;
    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));
  }
}

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
    {
      COMPLEX_DRAWING(selfLocatorField,draw(s,Drawings::black));
    }
  }
}
  
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)
          {
            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.numberOfPoints[type]; ++j)
    {
      const Vector2<int>& point = linesPercept.points[type][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))
    {
      COMPLEX_DRAWING(selfLocatorField,draw(t,Drawings::red));
      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 Pose2D& pose,Drawings::Color color) const
{ 
  Pose2D p = pose + Pose2D(-100,0);
  LINE(selfLocatorField,
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  p = pose + Pose2D(-40,-40);
  LINE(selfLocatorField,
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
  p = pose + Pose2D(-40,40);
  LINE(selfLocatorField,
       pose.translation.x,
       pose.translation.y,
       p.translation.x,
       p.translation.y,
       1,
       Drawings::ps_solid,
       color);
}

void 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);
  CIRCLE(selfLocator,p.x,p.y,3,1,Drawings::ps_solid,colors[type]);
}

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];
      paramTrans = d.data[6];
      paramRot = d.data[7];
      return true;
  }
  return false;
}

/*
 * Change log :
 * 
 * $Log: GT2004SelfLocator.cpp,v $
 * Revision 1.4  2004/06/29 10:08:33  thomas
 * reduced max-number of matched percepts per type from 4 to 3
 *
 * Revision 1.3  2004/06/28 09:48:14  roefer
 * SelfLocator
 *
 * Revision 1.2  2004/06/24 18:26:38  roefer
 * Lines table redesign, should not influence the performance of GT2003SL
 *
 * Revision 1.1  2004/06/17 11:24:04  roefer
 * Added RGIP and GT2004SL
 *
 * Revision 1.1.1.1  2004/05/22 17:20:44  cvsadm
 * created new repository GT2004_WM
 *
 * Revision 1.9  2004/04/07 14:42:56  risler
 * moved LandsmarksState to Cognition directory, generated by SelfLocator
 *
 * Revision 1.8  2004/03/27 16:17:24  juengel
 * Added DistanceToBorderEstimator.
 *
 * Revision 1.7  2004/03/08 02:11:46  roefer
 * Interfaces should be const
 *
 * Revision 1.6  2004/03/08 01:09:33  roefer
 * Use of LinesTables restructured
 *
 * Revision 1.5  2004/01/23 00:13:23  roefer
 * ERS-7 simulation, first draft
 *
 * Revision 1.4  2003/12/15 11:46:59  juengel
 * Introduced CameraInfo
 *
 * Revision 1.3  2003/11/14 19:02:26  goehring
 * frameNumber added
 *
 * Revision 1.2  2003/11/13 16:47:38  goehring
 * frameNumber added
 *
 * Revision 1.1  2003/10/06 14:10:14  cvsadm
 * Created GT2004 (M.J.)
 *
 * Revision 1.3  2003/09/26 11:38:52  juengel
 * - sorted tools
 * - clean-up in DataTypes
 *
 * Revision 1.2  2003/09/01 10:20:11  juengel
 * DebugDrawings clean-up 2
 * DebugImages clean-up
 * MessageIDs clean-up
 * Stopwatch clean-up
 *
 * Revision 1.1.1.1  2003/07/02 09:40:24  cvsadm
 * created new repository for the competitions in Padova from the 
 * tamara CVS (Tuesday 2:00 pm)
 *
 * removed unused solutions
 *
 * Revision 1.3  2003/06/04 10:46:56  mkunz
 * + 50 -> * 50 in getTemplate()
 *
 * Revision 1.2  2003/05/24 19:53:28  roefer
 * GT2004SL finished
 *
 * Revision 1.1  2003/05/22 07:53:05  roefer
 * GT2004SelfLocator added
 *
 */

---