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

# Modules/SelfLocator/SlamSelfLocator/SlamSelfLocator.h

Go to the documentation of this file.

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