Representations/Cognition/BallModel.h

Go to the documentation of this file.

/**
* @file BallModel.h
*
* Declaration of class BallPosition
*
* @author Martin Lötzsch
* @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
* @author <a href="mailto:stefanuhrig@gmx.net">Stefan Uhrig</a>
*/

#ifndef __BallModel_h_
#define __BallModel_h_

#include "Tools/Streams/InOut.h"
#include "Tools/Math/Pose2D.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Debugging/DebugDrawings.h"
#include "Tools/Streams/Streamable.h"


/**
 * @class BallState
 *
 * Base class for a ball state.
 */
class BallState : public Streamable
{
public:
  /**
   * Standard constructor initializing the ball state.
   */
  BallState();

public:
  
  /**
   * Resets all data of the ball state.
   */
  void reset();

  /**
   * Sets the ball data with parameters relative to the robot. The parameters
   * relative to the field are *automatically* generated.
   * @param robotPose The robot pose.
   * @param px x-position of the ball relative to the robot (in mm).
   * @param py y-position of the ball relative to the robot (in mm).
   * @param vx x-speed of the ball relative to the robot (in mm/s).
   * @param vy y-speed of the ball relative to the robot (in mm/s).
   */
  void setBallDataRelativeToRobot(
    const RobotPose& robotPose,
    double px,
    double py,
    double vx,
    double vy);

  /**
   * Sets the ball data with parameters relative to the field. The parameters
   * relative to the robot are *automatically* generated.
   * @param robotPose The robot pose.
   * @param px x-position of the ball relative to the field (in mm).
   * @param py y-position of the ball relative to the field (in mm).
   * @param vx x-speed of the ball relative to the field (in mm/s).
   * @param vy y-speed of the ball relative to the field (in mm/s).
   */
  void setBallDataRelativeToField(
    const RobotPose& robotPose,
    double px,
    double py,
    double vx,
    double vy);

  /**
   * Sets the gaussian distribution of the ball position relative to the
   * robot. The distribution relative to the field is *automatically*
   * generated.
   * @param robotPose The robot pose.
   * @param orientation The angle from the x-axis of the robot to the major
   *                    axis of the ellipsis (in radians).
   * @param majorAxis The length of the major axis (that is the standard
   *                  deviation in the major direction) (in mm).
   * @param minorAxis The length of the minor axis (that is the standard
   *                  deviation in the minor direction) (in mm).
   */
  void setPositionGaussianDataRelativeToRobot(
    const RobotPose& robotPose,
    double orientation,
    double majorAxis,
    double minorAxis);

  /**
   * Sets the gaussian distribution of the ball position relative to the
   * field. The distribution relative to the robot is *automatically*
   * generated.
   * @param robotPose The robot pose.
   * @param orientation The angle from the x-axis of the field to the major
   *                    axis of the ellipsis (in radians).
   * @param majorAxis The length of the major axis (that is the standard
   *                  deviation in the major direction) (in mm).
   * @param minorAxis The length of the minor axis (that is the standard
   *                  deviation in the minor direction) (in mm).
   */
  void setPositionGaussianDataRelativeToField(
    const RobotPose& robotPose,
    double orientation,
    double majorAxis,
    double minorAxis);


  /**
   * Sets the gaussian distribution of the ball speed relative to the
   * robot. The distribution relative to the field is *automatically*
   * generated.
   * @param robotPose The robot pose.
   * @param orientation The angle from the x-axis of the robot to the major
   *                    axis of the ellipsis (in radians).
   * @param majorAxis The length of the major axis (that is the standard
   *                  deviation in the major direction) (in mm).
   * @param minorAxis The length of the minor axis (that is the standard
   *                  deviation in the minor direction) (in mm).
   */
  void setSpeedGaussianDataRelativeToRobot(
    const RobotPose& robotPose,
    double orientation,
    double majorAxis,
    double minorAxis);

  /**
   * Sets the gaussian distribution of the ball speed relative to the
   * field. The distribution relative to the robot is *automatically*
   * generated.
   * @param robotPose The robot pose.
   * @param orientation The angle from the x-axis of the field to the major
   *                    axis of the ellipsis (in radians).
   * @param majorAxis The length of the major axis (that is the standard
   *                  deviation in the major direction) (in mm).
   * @param minorAxis The length of the minor axis (that is the standard
   *                  deviation in the minor direction) (in mm).
   */
  void setSpeedGaussianDataRelativeToField(
    const RobotPose& robotPose,
    double orientation,
    double majorAxis,
    double minorAxis);

  /**
   * Returns a position validity in the range from 0.0 to 1.0. This validity
   * is calculated from the position gauss if present. Probably there are
   * more sophisticated ways to calc the validity from the gaussian
   * distribution.
   * @return Validity calculated from the position gauss in the range from
   *         0.0 to 1.0. Return value is 1.0 if no position gauss present.
   */
  double getPositionValidity() const;

  /**
   * Returns a speed validity in the range from 0.0 to 1.0. This validity
   * is calculated from the speed gauss if present. Probably there are
   * more sophisticated ways to calc the validity from the gaussian
   * distribution.
   * @return Validity calculated from the speed gauss in the range from
   *         0.0 to 1.0. Return value is 1.0 if no speed gauss present.
   */
  double getSpeedValidity() const;

  /**
   * Draws the position of the ball to the field. Drawing must be finished
   * by the caller with DEBUG_DRAWING_FINISHED(ballLocatorField).
   * @param color The color to use for the drawing.
   */
  void drawPosition(Drawings::Color color) const;

  /**
   * Draws the speed as arrow to the field.  Drawing must be finished
   * by the caller with DEBUG_DRAWING_FINISHED(ballLocatorField).
   * @param color The color to use for the drawing.
   */
  void drawSpeed(Drawings::Color color) const;

  /**
   * Draws the position gaussian (if set) as the two axes of the ellipsis.
   * Drawing must be finished by the caller with
   * DEBUG_DRAWING_FINISHED(ballLocatorField).
   * @param color The color to use for the drawing.
   */
  void drawPositionGaussian(Drawings::Color color) const;

  /**
   * Draws the speed gaussian (if set) as the two axes of the ellipsis.
   * Drawing must be finished by the caller with
   * DEBUG_DRAWING_FINISHED(ballLocatorField).
   * @param color The color to use for the drawing.
   */
  void drawSpeedGaussian(Drawings::Color color) const;

  //Functions for streaming with TeamMessagesMessage:
  // int read(In& stream, int length);
  //void write(Out& stream);
  // unsigned int length();

protected:

  /**
   * Sets several ball flags from the actual ball state.
   */
  void setBallFlags();

  /**
   * Converts a position vector relative to the robot to a position vector
   * relative to the field.
   * @param robotPose The robot pose. 
   * @param pv The position vector relative to the robot.
   * @return The position vector relative to the field.
   */
  static Vector2<double> positionVectorFromRobotToField(
    const RobotPose& robotPose,
    const Vector2<double>& pv);
    
  /**
   * Converts a direction vector relative to the robot to a direction vector
   * relative to the field.
   * @param robotPose The robot pose. 
   * @param dv The direction vector relative to the robot.
   * @return The direction vector relative to the field.
   */
  static Vector2<double> directionVectorFromRobotToField(
    const RobotPose& robotPose,
    const Vector2<double>& dv);

  /**
   * Converts an orientation (in radians) relative to the robot to an
   * orientation relative to the field.
   * @param robotPose The robot pose.
   * @param orientation The orientation in radians relative to the robot.
   * @return The orientation in radians relative to the field.
   */
  static double orientationFromRobotToField(
    const RobotPose& robotPose,
    double orientation);

  /**
   * Converts a position vector relative to the field to a position vector
   * relative to the robot.
   * @param robotPose The robot pose. 
   * @param pv The position vector relative to the field.
   * @return The position vector relative to the robot.
   */
  static Vector2<double> positionVectorFromFieldToRobot(
    const RobotPose& robotPose,
    const Vector2<double>& pv);

  /**
   * Converts a direction vector relative to the field to a direction vector
   * relative to the robot.
   * @param robotPose The robot pose. 
   * @param pv The direction vector relative to the field.
   * @return The direction vector relative to the robot.
   */
  static Vector2<double> directionVectorFromFieldToRobot(
    const RobotPose& robotPose,
    const Vector2<double>& dv);

  /**
   * Converts an orientation (in radians) relative to the field to an
   * orientation relative to the robot.
   * @param robotPose The robot pose.
   * @param orientation The orientation in radians relative to the field.
   * @return The orientation in radians relative to the robot.
   */
  static double orientationFromFieldToRobot(
    const RobotPose& robotPose,
    double orientation);
 

public:
  
  /**
   * If a ball locator calculates a ball position and velocity probability
   * by itself it can set this variable to true and set the variable
   * positionProb and velocityProb and the functions getPositionValidity
   * and getSpeedValidity will return those values.
   */
  bool useGivenProbabilities;
  double positionProb;
  double velocityProb;
  
  /// The position of the ball relative to the robot (in mm)
  Vector2<double> positionRobot;
  
  /// The speed of the ball relative to the robot (in mm/s)
  Vector2<double> speedRobot;

  /// The position of the ball relative to the field (in mm)
  Vector2<double> positionField;

  /// The speed of the ball relative to the field (in mm/s)
  Vector2<double> speedField;

  /// The robot pose
  RobotPose robotPose;

  /// If true, the line from robot to ball crosses the goal line
  bool ballInFrontOfOpponentGoal;

  /// If true, the ball rolls by left in a short distance
  bool ballRollsByLeft;
  bool ballRollsFarByLeft;
  
  /// If true, the ball rolls by right in a short distance
  bool ballRollsByRight;
  bool ballRollsFarByRight;
  
  /// If true, the ball rolls towards the robot
  bool ballRollsTowardsRobot;
  
  /// If true, the ball has a high speed in any direction
  bool ballRollsFast;

  /// this is the projected distance of the ball on the Y axis of the robot
  /// (in mm)
  double projectedDistanceOnYAxis;

  /// time in ms which the ball needs to cross the robots y axis
  long timeTillBallCrossesYAxis;

  /// True if the position gauss was set
  bool positionGaussSet;

  /// The angle of the robot x-axis to the ellipse's major axis in radians.
  double positionGaussOrientationRobot;

  /// The angle of the field x-axis to ellipse's major axis in radians.
  double positionGaussOrientationField;

  /// The length of the ellipsis major axis (that is the standard deviation)
  /// (in mm)
  double positionGaussMajAxis;

  /// The length of the ellipsis minor axis (that is the standard deviation)
  /// (in mm)
  double positionGaussMinAxis;

  /// True if the position gauss was set
  bool speedGaussSet;

  /// The angle from the robot x-axis to ellipsis major axis in radians.
  double speedGaussOrientationRobot;

  /// The angle from the field x-axis to ellipsis major axis in radians.
  double speedGaussOrientationField;

  /// The length of the ellipsis major axis (that is the standard deviation)
  /// (in mm)
  double speedGaussMajAxis;

  /// The length of the ellipsis minor axis (that is the standard deviation)
  /// (in mm)
  double speedGaussMinAxis;

//this is for streaming (with specifications)
public:  
  virtual void serialize(In *in, Out *out);
};
//------------------------------------------------------------------------------
/**
 * Writes a ball state to a stream.
 * @param stream The stream the ball position is written to.
 * @param bs The ball state to write.
 * @return Reference to the stream.
 */
//Out& operator<<(Out& stream, const BallState& bs);
/**
 * Reads a ball state from a stream.
 * @param stream The stream the ball state is read from.
 * @param bs The ball state the read data is stored to.
 * @return Reference to the stream.
 */
//In& operator>>(In& stream, BallState& bs);
//------------------------------------------------------------------------------
/**
 * @class SeenBallState
 *
 * The knowledge abot the ball that was obtained by own observations.
 */
class SeenBallState : public BallState
{
public:

  /**
   * Constructor
   */
  SeenBallState();

public:

  /**
   * Resets all data of the ball position.
   */
  void reset();

  /**
  * If this is positive, we see the ball for that time consecutively.
  * If this is negative, we have not seen the ball consecutively for -that time.
  */
  long getConsecutivelySeenTime() const;

public:

  /**
  * The time when the ball was last seen by the own camera.
  * "position" and speed were set at that point in time.
  */
  unsigned long timeWhenLastSeen;
  
  /**
  * The time when the ball was last seen consecutively, i.e.
  * it has been seen "a lot" but not necessarily in all frames;
  * gaps of n frames or more are okay
  */
  unsigned long timeWhenFirstSeenConsecutively;
  
  /**
  * The time until the ball has been seen consecutively, i.e.
  * it has been seen "a lot" but not necessarily in all frames
  * until the time stored here;
  */
  unsigned long timeUntilSeenConsecutively;
//this is for streaming (with specifications)
public:  
  virtual void serialize(In *in, Out *out);
  //Functions for streaming with TeamMessagesMessage:
  //int read(In& stream, int length);
  //void write(Out& stream);
  //unsigned int length();

};
//------------------------------------------------------------------------------
/**
 * Writes a seen ball state to a stream.
 * @param stream The stream the ball position is written to.
 * @param sbs The ball state to write.
 * @return Reference to the stream.
 */
//Out& operator<<(Out& stream, const SeenBallState& sbs);
/**
 * Reads a seen ball state from a stream.
 * @param stream The stream the ball state is read from.
 * @param sbs The ball state the read data is stored to.
 * @return Reference to the stream.
 */
//In& operator>>(In& stream, SeenBallState& sbs);
//------------------------------------------------------------------------------
/*
 * @class PropagatedBallState
 *
 * The position of the ball estimated from own observations. It is estimated
 * even if the ball is not seen.
 */
class PropagatedBallState : public BallState
{
public:
  
  /**
   * Constructor
   */
  PropagatedBallState();

public:

  /**
   * Resets all data of the ball position.
   */
  void reset();

public:

  /// The last seen ball position
  SeenBallState lastSeenBallState;

//this is for streaming (with specifications)
public:  
  virtual void serialize(In *in, Out *out);
};
//------------------------------------------------------------------------------
/**
 * Writes a propagated ball state to a stream.
 * @param stream The stream the ball position is written to.
 * @param pbs The ball state to write.
 * @return Reference to the stream.
 */
//Out& operator<<(Out& stream, const PropagatedBallState& pbs);
/**
 * Reads a propagated ball state from a stream.
 * @param stream The stream the ball state is read from.
 * @param pbs The ball state the read data is stored to.
 * @return Reference to the stream.
 */
//In& operator>>(In& stream, PropagatedBallState& pbs);
//------------------------------------------------------------------------------
/*
 * @class CommunicatedBallState
 *
 * The position of the ball estimated from own observations and from
 * observation of other players.
 */
class CommunicatedBallState : public BallState
{
public:

  /**
   * Constructor
   */
  CommunicatedBallState();

public:

  /**
   * Resets all data of the ball position.
   */
  void reset();

public:

  /// Time when the observation was made by another robot
  unsigned long timeWhenLastObserved;
  
  //this is for streaming (with specifications)
  virtual void serialize(In *in, Out *out);
};
//------------------------------------------------------------------------------
/**
 * Writes a communicated ball state to a stream.
 * @param stream The stream the ball position is written to.
 * @param cbs The ball state to write.
 * @return Reference to the stream.
 */
//Out& operator<<(Out& stream, const CommunicatedBallState& cbs);
/**
 * Reads a communicated ball state from a stream.
 * @param stream The stream the ball state is read from.
 * @param cbs The ball state the read data is stored to.
 * @return Reference to the stream.
 */
//In& operator>>(In& stream, CommunicatedBallState& cbs);
//------------------------------------------------------------------------------
class HypotheticalBallState : public BallState
{
public:

  /**
   * Constructor
   */
  HypotheticalBallState();
};
//------------------------------------------------------------------------------
/**
 * Writes a hypothetical ball state to a stream.
 * @param stream The stream the ball position is written to.
 * @param hbs The ball state to write.
 * @return Reference to the stream.
 */
//Out& operator<<(Out& stream, const HypotheticalBallState& hbs);
/**
 * Reads a hypothetical ball state from a stream.
 * @param stream The stream the ball state is read from.
 * @param hbs The ball state the read data is stored to.
 * @return Reference to the stream.
 */
//In& operator>>(In& stream, HypotheticalBallState& cbs);
//------------------------------------------------------------------------------
/**
 * @class BallModel
 *
 * Contains the modeled knowledge about the ball.
 */
class BallModel : public Streamable
{
public:
  /**
   * The time for the behaviorControl after that communicated ball
   * positions are accepted
   */
  enum
  {
    behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted = 2000,
    timeAfterWhichPropagatedAreUsed = 100,
    maxDistanceToUseSeen = 500,
    minDistanceToUseSeen = 150
  };
  
  /**
   * Constructor.
   */
  BallModel();

  /**
   * Resets the ball model.
   */
  void reset();
  
  /**
   * Sets the frame number of the image this model was created from.
   * @param frameNumber The frame number.
   */
  void setFrameNumber(unsigned long frameNumber);
  
  /**
   * Returns a "known" ball position relative to the field.
   * If the ball was seen, the seen ball position is used. If the ball
   * was not seen for a timeAfterThatCommunicatedBallsAreAccepted,
   * the communicatedBallPosition is returned, if there was received a
   * ball position.
   * @param timeAfterWhichCommunicatedBallsAreAccepted The time after that
   *          the communicated position is returned
   */
  const Vector2<double>& getKnownPosition(
    unsigned long timeAfterWhichCommunicatedBallsAreAccepted) const;

  const Vector2<double>& getKnownPosition(
  unsigned long timeAfterWhichCommunicatedBallsAreAccepted,
  unsigned long timeAfterWhichPropagatedAreUsed,
  double maxDistanceToUseSeen,
  double minDistanceToUseSeen) const;

  const BallState& getKnownBallState(
    unsigned long timeAfterWhichCommunicatedBallsAreAccepted = behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted,
    unsigned long timeAfterPropagatedAreUsed = timeAfterWhichPropagatedAreUsed,
    double maxDistanceUseSeen = maxDistanceToUseSeen,
    double minDistanceUseSeen = minDistanceToUseSeen) const;
  
  /**
   * Returns the time since the ball was seen or communicated.
   * @param timeAfterWhichCommunicatedBallsAreAccepted as in getKnownPosition()
   */
  unsigned long getTimeSinceLastKnown(
    unsigned long timeAfterWhichCommunicatedBallsAreAccepted) const;
  
public:

  /// The frame number of the image this model was created from.
  unsigned long frameNumber;

  /// The number of consecutive images without ball percept.
  int numberOfImagesWithoutBallPercept;
  
  /// The number of consecutive images with ball percept.
  int numberOfImagesWithBallPercept;

  /// True if the ball was seen (a ball percept exists)
  bool ballWasSeen;

  /// The state of the ball estimated from own observations
  SeenBallState seen;
  
  /**
   * The state of the ball estimated from own observations;
   * it is propagated even if the ball is not seen
   */
  PropagatedBallState propagated;
  
  /**
   * The state of the ball estimated from own observations and from
   * observation of other players.
   */
  CommunicatedBallState communicated;

  /**
   * The state of the ball guessed from own negative information
   * after ball was lost
   */
  HypotheticalBallState hypothetical;

//this is for streaming (with specifications)
public:  
  virtual void serialize(In *in, Out *out)
  {
    STREAM_REGISTER_BEGIN();
    STREAM( frameNumber);
    STREAM( numberOfImagesWithoutBallPercept);
    STREAM( numberOfImagesWithBallPercept);
    STREAM( ballWasSeen);
    STREAM( seen);
    STREAM( propagated);
    STREAM( communicated);
    STREAM( hypothetical);
    STREAM_REGISTER_FINISH();
  }
};
//------------------------------------------------------------------------------
/**
 * Writes a ball model to a stream.
 * @param stream The stream the ball model is written to.
 * @param hbs The ball model to write.
 * @return Reference to the stream.
 */
//Out& operator<<(Out& stream, const BallModel& bm);
/**
 * Reads a ball model from a stream.
 * @param stream The stream the ball model is read from.
 * @param hbs The ball model the read data is stored to.
 * @return Reference to the stream.
 */
//In& operator>>(In& stream, BallModel& bm);
//------------------------------------------------------------------------------
#endif //__BallModel_h_

---