Modules/SelfLocator/LinesTables2004.h

Go to the documentation of this file.

/**
* @file Modules/SelfLocator/LinesTables2004.h
* 
* This file contains a class that represents the tables used for localization based on field lines.
*
* @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
*/

#ifndef __LinesTables2004_h_
#define __LinesTables2004_h_

#include "Tools/Debugging/DebugDrawings.h"
#include "Tools/Streams/InOut.h"
#include "Tools/Field.h"
#include "Platform/GTAssert.h"

/**
* @class ObservationTableBase
* The class is a helper to support streaming of template class ObservationTable.
*/
class ObservationTableBase
{
  public:
    /**
    * The function writes the contents of an object to a stream.
    * @param stream The stream the data is written to.
    */
    virtual void write(Out& stream) const = 0;
  
    /**
    * The function reads the contents of an object from a stream.
    * @param stream The stream the data is read from.
    */
    virtual void read(In& stream) = 0;
};

/**
* @class ObservationTable
* The class realizes a table of closest distances to lines of a certain type.
*/
template<int xSize,int ySize,int cellSize> class ObservationTable : public ObservationTableBase
{
  private:
    char point[ySize][xSize][2];

  public:
    void create(const Field& field,LinesPercept::LineType type)
    {
      for(int y = 0; y < ySize; ++y)
        for(int x = 0; x < xSize; ++x)
        {
          Vector2<double> p = field.getClosestPoint(
            Vector2<double>((x + 0.5 - xSize / 2) * cellSize,
            (y + 0.5 - ySize / 2) * cellSize),
            type);
          point[y][x][0] = char(sgn(p.x) * int(fabs(p.x / 20 + 0.5)));
          point[y][x][1] = char(sgn(p.y) * int(fabs(p.y / 20 + 0.5)));
        }
    }
  
    Vector2<double> getClosestPoint(const Vector2<double>& v) const
    {
      int x = int(v.x / cellSize + xSize / 2),
          y = int(v.y / cellSize + ySize / 2);
      if(x < 0 || x >= xSize || y < 0 || y >= ySize)
        return Vector2<double>(1e6,1e6);
      else
        return Vector2<double>(point[y][x][0] * 20,point[y][x][1] * 20);
    }
  
    void draw()
    {
      for(int y = 0; y < ySize; y += 5)
        for(int x = 0; x < xSize; x += 5)
        {
          double x1 = (x + 0.5 - xSize / 2) * cellSize,
                 y1 = (y + 0.5 - ySize / 2) * cellSize,
                 x2 = point[y][x][0] * 20,
                 y2 = point[y][x][1] * 20;
          if(sqrt(sqr(x1 - x2) + sqr(y1 - y2)) > 40)
          {
            LINE(selfLocatorMonteCarlo,x1,y1,x2,y2,
              0, Drawings::ps_solid,
              Drawings::gray);
            Pose2D p(atan2(y2-y1,x2-x1),Vector2<double>(x2,y2)),
                   p2 = p + Pose2D(Vector2<double>(-50,-50)),
                   p3 = p + Pose2D(Vector2<double>(-50,50));
            LINE(selfLocatorMonteCarlo,x2,y2,p2.translation.x,p2.translation.y,
              1, Drawings::ps_solid,
              Drawings::gray);
            LINE(selfLocatorMonteCarlo,x2,y2,p3.translation.x,p3.translation.y,
              1, Drawings::ps_solid,
              Drawings::gray);
          }
        }
    }

    void write(Out& stream) const 
    {
      stream.write(point,sizeof(point));
    }

    void read(In& stream) 
    {
      stream.read(point,sizeof(point));
    }
  };

inline Out& operator<<(Out& stream,const ObservationTableBase& table)
{
  table.write(stream);
  return stream;
}

inline In& operator>>(In& stream,ObservationTableBase& table)
{
  table.read(stream);
  return stream;
}

/**
* @class TemplateTableBase
* The class is a helper to support streaming of template class TemplateTable.
*/
class TemplateTableBase
{
  public:
    /**
    * The function writes the contents of an object to a stream.
    * @param stream The stream the data is written to.
    */
    virtual void write(Out& stream) const = 0;
  
    /**
    * The function reads the contents of an object from a stream.
    * @param stream The stream the data is read from.
    */
    virtual void read(In& stream) = 0;
};

/**
* The operator writes the template table into a stream.
* @param stream The stream the table is written into.
* @param table The table to be written.
* @return The stream.
*/
inline Out& operator<<(Out& stream,const TemplateTableBase& table)
{
  table.write(stream);
  return stream;
}

/**
* The operator reads the template table from a stream.
* @param stream The stream the table is read from.
* @param table The table to be read.
* @return The stream.
*/
inline In& operator>>(In& stream,TemplateTableBase& table)
{
  table.read(stream);
  return stream;
}


/**
* The class realizes a table of template poses.
*/
template<int TEMPLATES_MAX> class TemplateTable : public TemplateTableBase
{
  private:
    /**
    * The class is required for sorting the template table by the distances to lines.
    */
    class Temp : public Pose2D
    {
      public:
        int distance; /**< The distance to the closest line in direction of the pose. */
    };

    enum 
    {
      DISTANCE_MAX = 500 /**< The number of distances. */
    };

    char templates[TEMPLATES_MAX][3]; /**< The template poses sorted by distance. */
    unsigned short distance[DISTANCE_MAX + 1]; /**< Indices into the template table depending on the distance. */

    /**
    * The function is required for sorting the template poses.
    * It is used as compare function for qsort.
    * @param t1 The first element to compare.
    * @param t2 The second element to compare.
    * @return -1, 0, or 1 according to the specification required by qsort.
    */
    static int compare(const Temp* t1,const Temp* t2)
    {
      return t1->distance == t2->distance ? 0 : t1->distance < t2->distance ? -1 : 1;
    }

  public:
    /**
    * The function creates the table.
    * It must be called for construction.
    * @param field The field.
    * @param type The line type the table will be constructed for.
    */
    void create(const Field& field,LinesPercept::LineType type)
    {
      Temp* temp = new Temp[TEMPLATES_MAX];
      int i;
      double dist;
      for(i = 0; i < TEMPLATES_MAX; ++i)
        for(;;)
        {
          (Pose2D&) temp[i] = field.randomPose();
          dist = field.getDistance(temp[i],type);
          if(dist < 0)
            continue; // no line in that direction
          if(type == LinesPercept::skyblueGoal || type == LinesPercept::yellowGoal)
          {
            double distToBorder = field.getDistance(temp[i],LinesPercept::border);
            if(distToBorder > 0 && distToBorder < dist)
              continue; // goal line is hidden by border
          }
          temp[i].distance = (int) dist / 10;
          break; // everything all right -> leave loop
        }
      qsort(temp,TEMPLATES_MAX,sizeof(Temp),(int (*)(const void *,const void*)) compare);
      i = 0;
      int j = 0;
      distance[j] = i;
      while(i < TEMPLATES_MAX && j < DISTANCE_MAX)
      {
        while(i < TEMPLATES_MAX && j < DISTANCE_MAX && temp[i].distance == j)
        {
          char* t = templates[i];
          t[0] = char(temp[i].translation.x / 20);
          t[1] = char(temp[i].translation.y / 20);
          t[2] = char(temp[i].getAngle() * 127 / pi);
          ++i;
        }
        distance[++j] = i;
      }
      while(j < DISTANCE_MAX)
        distance[++j] = i;
      delete [] temp;
    }

    /**
    * The function draws a pose from the templates for a certain distance.
    * @param realDist The distance in which a line starting from the pose 
    *             should cut the closest field line.
    * @return A pose drawn from the table.
    */
    Pose2D sample(double realDist) const
    {
      realDist /= 10;
      int dist = (int) (realDist * (0.9 + 0.2 * random()));
      if(dist < 0)
        dist = 0;
      else if(dist >= DISTANCE_MAX)
        dist = DISTANCE_MAX - 1;
      int index = distance[dist] + int((distance[dist + 1] - distance[dist]) * random());
      const char* t = templates[index < TEMPLATES_MAX ? index : TEMPLATES_MAX - 1];
      ASSERT(t[1] < 68 && t[1] > -68);
      return Pose2D(t[2] * pi / 127,Vector2<double>(t[0] * 20,t[1] * 20));
    }

    void write(Out& stream) const 
    {
      stream.write(templates,sizeof(templates));
      stream.write(distance,sizeof(distance));
    }

    void read(In& stream) 
    {
      stream.read(templates,sizeof(templates));
      stream.read(distance,sizeof(distance));
    }
};

/**
* The class implements a lines-based Monte Carlo self-localization.
*/
class LinesTables2004
{
  protected:
    static Field field; /**< The field. */
    static ObservationTable<280,200,25>* observationTable; /**< The table maps points to closest edges. */
    static TemplateTable<50000>* templateTable; /**< The table contains candidate poses for certain distance measurements. */
    static int refCount; /**< A reference counter that ensures that the tables are only (de)allocated once. */

  public:
    /** 
    * Constructor.
    */
    LinesTables2004();

    /** 
    * Destructor.
    */
    ~LinesTables2004();
};

#endif// __LinesTables2004_h_

/*
* Change log :
* 
* $Log: LinesTables2004.h,v $
* Revision 1.5  2004/09/09 11:37:39  wachter
* - Fixed some more doxygen-errors
*
* Revision 1.4  2004/09/09 10:15:58  spranger
* fixed doxygen-errors
*
* Revision 1.3  2004/07/16 07:19:32  roefer
* LinesTables removed (it was replaced by LinesTables2004)
*
* Revision 1.2  2004/06/24 18:26:38  roefer
* Lines table redesign, should not influence the performance of GT2003SL
*
* Revision 1.1.1.1  2004/05/22 17:20:46  cvsadm
* created new repository GT2004_WM
*
* Revision 1.2  2004/03/08 01:09:34  roefer
* Use of LinesTables2004 restructured
*
* Revision 1.1  2003/10/06 14:10:14  cvsadm
* Created GT2004 (M.J.)
*
* 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.1  2003/05/22 07:53:05  roefer
* GT2003SelfLocator added
*
*/

---