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,int pointRes> 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 / pointRes + 0.5)));
          point[y][x][1] = char(sgn(p.y) * int(fabs(p.y / pointRes + 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] * pointRes,point[y][x][1] * pointRes);
    }
  
    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] * pointRes,
                 y2 = point[y][x][1] * pointRes;
          if(sqrt(sqr(x1 - x2) + sqr(y1 - y2)) > 40)
          {
            LINE(selfLocatorField,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(selfLocatorField,x2,y2,p2.translation.x,p2.translation.y,
              1, Drawings::ps_solid,
              Drawings::gray);
            LINE(selfLocatorField,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, int POINT_RES> 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 
    {
#ifdef HUGE_FIELD
      DISTANCE_MAX = 650 /**< The number of distances. */
#else
      DISTANCE_MAX = 500 /**< The number of distances. */
#endif
    };

    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 / POINT_RES);
          t[1] = char(temp[i].translation.y / POINT_RES);
          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];
      return Pose2D(t[2] * pi / 127,Vector2<double>(t[0] * POINT_RES,t[1] * POINT_RES));
    }

    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
{
  public:
    static Field field; /**< The field. */
#ifdef HUGE_FIELD
    typedef ObservationTable<320,240,25,25> ObsTable;
    typedef TemplateTable<65000, 25> TempTable;
#else
    typedef ObservationTable<280,200,25,20> ObsTable;
    typedef TemplateTable<50000, 20> TempTable;
#endif
    static ObsTable* observationTable; /**< The table maps points to closest edges. */
    static TempTable* 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. */

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

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

#endif// __LinesTables2004_h_

---