Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/SelfLocator/LinesTables2005.h

Go to the documentation of this file.

    00001 /**
    00002 * @file Modules/SelfLocator/LinesTables2005.h
    00003 *
    00004 * This file contains a class that represents the tables used for localization based on field lines.
    00005 *
    00006 * @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
    00007 */
    00008
    00009 #ifndef __LinesTables2005_h_
    00010 #define __LinesTables2005_h_
    00011
    00012 #include "Tools/Debugging/DebugDrawings.h"
    00013 #include "Tools/Streams/InOut.h"
    00014 #include "Tools/Field.h"
    00015 #include "Platform/GTAssert.h"
    00016
    00017 /**
    00018 * The class implements a lines-based Monte Carlo self-localization.
    00019 */
    00020 class LinesTables2005
    00021 {
    00022   public:
    00023     /**
    00024     * @class ObservationTableBase
    00025     * The class is a helper to support streaming of template class ObservationTable.
    00026     */
    00027     class ObservationTableBase
    00028     {
    00029       public:
    00030         /**
    00031         * The function writes the contents of an object to a stream.
    00032         * @param stream The stream the data is written to.
    00033         */
    00034         virtual void write(Out& stream) const = 0;
    00035
    00036         /**
    00037         * The function reads the contents of an object from a stream.
    00038         * @param stream The stream the data is read from.
    00039         */
    00040         virtual void read(In& stream) = 0;
    00041     };
    00042
    00043     /**
    00044     * @class ObservationTable
    00045     * The class realizes a table of closest distances to lines of a certain type.
    00046     */
    00047     template<int xSize,int ySize,int cellSize,int pointRes> class ObservationTable : public ObservationTableBase
    00048     {
    00049       private:
    00050         char point[4][ySize][xSize][2];
    00051         bool pointIsValid[4][ySize][xSize];
    00052
    00053
    00054       public:
    00055         void create(const Field& field,LinesPercept::LineType type);
    00056
    00057         Vector2<double> getClosestPoint(const Pose2D& v) const
    00058         {
    00059           double r = v.rotation / pi2 * 4 + 0.5;
    00060           if(r < 0)
    00061             r += 4;
    00062           int targetRot = int(r);
    00063           if (targetRot==4) targetRot=3;
    00064           ASSERT(targetRot >= 0 && targetRot < 4);
    00065           int x = int(v.translation.x / cellSize + xSize / 2),
    00066               y = int(v.translation.y / cellSize + ySize / 2);
    00067           if(x < 0 || x >= xSize || y < 0 || y >= ySize || !pointIsValid[targetRot][y][x])
    00068             return Vector2<double>(1e6,1e6);
    00069           else
    00070             return Vector2<double>(point[targetRot][y][x][0] * pointRes,point[targetRot][y][x][1] * pointRes);
    00071         }
    00072
    00073         void draw(double r)
    00074         {
    00075           NDECLARE_DEBUGDRAWING("linestables:observationTable","drawingOnField","displays the observation lines table");
    00076           r = r / pi2 * 4 + 0.5;
    00077           if(r < 0)
    00078             r += 4;
    00079           int targetRot = int(r);
    00080           ASSERT(targetRot >= 0 && targetRot < 4);
    00081           for(int y = 0; y < ySize; y += 5)
    00082             for(int x = 0; x < xSize; x += 5)
    00083               if (pointIsValid[targetRot][y][x])
    00084               {
    00085                 double x1 = (x + 0.5 - xSize / 2) * cellSize,
    00086                       y1 = (y + 0.5 - ySize / 2) * cellSize,
    00087                       x2 = point[targetRot][y][x][0] * pointRes,
    00088                       y2 = point[targetRot][y][x][1] * pointRes;
    00089                 //if(sqrt(sqr(x1 - x2) + sqr(y1 - y2)) > 40)
    00090                 {
    00091                   NLINE("linestables:observationTable",x1,y1,x2,y2,
    00092                     0, Drawings::ps_solid,
    00093                     Drawings::gray);
    00094                   Pose2D p(atan2(y2-y1,x2-x1),Vector2<double>(x2,y2)),
    00095                         p2 = p + Pose2D(Vector2<double>(-50,-50)),
    00096                         p3 = p + Pose2D(Vector2<double>(-50,50));
    00097                   NLINE("linestables:observationTable",x2,y2,p2.translation.x,p2.translation.y,
    00098                     1, Drawings::ps_solid,
    00099                     Drawings::gray);
    00100                   NLINE("linestables:observationTable",x2,y2,p3.translation.x,p3.translation.y,
    00101                     1, Drawings::ps_solid,
    00102                     Drawings::gray);
    00103                 }
    00104               }
    00105         }
    00106
    00107         void write(Out& stream) const
    00108         {
    00109           stream.write(point,sizeof(point));
    00110           stream.write(pointIsValid,sizeof(pointIsValid));
    00111         }
    00112
    00113         void read(In& stream)
    00114         {
    00115           stream.read(point,sizeof(point));
    00116           stream.read(pointIsValid,sizeof(pointIsValid));
    00117         }
    00118       };
    00119
    00120     /**
    00121     * @class TemplateTableBase
    00122     * The class is a helper to support streaming of template class TemplateTable.
    00123     */
    00124     class TemplateTableBase
    00125     {
    00126       public:
    00127         /**
    00128         * The function writes the contents of an object to a stream.
    00129         * @param stream The stream the data is written to.
    00130         */
    00131         virtual void write(Out& stream) const = 0;
    00132
    00133         /**
    00134         * The function reads the contents of an object from a stream.
    00135         * @param stream The stream the data is read from.
    00136         */
    00137         virtual void read(In& stream) = 0;
    00138     };
    00139
    00140     /**
    00141     * The class realizes a table of template poses.
    00142     */
    00143     template<int TEMPLATES_MAX, int POINT_RES> class TemplateTable : public TemplateTableBase
    00144     {
    00145       private:
    00146         /**
    00147         * The class is required for sorting the template table by the distances to lines.
    00148         */
    00149         class Temp : public Pose2D
    00150         {
    00151           public:
    00152             int distance; /**< The distance to the closest line in direction of the pose. */
    00153         };
    00154
    00155         enum
    00156         {
    00157     #ifdef HUGE_FIELD
    00158           DISTANCE_MAX = 650 /**< The number of distances. */
    00159     #else
    00160           DISTANCE_MAX = 500 /**< The number of distances. */
    00161     #endif
    00162         };
    00163
    00164         char templates[TEMPLATES_MAX][3]; /**< The template poses sorted by distance. */
    00165         unsigned short distance[DISTANCE_MAX + 1]; /**< Indices into the template table depending on the distance. */
    00166
    00167         /**
    00168         * The function is required for sorting the template poses.
    00169         * It is used as compare function for qsort.
    00170         * @param t1 The first element to compare.
    00171         * @param t2 The second element to compare.
    00172         * @return -1, 0, or 1 according to the specification required by qsort.
    00173         */
    00174         static int compare(const Temp* t1,const Temp* t2)
    00175         {
    00176           return t1->distance == t2->distance ? 0 : t1->distance < t2->distance ? -1 : 1;
    00177         }
    00178
    00179       public:
    00180         /**
    00181         * The function creates the table.
    00182         * It must be called for construction.
    00183         * @param field The field.
    00184         * @param type The line type the table will be constructed for.
    00185         */
    00186         void create(const Field& field,LinesPercept::LineType type)
    00187         {
    00188           Temp* temp = new Temp[TEMPLATES_MAX];
    00189           int i;
    00190           double dist;
    00191           for(i = 0; i < TEMPLATES_MAX; ++i)
    00192             for(;;)
    00193             {
    00194               (Pose2D&) temp[i] = field.randomPose();
    00195               dist = field.getDistance(temp[i],type);
    00196               if(dist < 0)
    00197                 continue; // no line in that direction
    00198               /*if(type == LinesPercept::skyblueGoal || type == LinesPercept::yellowGoal)
    00199               {
    00200                 double distToBorder = field.getDistance(temp[i],LinesPercept::border);
    00201                 if(distToBorder > 0 && distToBorder < dist)
    00202                   continue; // goal line is hidden by border
    00203               }*/
    00204               temp[i].distance = (int) dist / 10;
    00205               break; // everything all right -> leave loop
    00206             }
    00207           qsort(temp,TEMPLATES_MAX,sizeof(Temp),(int (*)(const void *,const void*)) compare);
    00208           i = 0;
    00209           int j = 0;
    00210           distance[j] = i;
    00211           while(i < TEMPLATES_MAX && j < DISTANCE_MAX)
    00212           {
    00213             while(i < TEMPLATES_MAX && j < DISTANCE_MAX && temp[i].distance == j)
    00214             {
    00215               char* t = templates[i];
    00216               t[0] = char(temp[i].translation.x / POINT_RES);
    00217               t[1] = char(temp[i].translation.y / POINT_RES);
    00218               t[2] = char(temp[i].getAngle() * 127 / pi);
    00219               ++i;
    00220             }
    00221             distance[++j] = i;
    00222           }
    00223           while(j < DISTANCE_MAX)
    00224             distance[++j] = i;
    00225           delete [] temp;
    00226         }
    00227
    00228         /**
    00229         * The function draws a pose from the templates for a certain distance.
    00230         * @param realDist The distance in which a line starting from the pose should cut the closest field line.
    00231         * @return A pose drawn from the table.
    00232         */
    00233         Pose2D sample(double realDist) const
    00234         {
    00235           realDist /= 10;
    00236           int dist = (int) (realDist * (0.9 + 0.2 * random()));
    00237           if(dist < 0)
    00238             dist = 0;
    00239           else if(dist >= DISTANCE_MAX)
    00240             dist = DISTANCE_MAX - 1;
    00241           int index = distance[dist] + int((distance[dist + 1] - distance[dist]) * random());
    00242           const char* t = templates[index < TEMPLATES_MAX ? index : TEMPLATES_MAX - 1];
    00243           return Pose2D(t[2] * pi / 127,Vector2<double>(t[0] * POINT_RES,t[1] * POINT_RES));
    00244         }
    00245
    00246         void write(Out& stream) const
    00247         {
    00248           stream.write(templates,sizeof(templates));
    00249           stream.write(distance,sizeof(distance));
    00250         }
    00251
    00252         void read(In& stream)
    00253         {
    00254           stream.read(templates,sizeof(templates));
    00255           stream.read(distance,sizeof(distance));
    00256         }
    00257     };
    00258
    00259     static Field field; /**< The field. */
    00260     typedef ObservationTable<320,240,25,25> ObsTable;
    00261 //  typedef ObservationTable<800,600,10,10> ObsTable;
    00262     typedef TemplateTable<65000, 25> TempTable;
    00263     static ObsTable* observationTable; /**< The table maps points to closest edges. */
    00264     static TempTable* templateTable; /**< The table contains candidate poses for certain distance measurements. */
    00265     static int refCount; /**< A reference counter that ensures that the tables are only (de)allocated once. */
    00266
    00267     /**
    00268     * Constructor.
    00269     */
    00270     LinesTables2005();
    00271
    00272     /**
    00273     * Destructor.
    00274     */
    00275     ~LinesTables2005();
    00276 };
    00277
    00278 inline Out& operator<<(Out& stream,const LinesTables2005::ObservationTableBase& table)
    00279 {
    00280   table.write(stream);
    00281   return stream;
    00282 }
    00283
    00284 inline In& operator>>(In& stream,LinesTables2005::ObservationTableBase& table)
    00285 {
    00286   table.read(stream);
    00287   return stream;
    00288 }
    00289
    00290 /**
    00291 * The operator writes the template table into a stream.
    00292 * @param stream The stream the table is written into.
    00293 * @param table The table to be written.
    00294 * @return The stream.
    00295 */
    00296 inline Out& operator<<(Out& stream,const LinesTables2005::TemplateTableBase& table)
    00297 {
    00298   table.write(stream);
    00299   return stream;
    00300 }
    00301
    00302 /**
    00303 * The operator reads the template table from a stream.
    00304 * @param stream The stream the table is read from.
    00305 * @param table The table to be read.
    00306 * @return The stream.
    00307 */
    00308 inline In& operator>>(In& stream, LinesTables2005::TemplateTableBase& table)
    00309 {
    00310   table.read(stream);
    00311   return stream;
    00312 }
    00313
    00314 #endif// __LinesTables2005_h_