Modules/SelfLocator/GT2005SelfLocator/GT2005SampleTemplateGenerator.cpp

Go to the documentation of this file.

#include "GT2005SampleTemplateGenerator.h"


/*
* Template generation for the gt2005 SelfLocator, completely
* identical to the gt2004 SelfLocators template generation
*
* @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>
*/


GT2005SampleTemplateGenerator::GT2005SampleTemplateGenerator(const GT2005SelfLocatorParameters& parameters) :
  parameters(parameters)
{
  numberOfTemplates = 0;
  numberOfFlags = 0;
  nextTemplate = 0;
  randomFactor = 0;
}


/**
* this method returns a generated template
* TODO: declare constant value as constants to modify during runtime
*/
GT2005SelfLocatorSample GT2005SampleTemplateGenerator::getTemplate()
{
  if(nextTemplate >= numberOfTemplates)
  {
    nextTemplate = 0;
    ++randomFactor;
  }

  while(nextTemplate < numberOfTemplates)
  {
    const Pose2D& t = templates[nextTemplate++];

    // if the template is inside the field, we put it to this position +/- random value
    if(field.isInside(t.translation))
    {
      return GT2005SelfLocatorSample(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)));
    }
  }
  return GT2005SelfLocatorSample(field.randomPose());
}

/*
* this method generates a robot pose from 3 bearings to known mark0,1,2
*/
bool GT2005SampleTemplateGenerator::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)
{
  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; // rotates the vec 1->0 arount 1 by - atan(py,px)
    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));
        if (!checkTemplatePose(resultingPose, mark0, dir0) || 
            !checkTemplatePose(resultingPose, mark1, dir1) ||
            !checkTemplatePose(resultingPose, mark2, dir2))
          return 0;
        return true;
      }
    }
  }
  return false;
}

/*
* this method generates up to 2 possible poses from two marks seen and the distance
* to "mark1"
*/
int GT2005SampleTemplateGenerator::poseFromBearingsAndDistance(
  double dir0,double dir1,double dist, // dir0, dir1 seen angles to the marks, dist= distance to ,mark1
  const Vector2<double>& mark0,   // mark0,1 = mark0/1 in absolute coordinates
  const Vector2<double>& mark1,
  const Vector2<double>& cameraOffset,
  Pose2D& resultingPose1,
  Pose2D& resultingPose2)
  {
    double alpha = dir0 - dir1;         // seen angle  between marks
    Vector2<double> diff = mark0 - mark1;   // deltavector between marks in absolute coords pointing from 1 to 0
    double dir10 = atan2(diff.y,diff.x);    // angle at mark1 to absolute coordinate system pointing to mark0
    double f = dist * sin(alpha) / diff.abs();  // angle at dist*sin(alpha) is 90deg, f = sin(gamma) angle at mark0
    if(fabs(f) <= 1)              // valid length relations?
    {
      double gamma = asin(f),         // gamma = asin(f) and beta (winkel at mark1) from angles sum
        beta = pi - alpha - gamma;

      resultingPose1 = Pose2D(dir10 + beta,mark1) +         // if alpha>pi/2 dir10+beta points to robot postion
              Pose2D(pi - dir1,Vector2<double>(dist,0)) +     // this pose is always inserted
              Pose2D(Vector2<double>(-cameraOffset.x,
                  -cameraOffset.y));
      if (!checkTemplatePose(resultingPose1, mark0, dir0) || 
          !checkTemplatePose(resultingPose1, mark1, dir1))
        return 0;

      if(fabs(alpha) < pi_2 || fabs(alpha) > 3 * pi_2)
      {                               // if alpha<pi/2 gamma might be >pi/2 so this solution might be correct
      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));
      if (!checkTemplatePose(resultingPose2, mark0, dir0) || 
          !checkTemplatePose(resultingPose2, mark1, dir1))
        return 0;

      return 2;
      }
      else
      return 1;
    }
    else
      return 0;
}


bool GT2005SampleTemplateGenerator::getBearing(const LandmarksPercept& landmarksPercept,int i,
                Vector2<double>& mark,
                double& dir,double& dist)
{
  if(i < numberOfFlags)
  {
    const Flag& flag = flags[i];
    mark = flag.position;
    dir = flag.angle;
    dist = flag.distance;
    return true;
  }
  else
  {
    i -= numberOfFlags;
    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;
  }
}

/*
* replaces the flag of the current type in the flags buffer
* 
*/
void GT2005SampleTemplateGenerator::addFlag(const Flag& flag)
{
  if(!flag.isOnBorder(flag.x.min) && !flag.isOnBorder(flag.x.max))
  {
    int i;
    for(i = 0; i < numberOfFlags && flags[i].type != flag.type; ++i)
      ; // i is the index of the first occurance of the flag type

    if(i < numberOfFlags)
      --numberOfFlags;

    if(i < FLAGS_MAX)
      ++i; 

    while(--i > 0)
      flags[i] = flags[i - 1];

    flags[0] = flag;

    if(numberOfFlags < FLAGS_MAX)
      ++numberOfFlags; // one was added
  }
}

/**
* this method generates new sample templates using seen-before flags and current ones
* TODO: smarter solution for flags/goals maybe regard goals as flags
*/
void GT2005SampleTemplateGenerator::generateTemplates(const LandmarksPercept& landmarksPercept, 
                                                      const LinesPercept& linesPercept,
                                                      const Pose2D& odometry)
{
  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");

  DEBUG_RESPONSE( "gt05-sl:reset flag buffer", 
    numberOfFlags = 0;
  );

  randomFactor = 0;

  // for all old flags stored update them by odometry
  // TODO: decrease probability after odo update?
  int i;
  for(i = 0; i < numberOfFlags; ++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]);
  }

  //quick hack only current flags
  //for (i=0;i<landmarksPercept.numberOfFlags;i++)
  //  flags[i] = landmarksPercept.flags[i];
  //numberOfFlags = landmarksPercept.numberOfFlags;

  NCOMPLEX_DRAWING ( "gt05-sl:flags",
    // draw flags
    for (int i = 0; i < numberOfFlags; i++)
    {
      NCIRCLE("gt05-sl:flags", flags[i].position.x, flags[i].position.y, 
            50,1,Drawings::ps_solid, Drawings::white);
    }
  );

  numberOfTemplates = 0;
  nextTemplate = 0;
  double dir0,
         dir1,
         dir2;
  double dist0,
         dist1,
         dist2;
  Vector2<double> mark0,
                  mark1,
                  mark2,
                  cameraOffset(landmarksPercept.cameraOffset.x,
                               landmarksPercept.cameraOffset.y);


  int n = numberOfFlags + 2 * landmarksPercept.numberOfGoals;

  for(i = 0; i < n - 1; ++i)
    // calculate bearing for the i-th mark inside the landmarks percept
    if(getBearing(landmarksPercept,i,mark0,dir0,dist0))
      for(int j = i + 1; j < n; ++j)
        // calculate bearing for the j-th mark inside the landmarks percept
        if(getBearing(landmarksPercept,j,mark1,dir1,dist1))
        {
          if(numberOfTemplates >= TEMPLATES_MAX - 4)
            return;
         int t = poseFromBearingsAndDistance(dir0,dir1,dist1,mark0,mark1,cameraOffset,
                                                        templates[numberOfTemplates],templates[numberOfTemplates + 1]);
         if (t > 0) draw(templates[numberOfTemplates],Drawings::red, mark0, mark1, dir0, dir1);
         if (t > 1) draw(templates[numberOfTemplates + 1],Drawings::red, mark0, mark1, dir0, dir1);
         numberOfTemplates += t;
        
         t = poseFromBearingsAndDistance(dir1,dir0,dist0,mark1,mark0,cameraOffset,
                                                        templates[numberOfTemplates],templates[numberOfTemplates + 1]);
         if (t > 0) draw(templates[numberOfTemplates],Drawings::red, mark0, mark1, dir0, dir1);
         if (t > 1) draw(templates[numberOfTemplates + 1],Drawings::red, mark0, mark1, dir0, dir1);
         numberOfTemplates += t;

          for(int k = j + 1; k < n; ++k)
            if(getBearing(landmarksPercept,k,mark2,dir2,dist2))
              if(poseFromBearings(dir0,dir1,dir2,mark0,mark1,mark2,cameraOffset,
                                  templates[numberOfTemplates]))
              {
                draw(templates[numberOfTemplates],Drawings::pink, mark0, mark1, mark2, dir0, dir1, dir2);
                if(++numberOfTemplates == TEMPLATES_MAX)
                  return; 
              }
    }


  // add template positions from goal linepoints
  // this is not so nice, will see if we change it later
  for(i = LinesPercept::yellowGoal; i <= LinesPercept::skyblueGoal; ++i)
  {
    for(int j = 0; j < linesPercept.points[i].numberOfPoints; ++j)
    {
      const Vector2<int>& point = linesPercept.points[i].pointsOnField[j];
      Pose2D t = templateTable->sample(point.abs());
      if((getPlayer().getTeamColor() == Player::blue && i == LinesPercept::yellowGoal) ||
         (getPlayer().getTeamColor() == Player::red && i == LinesPercept::skyblueGoal))
      {
        // mirror position for opponent goal
        t = Pose2D(pi) + t;
      }
      templates[numberOfTemplates++] = t + Pose2D(-atan2((double)point.y,(double)point.x));
      draw(templates[numberOfTemplates - 1], Drawings::yellow);
      if(numberOfTemplates == TEMPLATES_MAX)
        return;
    }
  }
}

int GT2005SampleTemplateGenerator::getNumberOfTemplates() const
{
  return numberOfTemplates;
}

void GT2005SampleTemplateGenerator::draw(const Pose2D &pose, const Drawings::Color &color) const
{
  NCOMPLEX_DRAWING( "gt05-sl:templates",  
    // draw templates
    NCIRCLE("gt05-sl:templates", pose.translation.x, pose.translation.y, 
          50,1,Drawings::ps_solid, color);
    Pose2D p = pose + Pose2D(50,0);
    NLINE("gt05-sl:templates",
        pose.translation.x,
        pose.translation.y,
        p.translation.x,
        p.translation.y,
        1,
        Drawings::ps_solid,
        color);
  );
}

void GT2005SampleTemplateGenerator::draw(const Pose2D &pose, const Drawings::Color &color,
                                         const Vector2<double>& mark1,
                                         const Vector2<double>& mark2, double dir1, double dir2) const
{
  NCOMPLEX_DRAWING( "gt05-sl:templates",  
    // draw templates
    NLINE("gt05-sl:templates",
        pose.translation.x,
        pose.translation.y,
        mark1.x,
        mark1.y,
        1,
        Drawings::ps_solid,
        Drawings::black);
    NLINE("gt05-sl:templates",
        pose.translation.x,
        pose.translation.y,
        mark2.x,
        mark2.y,
        1,
        Drawings::ps_solid,
        Drawings::black);
    Pose2D p1 = pose + Pose2D(dir1) + Pose2D(50,0);
    Pose2D p2 = pose + Pose2D(dir1) + Pose2D(100,0);
    NLINE("gt05-sl:templates",
        p1.translation.x,
        p1.translation.y,
        p2.translation.x,
        p2.translation.y,
        1,
        Drawings::ps_solid,
        color);
    p1 = pose + Pose2D(dir2) + Pose2D(50,0);
    p2 = pose + Pose2D(dir2) + Pose2D(100,0);
    NLINE("gt05-sl:templates",
        p1.translation.x,
        p1.translation.y,
        p2.translation.x,
        p2.translation.y,
        1,
        Drawings::ps_solid,
        color);
    draw(pose, color);
  );
}

void GT2005SampleTemplateGenerator::draw(const Pose2D &pose, const Drawings::Color &color,
                                         const Vector2<double>& mark1,
                                         const Vector2<double>& mark2,
                                         const Vector2<double>& mark3,
                                         double dir1, double dir2, double dir3) const
{
  NCOMPLEX_DRAWING( "gt05-sl:templates",  
    // draw templates
    NLINE("gt05-sl:templates",
        pose.translation.x,
        pose.translation.y,
        mark1.x,
        mark1.y,
        1,
        Drawings::ps_solid,
        Drawings::black);
    Pose2D p1 = pose + Pose2D(dir1) + Pose2D(50,0);
    Pose2D p2 = pose + Pose2D(dir1) + Pose2D(100,0);
    NLINE("gt05-sl:templates",
        p1.translation.x,
        p1.translation.y,
        p2.translation.x,
        p2.translation.y,
        1,
        Drawings::ps_solid,
        color);
    draw(pose, color, mark2, mark3, dir2, dir3);
  );
}

bool GT2005SampleTemplateGenerator::checkTemplatePose(const Pose2D &pose, const Vector2<double> &mark, double dir)
{
  double dirFromPose = (mark - pose.translation).angle();
  double dirFromBearing = dir + pose.rotation;

  return fabs(normalize(dirFromPose - dirFromBearing)) < 0.05; 
}

---