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

# Modules/BallLocator/GT2005BallLocator.h

Go to the documentation of this file.

    00001 /**
    00002  * @file GT2005BallLocator.h
    00003  * Contains a BallLocator implementation using Particle Filters
    00004  *
    00005  * @author <a href="mailto:christinez@gmx.de">Christine Zarges</a>
    00006  * @author <a href="mailto:Thorsten.Kerkhof@gmx.de">Thorsten Kerkhof</a>
    00007  */
    00008 //---------------------------------------------------------------------------------------
    00009 #ifndef GT2005BallLocator_H_INCLUDED
    00010 #define GT2005BallLocator_H_INCLUDED
    00011 //---------------------------------------------------------------------------------------
    00012 #include "BallLocator.h"
    00013 #include "Tools/Debugging/GenericDebugData.h"
    00014 #include <vector>
    00015 #include "Tools/Math/Matrix2x2.h"
    00016 #include "GT2005Particles/GT2005Particle.h"
    00017 #include "GT2005Particles/GT2005ParticleContainer.h"
    00018 #include "GT2005BallLocatorParameters.h"
    00019 //---------------------------------------------------------------------------------------
    00020 /**
    00021 * @class GT2005BallLocator
    00022 * A BallLocator using a particle filter.
    00023 */
    00024 class GT2005BallLocator : public BallLocator
    00025 {
    00026 public:
    00027   GT2005BallLocator(const BallLocatorInterfaces& interfaces);
    00028
    00029   //Executes the GT2005BallLocator module:
    00030   virtual void execute();
    00031
    00032   /**
    00033   * Handles debug messages
    00034   * @param message The message
    00035   * @return true if message was handled
    00036   */
    00037   virtual bool handleMessage(InMessage& message);
    00038
    00039   //Calls MODIFY macro of RC2 for some paramters:
    00040   void modify();
    00041
    00042   /* MeasurementUpdate-functions to update position and velocity seperately.
    00043    * First the positions are updated and the estimated ball position is calculated
    00044    * and this information is used to calculate the velocities.
    00045    * @param ballOffset The relative position of the percept.
    00046    * @param reliability The reliability of the percept.
    00047    */
    00048   void measurementUpdate(Vector2<double> ballOffset,
    00049                          double reliability,
    00050                          double distanceValue,
    00051                          double panningVelocityValue,
    00052                          double robotSpeedValue);
    00053
    00054
    00055   //Sets the state of the ball in the BallSymbols:
    00056   void setBallSymbols();
    00057
    00058   /* The function draws an particle as an arrow.
    00059    * @param particle The particle to draw
    00060    *   -> the position of the particle is the position to draw the arrow
    00061    *   -> the velocity gives the orientation and length of the arrow
    00062    *   -> the probability gives the thickness of the arrow
    00063    * @param color The color of the arrow.
    00064    */
    00065   void drawParticle(const GT2005Particle& particle, Drawings::Color color) const;
    00066
    00067   /* The function calls drawParticle for every particle.
    00068    * @param particleColor The color of the particles
    00069    * @param estimatedBallColor The color of the estimated ball
    00070    */
    00071   void draw(Drawings::Color particleColor, Drawings::Color estimatedBallColor) const;
    00072
    00073 private:
    00074   //Estimated position of the last frame:
    00075   double oldX;
    00076   double oldY;
    00077
    00078   //The time between the last and the current frame (s):
    00079   double timeChange;
    00080
    00081   //The maximum length possible for two points on the field:
    00082   double dFieldDiagonalLength;
    00083
    00084   //Some constant values which might be evolved:
    00085   double framesNoBallSeenThreshold;
    00086
    00087   //The estimated ball position and velocity with the calculated probability:
    00088   double ballX;
    00089   double ballY;
    00090   double ballVX;
    00091   double ballVY;
    00092   double ballP;
    00093   double ballVP;
    00094
    00095   //Camera matrix of the last frame:
    00096   CameraMatrix prevCameraMatrix;
    00097
    00098   //Number of frames in the simulation:
    00099   int frameNumber;
    00100
    00101   //In this variable the odometry data of the last frame is saved:
    00102   Pose2D lastOdometryData;
    00103
    00104   //Number of frames when no ball was seen:
    00105   int framesNoBallSeen;
    00106
    00107   //System time when the ball was last seen:
    00108   unsigned long ballSeenTime;
    00109
    00110   //Last position where the ball was seen:
    00111   Vector2<double> lastPositionSeen;
    00112
    00113   //Flag that indicates if the current GameControlData-State ist "Set":
    00114   bool stateIsSet;
    00115
    00116   //This functions returns the factor by which the probability of the particle
    00117   //will be decreased dependend on the time which the ball was not seen:
    00118   double getTimeFactor();
    00119
    00120   //The current system time: (s)
    00121   unsigned long currentSystemTime;
    00122
    00123   //The system time of the last frame: (s)
    00124   unsigned long lastSystemTime;
    00125
    00126   //This function is needed for an seen as well as for an unseen ball:
    00127   void timeUpdate();
    00128
    00129   //This function eliminates particles with nonsense ball positions:
    00130   void noNonsenseParticles();
    00131
    00132   //Determine nonsense ball positions:
    00133   bool isNonSensePos(Vector2<double> position);
    00134
    00135   //A function that initializes the particles depending on the position
    00136   //where the ball will be placed at (for example the kickoff position)
    00137   void initializeParticles();
    00138
    00139   /* Calculates depending on the distance to a percept the approximative error.
    00140    * @param distance The distance to the percept.
    00141    * @param squareDistance distance*distance
    00142    */
    00143   double calculateDistanceValue(Vector2<double> ballOffset);
    00144
    00145   /* Calculates depending on the panning velocity the approximative error.
    00146    * @param panningVelocity The velocity the head is moved with.
    00147    */
    00148   double calculatePanningVelocityValue();
    00149   double calculateRobotSpeedValue();
    00150
    00151   //Count how many percepts have a nonsense pose:
    00152   double nonsensePos;
    00153 };
    00154 //---------------------------------------------------------------------------------------
    00155 #endif
    00156 //---------------------------------------------------------------------------------------