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

# Modules/SelfLocator/GT2005SelfLocator/GT2005SelfLocator.h

Go to the documentation of this file.

    00001 /**
    00002 * @file Modules/SelfLocator/GT2005SelfLocator.h
    00003 *
    00004 * This file contains a class for self-localization based on the Monte Carlo approach
    00005 * using goals, landmarks, line crossings, field lines and the center circle
    00006 *
    00007 *    @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
    00008 *    @author <A href=mailto:c_rohde@web.de>Carsten Rohde</A>
    00009 *    @author <A href=mailto:judith-winter@web.de>Judith Winter</A>
    00010 *
    00011 */
    00012
    00013 #ifndef __GT2005SelfLocator_h_
    00014 #define __GT2005SelfLocator_h_
    00015
    00016 #include "../SelfLocator.h"
    00017 #include "../LinesTables2005.h"
    00018 #include "GT2005SelfLocatorSample.h"
    00019 #include "GT2005SampleTemplateGenerator.h"
    00020 #include "GT2005LineCrossingsTable.h"
    00021 #include "GT2005SelfLocatorParameters.h"
    00022 #include "Tools/Debugging/GenericDebugData.h"
    00023 #include "Tools/ColorClasses.h"
    00024 #include "Tools/Math/Geometry.h"
    00025 #include "Tools/TRingBufferWithSum.h"
    00026
    00027 class GT2005SelfLocator : public SelfLocator, public LinesTables2005
    00028 {
    00029   public:
    00030     /*------------------------------ public methods -----------------------------------*/
    00031
    00032     /**
    00033     * Constructor.
    00034     * @param interfaces The paramters of the SelfLocator module.
    00035     */
    00036     GT2005SelfLocator(const SelfLocatorInterfaces& interfaces);
    00037
    00038     /**
    00039     * The function executes the module.
    00040     */
    00041     virtual void execute();
    00042     virtual bool handleMessage(InMessage& message);
    00043
    00044
    00045   private:
    00046
    00047   /* ------------------------- private classes here --------------------- */
    00048
    00049     /**
    00050     * The class represents a cell in a cube that is used to determine the largest sample cluster.
    00051     */
    00052     class Cell
    00053     {
    00054       public:
    00055         int count;                  /**< The number of samples in this cube. */
    00056         GT2005SelfLocatorSample* first;       /**< The first sample in this cube. */
    00057
    00058       /**
    00059       * Constructor.
    00060       * preinititalize first with 0 to enshure correct end of queue detection
    00061       */
    00062       Cell() {count = 0; first = 0;}
    00063     };
    00064
    00065
    00066 /* ----------------------------------- private constants here ----------------------------------*/
    00067
    00068
    00069     enum FlagSides{
    00070       LEFT_SIDE_OF_FLAG = 1,                    /**< flag indicating a position on the left side of a flag */
    00071       RIGHT_SIDE_OF_FLAG = -1                   /**< flag indicating a position on the right side of a flag */
    00072     };
    00073
    00074     enum{
    00075       SAMPLES_MAX = 100,                          /**< maximum number of samples */
    00076       POSE_SPACE_GRID = 10,                       /**< quantization of the pose space */
    00077
    00078    /** in the original 04 observation table LinesPercept::field is a x directed line and
    00079         LinesPercept::numberOfLineTypes is a y directed line */
    00080        OBS_TABLE_X_FIELD_LINE = LinesPercept::field,
    00081        OBS_TABLE_Y_FIELD_LINE = LinesPercept::numberOfLineTypes,
    00082
    00083        //HEAD_HEIGHT_SMOOTHING_FRAMENUMBER = 5,
    00084
    00085        NUM_OF_CALCULATED_POSES = 3
    00086     };
    00087
    00088     static double const QUASI_ZERO;
    00089
    00090 /* ----------------------------------- private variables here ----------------------------------*/
    00091
    00092     SampleSet<GT2005SelfLocatorSample, SAMPLES_MAX> sampleSet;    /**< the sampleSet */
    00093     bool teamColorBlue;
    00094
    00095     //double headHeight;             // is calculated dynamically
    00096
    00097     double linePointZAngleMotionDependentVariance,
    00098           /*linePointYAngleMotionDependentVariance,
    00099
    00100           crossingZAngleMotionDependentVariance,
    00101           crossingYAngleMotionDependentVariance,
    00102
    00103           centerCircleZAngleMotionDependentVariance,
    00104           centerCircleYAngleMotionDependentVariance,
    00105           centerCircleOrientationAngleMotionDependentVariance,
    00106
    00107           flagZAngleMotionDependentVariance,
    00108           flagYAngleMotionDependentVariance,
    00109
    00110           goalZAngleMotionDependentVariance,
    00111           goalYAngleMotionDependentVariance,*/
    00112
    00113           linePointZAngleTrust,
    00114           linePointYAngleTrust,
    00115
    00116           crossingZAngleTrust,
    00117           crossingYAngleTrust,
    00118
    00119           centerCircleZAngleTrust,
    00120           centerCircleYAngleTrust,
    00121           centerCircleOrientationAngleTrust,
    00122
    00123           //flagZAngleTrust,
    00124           flagYAngleTrust,
    00125
    00126           goalZAngleTrust,
    00127           goalYAngleTrust;
    00128
    00129     Pose2D lastOdometry, lastOdometry2;                       /**< the last Odometry, used to calculate the delta_odometry */
    00130     unsigned timeStamp;                                       /**< timestap */
    00131     bool observationUpdateDone;                               /**< a flag indicating if an observation update was done during the current run of execute */
    00132     double speed;                                             /**< current robot speed */
    00133
    00134     double averagePerceptTypeProb[GT2005SelfLocatorSample::numberOfPerceptTypes]; /**< the average probability for each PerceptType and all Samples*/
    00135
    00136     GT2005SampleTemplateGenerator sampleTemplateGenerator;                        /**< this class encapsules template generation */
    00137
    00138     GT2005LineCrossingsTable lineCrossingsTable;
    00139
    00140     //TRingBufferWithSum<double,HEAD_HEIGHT_SMOOTHING_FRAMENUMBER> headHeightBuffer;
    00141
    00142     /** testing and debugging stuff */
    00143     GT2005SelfLocatorSample* testSample;
    00144     int testSampleIndex;
    00145     Vector2<double> lastModelCrossing, lastSeenCrossing;
    00146     GT2005LineCrossingsTable::CrossingClass lastSeenCrossingClass;
    00147     Pose2D lastSeenCenterCircle;
    00148
    00149     GT2005SelfLocatorParameters parameters;
    00150
    00151     /** eigenvectors of particle distribution covariance matrix */
    00152     Vector2<double> eigenVec0, eigenVec1;
    00153
    00154     double varianceCovarianceMatrix[2][2];
    00155     double stdDevRot;
    00156     double averageX;
    00157     double averageY;
    00158     double eigenVal0, eigenVal1, eigenSwap;
    00159     double averageRot;
    00160
    00161
    00162 /* ----------------------------------- private methods here ------------------------------------*/
    00163
    00164     /**
    00165     * The function distributes the parameter in a Gaussian way.
    00166     * @param d A value that should be distributed.
    00167     * @return A transformation of d according to a Gaussian curve.
    00168     */
    00169     double gaussian(double d) const {return exp(- d * d);}
    00170
    00171     /**
    00172     * The function updates the samples by the odometry offset.
    00173     * @param odometry The motion since the last call to this function.
    00174     * @param camera The camera offset.
    00175     * @param noise Dermines whether some additional noise is added to the sample poses.
    00176     *
    00177     * enhance maybe by replacing bool noise by a noisefunction?
    00178     */
    00179     void motionUpdate(const Pose2D& odometry,const Pose2D& camera,bool noise);
    00180
    00181
    00182     /**
    00183     * The function updates the samples by a single edge of a flag recognized.
    00184     * @param flagFieldPosition The position of the flag.
    00185     * @param sideOfFlag The side of the flag that was seen.
    00186     * @param measuredBearing The bearing, under which the edge was seen.
    00187     */
    00188     void updateByFlag(const Vector2<double>& flagFieldPosition,
    00189             FlagSides sideOfFlag,
    00190             double measuredBearing);
    00191
    00192     /**
    00193     * The function updates the samples by a single goal post recognized.
    00194     * @param goalPost The position of the goal post.
    00195     * @param measuredBearing The bearing, under which the goal post was seen.
    00196     */
    00197     void updateByGoalPost(const Vector2<double>& goalPost,
    00198               double measuredBearing);
    00199
    00200     /**
    00201     * The function updates the samples by the recognized landmarks (goal, flag..)
    00202     * triggers updateByFlag and updateByGoalPost
    00203     * @param landmarksPercept The landmarksPercept
    00204     */
    00205     void landmarksObservationUpdate(const LandmarksPercept& landmarksPercept);
    00206
    00207
    00208     /**calculates the CrossingClass out of the classification (side1..side4) in the LinesPercept
    00209     */
    00210     GT2005LineCrossingsTable::CrossingClass getCrossingClassification(const LinesPercept::LineCrossingPoint& point);
    00211
    00212     /** observationUpdate by the center circle
    00213     */
    00214     void updateByCenterCircle(const LinesPercept::CenterCircle& centerCircle);
    00215
    00216     /** observationUpdate using the linecrossing percept
    00217     *   @param relative position of the linecrossing
    00218     */
    00219     void updateByCrossing(const LinesPercept::LineCrossingPoint& point);
    00220     /**
    00221     * does an observationupdate with a point on the field (line or goal)
    00222     * @param point a point on a line
    00223     * @param type the type of the line
    00224     */
    00225     void updateByPoint(const LinesPercept::LinePoint& point,GT2005SelfLocatorSample::PerceptType type, LinesPercept::LineType observationTableIndex);
    00226
    00227     /**
    00228     * The function updates the samples by recognized lines
    00229     * @param linesPercept The LinesPercept
    00230     */
    00231     void lineObservationUpdate(const LinesPercept& linesPercept);
    00232
    00233     /**
    00234     * Calculates the average possibility for each perceptType and all samples
    00235     * and returns the product over all averages.
    00236     * contains the part of the gt04 resampling method that calculates
    00237     * @return product over all average percepttype-probabilities
    00238     */
    00239     double calcAveragePerceptTypeProbabilities();
    00240
    00241     /**
    00242     * redistibution of the sampleSet according to the probabilities of the samples
    00243     */
    00244     void resample();
    00245
    00246
    00247     /**
    00248     * Calculates the localization validity by Standard Deviation
    00249     */
    00250     double calcDistributionValidityByStandardDeviation();
    00251
    00252     /**
    00253     * Calculates the localization validity by Entropy
    00254     */
    00255     double calcDistributionValidityByEntropy();
    00256
    00257     /**
    00258     * The function calculates a Pose by averaging the sample poses in a sub cube of
    00259     * the pose-room
    00260     */
    00261     RobotPose calcPoseFromSubCube(Cell poseSpace[POSE_SPACE_GRID][POSE_SPACE_GRID][POSE_SPACE_GRID], int x, int y, int r);
    00262
    00263     /**
    00264     * The function determines the most probable pose from the sample distribution.
    00265     * @param pose The pose is returned to this variable.
    00266     * @param validity The validity of the pose is returned to this variable.
    00267     */
    00268     void calcPose(Pose2D& pose,double& validity);
    00269
    00270
    00271     // Debug drawings
    00272
    00273     /**
    00274     * The function draws the current sample set to a debug drawing.
    00275     */
    00276     void draw() const;
    00277
    00278     /**
    00279     * Returns a color corresponding to the sample probability.
    00280     */
    00281     Drawings::Color getSampleColor(double probability) const;
    00282
    00283     /**
    00284     * The function draws an arrow to the sample set drawing.
    00285     * @param pose The position and direction of the arrow.
    00286     * @param color The color of the arrow.
    00287     */
    00288     void draw(const Pose2D& pose,Drawings::Color color) const;
    00289
    00290     /**
    00291     * The function draws an arrow to the test sample drawing.
    00292     * @param pose The position and direction of the arrow.
    00293     * @param color The color of the arrow.
    00294     */
    00295     void drawTestSample(const Pose2D& pose,Drawings::Color color) const;
    00296
    00297     /**
    00298     * The function calculates the variances and trust values
    00299     * from the current speed
    00300     */
    00301     void updateVariancesBySpeed(double speed);
    00302
    00303     /**
    00304     * The function calculates the trust values from variance and
    00305     * weight parameters
    00306     */
    00307     void calculateTrust();
    00308
    00309     /**
    00310     * Reset all samples to random position
    00311     */
    00312     void resetSamples();
    00313
    00314     /**
    00315     * Reset all samples to a given position
    00316     */
    00317     void resetSamples(const Pose2D &pose);
    00318
    00319     /**
    00320     * Reset all samples when robot is penalized
    00321     * 25% samples are placed on each side of the field at the center line respectively
    00322     * 50% at random positions
    00323     */
    00324     void resetSamplesWhenPenalized();
    00325 };
    00326
    00327 #endif //GT2005SelfLocator.h