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

# Modules/SelfLocator/GT2004SelfLocator.h

Go to the documentation of this file.

    00001 /**
    00002 * @file Modules/SelfLocator/GT2004SelfLocator.h
    00003 *
    00004 * This file contains a class for self-localization based on the Monte Carlo approach
    00005 * using goals, landmarks, and field lines.
    00006 *
    00007 * @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
    00008 */
    00009
    00010 #ifndef __GT2004SelfLocator_h_
    00011 #define __GT2004SelfLocator_h_
    00012
    00013 #include "SelfLocator.h"
    00014 #include "LinesTables2004.h"
    00015
    00016 /**
    00017 * The class implements a lines-based Monte Carlo self-localization.
    00018 */
    00019 class GT2004SelfLocator : public SelfLocator, public LinesTables2004
    00020 {
    00021   private:
    00022     /**
    00023     * The class represents a sample.
    00024     */
    00025     class Sample : public PoseSample
    00026     {
    00027       public:
    00028         enum
    00029         {
    00030           numberOfQualities = LinesPercept::numberOfLineTypes + 1
    00031         };
    00032         Pose2D camera; /**< Temporary representing the pose of the camera. */
    00033         double quality[numberOfQualities]; /**< The quality of the sample, i.e. a lowpass filtered probability. */
    00034         Sample* next; /**< The next sample in the cell cube. */
    00035
    00036         /**
    00037         * Constructor.
    00038         */
    00039         Sample();
    00040
    00041         /**
    00042         * Constructor.
    00043         * @param pose The pose of the sample.
    00044         * @param quality The quality of the sample.
    00045         */
    00046         Sample(const Pose2D& pose,const double* quality);
    00047
    00048         double getQuality() const {return probability;}
    00049
    00050         void updateQuality(const double* average);
    00051
    00052         void setProbability(LinesPercept::LineType type,double value);
    00053
    00054         bool isValid() const {return probability != 2;}
    00055     };
    00056
    00057     /**
    00058     * The class represents a cell in a cube that is used to determine the largest sample cluster.
    00059     */
    00060     class Cell
    00061     {
    00062     public:
    00063       int count; /**< The number of samples in this cube. */
    00064       Sample* first; /**< The first sample in this cube. */
    00065
    00066       /**
    00067
    00068       * Constructor.
    00069       */
    00070       Cell() {count = 0; first = 0;}
    00071     };
    00072
    00073
    00074     enum
    00075     {
    00076       SAMPLES_MAX = 100, /**< The number of samples. */
    00077       GRID_MAX = 10, /**< The number of cells in one dimension. */
    00078       FLAGS_MAX = 3 /**< The size of the flag buffer. */
    00079     };
    00080
    00081     enum FlagSides
    00082     {
    00083       LEFT_SIDE_OF_FLAG = 1, /**< A marker for left edges of flags. */
    00084       RIGHT_SIDE_OF_FLAG = -1 /**< A marker for right edges of flags. */
    00085     };
    00086
    00087     SampleSet<Sample, SAMPLES_MAX> sampleSet; /**< The sample set. */
    00088     Pose2D lastOdometry, /**< The state of the odometry at the previous call of this module. */
    00089            lastOdometry2,
    00090            templates[SAMPLES_MAX]; /**< Templates for poses replacing bad samples. */
    00091     Flag flags[FLAGS_MAX]; /**< A buffer for previously seen flags. */
    00092     int numOfFlags, /**< The number of flags in the buffer. */
    00093         numOfTemplates, /**< The number of templates generated. */
    00094         nextTemplate; /**< The next template delivered. */
    00095     int randomFactor; /**< A factor that is increased if more templates are required. */
    00096     LinesPercept::LineType types[LinesPercept::numberOfLineTypes];
    00097     int numberOfTypes;
    00098     bool sensorUpdated; /**< Did any update of the samples by a sensor reading happen? */
    00099     double average[Sample::numberOfQualities];
    00100     unsigned timeStamp;
    00101     double speed;
    00102
    00103     /**
    00104     * The function distributes the parameter in a Gaussian way.
    00105     * @param d A value that should be distributed.
    00106     * @return A transformation of d according to a Gaussian curve.
    00107     */
    00108     double sigmoid(double d) const {return exp(- d * d);}
    00109
    00110     /**
    00111     * The function updates the samples by the odometry offset.
    00112     * @param odometry The motion since the last call to this function.
    00113     * @param camera The camera offset.
    00114     * @param noise Dermines whether some additional noise is added to the sample poses.
    00115     */
    00116     void updateByOdometry(const Pose2D& odometry,const Pose2D& camera,bool noise);
    00117
    00118     /**
    00119     * The function updates the samples by a single line point recognized.
    00120     * @param point The relative offset of the point.
    00121     * @param type The type of the point.
    00122     */
    00123     void updateByPoint(const LinesPercept::LinePoint& point,LinesPercept::LineType type);
    00124
    00125     /**
    00126     * The function updates the samples by a single edge of a flag recognized.
    00127     * @param flag The position of the flag.
    00128     * @param sideOfFlag The side of the flag that was seen.
    00129     * @param measuredBearing The bearing, under which the edge was seen.
    00130     */
    00131     void updateByFlag(const Vector2<double>& flag,
    00132                       FlagSides sideOfFlag,
    00133                       double measuredBearing);
    00134
    00135     /**
    00136     * The function updates the samples by a single goal post recognized.
    00137     * @param goalPost The position of the goal post.
    00138     * @param measuredBearing The bearing, under which the goal post was seen.
    00139     */
    00140     void updateByGoalPost(const Vector2<double>& goalPost,
    00141                           double measuredBearing);
    00142
    00143     /**
    00144     * The function re-distributes the samples according to their probabilities.
    00145     */
    00146     void resample();
    00147
    00148     /**
    00149     * The function determines the most probable pose from the sample distribution.
    00150     * @param pose The pose is returned to this variable.
    00151     * @param validity The validity of the pose is returned to this variable.
    00152     */
    00153     void calcPose(Pose2D& pose,double& validity);
    00154
    00155     /**
    00156     * The function adds a flag to the buffer.
    00157     * @param flag The new flag.
    00158     */
    00159     void addFlag(const Flag& flag);
    00160
    00161     /**
    00162     * The function calculates the current pose of the robot from three bearings.
    00163     * @param dir0 The bearing on the first landmark.
    00164     * @param dir1 The bearing on the second landmark.
    00165     * @param dir2 The bearing on the third landmark.
    00166     * @param mark0 The position of the first landmark.
    00167     * @param mark1 The position of the second landmark.
    00168     * @param mark2 The position of the third landmark.
    00169     * @param cameraOffset The offset of the camera relative to the body center.
    00170     * @param resultingPose The calculated pose is returned to this variable.
    00171     * @return Was the function successful?
    00172     */
    00173     bool poseFromBearings(double dir0,double dir1,double dir2,
    00174                           const Vector2<double>& mark0,
    00175                           const Vector2<double>& mark1,
    00176                           const Vector2<double>& mark2,
    00177                           const Vector2<double>& cameraOffset,
    00178                           Pose2D& resultingPose) const;
    00179
    00180     /**
    00181     * The function calculates the up to two current poses of the robot from two bearings and a distance.
    00182     * @param dir0 The bearing on the first landmark.
    00183     * @param dir1 The bearing on the second landmark.
    00184     * @param dist The distance of the first landmark.
    00185     * @param mark0 The position of the first landmark.
    00186     * @param mark1 The position of the second landmark.
    00187     * @param cameraOffset The offset of the camera relative to the body center.
    00188     * @param resultingPose1 One calculated pose is returned to this variable.
    00189     * @param resultingPose2 A second calculated pose is returned to this variable.
    00190     * @return The number of poses calculated?
    00191     */
    00192     int poseFromBearingsAndDistance(double dir0,double dir1,
    00193                                     double dist,
    00194                                     const Vector2<double>& mark0,
    00195                                     const Vector2<double>& mark1,
    00196                                     const Vector2<double>& cameraOffset,
    00197                                     Pose2D& resultingPose1,
    00198                                     Pose2D& resultingPose2) const;
    00199
    00200     /**
    00201     * The function determines a pair of landmark positions and bearings from a landmarks percept.
    00202     * @param landmarksPercept The landmarks percept.
    00203     * @param i The index of the entry in the percept.
    00204     * @param mark The position of the mark is returned here.
    00205     * @param dir The bearing on the mark is returned to this variable.
    00206     * @param dist The distance of the mark is returned to this variable.
    00207     * @return returns true if getting the bearing was successful.
    00208     */
    00209     bool getBearing(const LandmarksPercept& landmarksPercept,int i,
    00210                     Vector2<double>& mark,
    00211                     double& dir,double& dist) const;
    00212
    00213     /**
    00214     * The function generates pose templates from a landmark percept.
    00215     * The pose templates can be used to initialize new samples.
    00216     * @param landmarksPercept The landmarks percept.
    00217     * @param odometry The odometry offset since the last call of this function.
    00218     */
    00219     void generatePoseTemplates(const LandmarksPercept& landmarksPercept,
    00220                                const Pose2D& odometry);
    00221
    00222     /**
    00223     * The function returns the next pose template or a random one if no templates were determined.
    00224     * @return A new sample.
    00225     */
    00226     Sample getTemplate();
    00227
    00228     /**
    00229     * The function creates a random sample inside the field boundaries.
    00230     * @return The random sample.
    00231     */
    00232     //Sample getTemplate() const;
    00233
    00234     /**
    00235     * The function draws an arrow to a debug drawing.
    00236     * @param pose The position and direction of the arrow.
    00237     * @param color The color of the arrow.
    00238     */
    00239     void draw(const Sample& sample) const;
    00240
    00241     /**
    00242     * The function draws a point of a line percept.
    00243     * @param point The relative position in field coordinates.
    00244     * @param type The line type of the point.
    00245     */
    00246     void draw(const Vector2<int>& point,LinesPercept::LineType type) const;
    00247
    00248   /**
    00249     * The function draws an ellipse representing the localization covariance
    00250   */
    00251   void drawEllipse();
    00252
    00253   public:
    00254     /**
    00255     * Constructor.
    00256     * @param interfaces The paramters of the SelfLocator module.
    00257     */
    00258     GT2004SelfLocator(const SelfLocatorInterfaces& interfaces);
    00259
    00260     /**
    00261      * The function executes the module.
    00262      */
    00263     virtual void execute();
    00264     virtual bool handleMessage(InMessage& message);
    00265
    00266     static double paramUp, // step size for increasing qualities
    00267                   paramDown, // step size for decreasing qualities
    00268                   paramDelay, // factor
    00269                   paramHeight, // hypothetic height of head
    00270                   paramZ, // sharpness of Gauss for rotational errors
    00271                   paramY; // sharpness of Gauss for translational errors
    00272 };
    00273
    00274 #endif// __GT2004SelfLocator_h_