Modules/SelfLocator/LinesTables2005.h

Go to the documentation of this file.

/**
* @file Modules/SelfLocator/LinesTables2005.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 __LinesTables2005_h_
#define __LinesTables2005_h_

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

/**
* The class implements a lines-based Monte Carlo self-localization.
*/
class LinesTables2005
{
  public:
    /**
    * @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[4][ySize][xSize][2];
        bool pointIsValid[4][ySize][xSize];


      public:
        void create(const Field& field,LinesPercept::LineType type);
      
        Vector2<double> getClosestPoint(const Pose2D& v) const
        {
          double r = v.rotation / pi2 * 4 + 0.5;
          if(r < 0)
            r += 4;
          int targetRot = int(r);
          if (targetRot==4) targetRot=3;
          ASSERT(targetRot >= 0 && targetRot < 4);
          int x = int(v.translation.x / cellSize + xSize / 2),
              y = int(v.translation.y / cellSize + ySize / 2);
          if(x < 0 || x >= xSize || y < 0 || y >= ySize || !pointIsValid[targetRot][y][x])
            return Vector2<double>(1e6,1e6);
          else
            return Vector2<double>(point[targetRot][y][x][0] * pointRes,point[targetRot][y][x][1] * pointRes);
        }
      
        void draw(double r)
        {
          NDECLARE_DEBUGDRAWING("linestables:observationTable","drawingOnField","displays the observation lines table");
          r = r / pi2 * 4 + 0.5;
          if(r < 0)
            r += 4;
          int targetRot = int(r);
          ASSERT(targetRot >= 0 && targetRot < 4);
          for(int y = 0; y < ySize; y += 5)
            for(int x = 0; x < xSize; x += 5)
              if (pointIsValid[targetRot][y][x])
              {
                double x1 = (x + 0.5 - xSize / 2) * cellSize,
                      y1 = (y + 0.5 - ySize / 2) * cellSize,
                      x2 = point[targetRot][y][x][0] * pointRes,
                      y2 = point[targetRot][y][x][1] * pointRes;
                //if(sqrt(sqr(x1 - x2) + sqr(y1 - y2)) > 40)
                {
                  NLINE("linestables:observationTable",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));
                  NLINE("linestables:observationTable",x2,y2,p2.translation.x,p2.translation.y,
                    1, Drawings::ps_solid,
                    Drawings::gray);
                  NLINE("linestables:observationTable",x2,y2,p3.translation.x,p3.translation.y,
                    1, Drawings::ps_solid,
                    Drawings::gray);
                }
              }
        }

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

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

    /**
    * @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 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));
        }
    };

    static Field field; /**< The field. */
    typedef ObservationTable<320,240,25,25> ObsTable;
//  typedef ObservationTable<800,600,10,10> ObsTable;
    typedef TemplateTable<65000, 25> TempTable;
    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.
    */
    LinesTables2005();

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

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

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

/**
* 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 LinesTables2005::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, LinesTables2005::TemplateTableBase& table)
{
  table.read(stream);
  return stream;
}

#endif// __LinesTables2005_h_

---