Modules/SelfLocator/SlamSelfLocator/SlamSampleTemplateGenerator.cpp

Go to the documentation of this file.

#include "SlamSampleTemplateGenerator.h"


/*
*
*  -Buffer class not used. maybe implement priority queue
*  not so important task since template generation is already ok
*  -Implement usage of different seen linetypes!
*   maybe we get this with a separated observation model
*
*/


  SlamSampleTemplateGenerator::SlamSampleTemplateGenerator()
  {
    numberOfTemplates = 0;
    numberOfFlags = 0;
    nextTemplate = 0;
    randomFactor = 0;
  }


  /**
  * this method returns a generated template
  * TODO: declare constant value as constants to modify during runtime
  */
  SlamSelfLocatorSample SlamSampleTemplateGenerator::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 SlamSelfLocatorSample(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 SlamSelfLocatorSample(field.randomPose());
  }



  /*
  * this method generates a robot pose from 3 bearings to known mark0,1,2
  */
  bool SlamSampleTemplateGenerator::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));
        return true;
      }
      }
    }
    return false;
  }

  /*
  * this method generates up to 2 possible poses from two marks seen and the distance
  * to "mark1"
  */
  int SlamSampleTemplateGenerator::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(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));
        return 2;
        }
        else
        return 1;
      }
      else
        return 0;
  }
  

  bool SlamSampleTemplateGenerator::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 SlamSampleTemplateGenerator::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: find a smarter solution for numberOfPerceptTypes and types[]
  * TODO: smarter solution for flags/goals maybe regard goals as flags
  */
void SlamSampleTemplateGenerator::generateTemplates(const LandmarksPercept& landmarksPercept, 
                           const LinesPercept& linesPercept,
                           const Pose2D& odometry, 
                           int   numberOfPerceptTypes, 
                           const LinesPercept::LineType types[LinesPercept::numberOfLineTypes],
                           bool useLandmarks)
{
  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]);
  }

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

  /**
  * Do not generate new templates from landmark percepts
  */
  if (useLandmarks)
  {
    int n = numberOfFlags + 2 * landmarksPercept.numberOfGoals;
    for(i = 0; i < n - 2; ++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 - 1; ++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;
        numberOfTemplates += poseFromBearingsAndDistance(dir0,dir1,dist1,mark0,mark1,cameraOffset,
                                templates[numberOfTemplates],templates[numberOfTemplates + 1]);
        numberOfTemplates += poseFromBearingsAndDistance(dir1,dir0,dist0,mark1,mark0,cameraOffset,
                                templates[numberOfTemplates],templates[numberOfTemplates + 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[numberOfTemplates]))
            if(++numberOfTemplates == TEMPLATES_MAX)
            return; 
            
        }
  };
  /**
  * ----------------------------------------------------
  */

  // this is not so nice, will see if we change it later
  for(i = 0; i < numberOfPerceptTypes; ++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[numberOfTemplates++] =  templateTable[index - LinesPercept::yellowGoal].sample(point.abs()) + Pose2D(-atan2((double)point.y,(double)point.x));
      if(numberOfTemplates == TEMPLATES_MAX)
        return;
    }
  }
}

int SlamSampleTemplateGenerator::getNumberOfTemplates(){
  return numberOfTemplates;
}

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

---