Modules/BallLocator/GT2005BallLocator.h

Go to the documentation of this file.

/**
 * @file GT2005BallLocator.h
 * Contains a BallLocator implementation using Particle Filters
 *
 * @author <a href="mailto:christinez@gmx.de">Christine Zarges</a>
 * @author <a href="mailto:Thorsten.Kerkhof@gmx.de">Thorsten Kerkhof</a>
 */
//---------------------------------------------------------------------------------------
#ifndef GT2005BallLocator_H_INCLUDED
#define GT2005BallLocator_H_INCLUDED
//---------------------------------------------------------------------------------------
#include "BallLocator.h"
#include "Tools/Debugging/GenericDebugData.h"
#include <vector>
#include "Tools/Math/Matrix2x2.h"
#include "GT2005Particles/GT2005Particle.h"
#include "GT2005Particles/GT2005ParticleContainer.h"
#include "GT2005BallLocatorParameters.h"
//---------------------------------------------------------------------------------------
/**
* @class GT2005BallLocator
* A BallLocator using a particle filter.
*/
class GT2005BallLocator : public BallLocator
{
public:
  GT2005BallLocator(const BallLocatorInterfaces& interfaces);

  //Executes the GT2005BallLocator module:
  virtual void execute();
  
  /**
  * Handles debug messages
  * @param message The message
  * @return true if message was handled
  */
  virtual bool handleMessage(InMessage& message);

  //Calls MODIFY macro of RC2 for some paramters:
  void modify();

  /* MeasurementUpdate-functions to update position and velocity seperately.
   * First the positions are updated and the estimated ball position is calculated
   * and this information is used to calculate the velocities.
   * @param ballOffset The relative position of the percept.
   * @param reliability The reliability of the percept.
   */
  void measurementUpdate(Vector2<double> ballOffset,
                         double reliability,
                         double distanceValue,
                         double panningVelocityValue,
                         double robotSpeedValue);
  

  //Sets the state of the ball in the BallSymbols:
  void setBallSymbols();

  /* The function draws an particle as an arrow.
   * @param particle The particle to draw
   *   -> the position of the particle is the position to draw the arrow
   *   -> the velocity gives the orientation and length of the arrow
   *   -> the probability gives the thickness of the arrow
   * @param color The color of the arrow.
   */
  void drawParticle(const GT2005Particle& particle, Drawings::Color color) const;

  /* The function calls drawParticle for every particle.
   * @param particleColor The color of the particles
   * @param estimatedBallColor The color of the estimated ball
   */
  void draw(Drawings::Color particleColor, Drawings::Color estimatedBallColor) const;

private:
  //Estimated position of the last frame:
  double oldX;
  double oldY;

  //The time between the last and the current frame (s):
  double timeChange;

  //The maximum length possible for two points on the field:
  double dFieldDiagonalLength;

  //Some constant values which might be evolved:
  double framesNoBallSeenThreshold;

  //The estimated ball position and velocity with the calculated probability:
  double ballX;
  double ballY;
  double ballVX;
  double ballVY;
  double ballP;
  double ballVP;

  //Camera matrix of the last frame:
  CameraMatrix prevCameraMatrix;

  //Number of frames in the simulation:
  int frameNumber;

  //In this variable the odometry data of the last frame is saved:
  Pose2D lastOdometryData;

  //Number of frames when no ball was seen:
  int framesNoBallSeen;

  //System time when the ball was last seen:
  unsigned long ballSeenTime;

  //Last position where the ball was seen:
  Vector2<double> lastPositionSeen;

  //Flag that indicates if the current GameControlData-State ist "Set":
  bool stateIsSet;

  //This functions returns the factor by which the probability of the particle
  //will be decreased dependend on the time which the ball was not seen:
  double getTimeFactor();

  //The current system time: (s)
  unsigned long currentSystemTime;

  //The system time of the last frame: (s)
  unsigned long lastSystemTime;

  //This function is needed for an seen as well as for an unseen ball:
  void timeUpdate();

  //This function eliminates particles with nonsense ball positions:
  void noNonsenseParticles();

  //Determine nonsense ball positions:
  bool isNonSensePos(Vector2<double> position);
  
  //A function that initializes the particles depending on the position
  //where the ball will be placed at (for example the kickoff position)
  void initializeParticles();

  /* Calculates depending on the distance to a percept the approximative error.
   * @param distance The distance to the percept.
   * @param squareDistance distance*distance
   */
  double calculateDistanceValue(Vector2<double> ballOffset);

  /* Calculates depending on the panning velocity the approximative error.
   * @param panningVelocity The velocity the head is moved with.
   */
  double calculatePanningVelocityValue();
  double calculateRobotSpeedValue();

  //Count how many percepts have a nonsense pose:
  double nonsensePos;
};
//---------------------------------------------------------------------------------------
#endif
//---------------------------------------------------------------------------------------

---