Representations/Perception/LandmarksPercept.h

Go to the documentation of this file.

/**
* @file Representations/Perception/LandmarksPercept.h
*
* Contains the definition of class LandmarksPercept. 
*
* @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
* @author <A href=mailto:asbre01@tzi.de>Andreas Sztybryc</A>
*/ 

#ifndef __LandmarksPercept_h_
#define __LandmarksPercept_h_

#include "Tools/Streams/InOut.h"
#include "Tools/Math/Vector3.h"
#include "Tools/Math/Common.h"
#include "Tools/ColorClasses.h"
#include "Tools/Boundary.h"
#include "Tools/Streams/Streamable.h"
#include "Representations/Perception/CameraMatrix.h"

/**
* The class represents a rectangular landmark boundary.
* It also encodes which edges of the boundary touch the image border.
*/
class ConditionalBoundary : public Boundary<double>, public Streamable
{
private:
  Boundary<double> freeBorders; /**< Contains the edges that do not touch the image border. */
  
public:
/**
* Constructor.
* The boundary is empty.
  */
  ConditionalBoundary()
    : Boundary<double>(-pi,pi),
    freeBorders(-pi,pi) {}
  
  /**
   * The function adds a point to the boundary.
   * @param px The x-coordinate of a new x-boundary-candidate.
   * @param isOnBorder Is the point on the image border?
   */
  void addX(const double px,bool isOnBorder)
  {
    x.add(px);
    if(!isOnBorder)
      freeBorders.x.add(px);
  }
  
  
  /**
   * The function adds a point to the boundary.
   * @param py The y-coordinate of a new y-boundary-candidate.
   * @param isOnBorder Is the point on the image border?
   */
  void addY(const double py,bool isOnBorder)
  {
    y.add(py);
    if(!isOnBorder)
      freeBorders.y.add(py);
  }

  /**
  * The function adds another boundary to this one.
  * @param b The other boundary.
  */
  void add(const ConditionalBoundary& b)
  {
    Boundary<double>::add(b);
    freeBorders.add(b.freeBorders);
  }
  
  /**
  * The function determines whether a certain edge lies on the image border.
  * @param border This parameter specifies the edge to test. The parameter
  *               must be one of the following four members of this object:
  *               x.min, x.max, y.min, y.max. A typical call would be:
  *               b.isOnBorder(b.min.x)
  * @return Does the specified edge touch the image border?
  */
  bool isOnBorder(const double& border) const
  {
    if(&x.min == &border)
      return x.min != freeBorders.x.min;
    else if(&x.max == &border)
      return x.max != freeBorders.x.max;
    else if(&y.min == &border)
      return y.min != freeBorders.y.min;
    else if(&y.max == &border)
      return y.max != freeBorders.y.max;
    return false;
  }

  void serialize(In* in, Out* out)
  {
    STREAM_REGISTER_BEGIN();
    STREAM(x.min);
    STREAM(x.max);
    STREAM(y.min);
    STREAM(y.max);
    STREAM(freeBorders.x.min);
    STREAM(freeBorders.x.max);
    STREAM(freeBorders.y.min);
    STREAM(freeBorders.y.max);
    STREAM_REGISTER_FINISH();
  }
};

/**
* The class represents a flag.
*/
class Flag : public ConditionalBoundary
{
public:
  enum FlagType
  {
    pinkAboveYellow, pinkAboveSkyblue, 
    yellowAbovePink, skyblueAbovePink,
    numberOfFlagTypes
  }; /**< The type is used to identify the four different flags on the field. */
  
  FlagType type; /**< The type of this flag. */
  Vector2<double> position; /**< The position of this flag on the field. */
  double distanceValidity; /**< The validity of the determined distance. Not used yet. */
  double angleValidity; /**< The validity of the determined direction. Not used yet. */

  /** distance to the flag, relative to robot */
  double distance;
  
  /** angle to the flag, relative to robot */
  double angle;
  
  Vector2<int> topLeft; /**< Position of the upper left corner of the flag in the image. */
  Vector2<int> topRight; /**< Position of the upper right corner of the flag in the image. */
  Vector2<int> bottomLeft; /**< Position of the lower left corner of the flag in the image. */
  Vector2<int> bottomRight; /**< Position of the lower right corner of the flag in the image. */

  /** the bottom color of the flag */
  colorClass bottomColor;
  /**
  * The function sets the color of the lower part of this flag.
  */
  void setBottomColor();
  
  /** the upper color of the flag */
  colorClass topColor;
  /**
  * The function sets the color of the upper part of this flag.
  */
  void setTopColor();

  void serialize(In* in, Out* out)
  {
    STREAM_REGISTER_BEGIN();
    STREAM_ENUMASINT(type);
    STREAM_ENUMASINT(topColor);
    STREAM_ENUMASINT(bottomColor);
    STREAM(position);
    STREAM(distanceValidity);
    STREAM(angleValidity);
    STREAM(distance);
    STREAM(angle);
    STREAM(topLeft);
    STREAM(topRight);
    STREAM(bottomLeft);
    STREAM(bottomRight);
    STREAM_BASE(ConditionalBoundary);
    STREAM_REGISTER_FINISH();
  }
};

/**
* The class represents a goal.
*/
class Goal : public ConditionalBoundary
{
public:
  colorClass color; /**< The color of the goal. */
  Vector2<double> leftPost, /**< The position of the goal post that is <i>seen</i> left. */
    rightPost; /**< The position of the goal post that is <i>seen</i> right. */
  
  double distanceValidity; /**< The validity of the determined distance. Not used yet. */
  double angleValidity; /**< The validity of the determined direction. Not used yet. */
  
  /** distance to left corner of the goal, relative to robot */
  double distance;
  
  /** angle to the goal, relative to robot (meassured to the middle of the goalline) */
  double angle;
  
  /** rotation of the the goal, relative to robot (meassured to the middle of the goalline) */
  double rotation;

  Vector2<int> topLeft; /**< Position of the upper left corner of the goal in the image. */
  Vector2<int> topRight; /**< Position of the upper right corner of the goal in the image. */
  Vector2<int> bottomLeft; /**< Position of the lower left corner of the goal in the image. */
  Vector2<int> bottomRight; /**< Position of the lower right corner of the goal in the image. */

  void serialize(In* in, Out* out)
  {
    STREAM_REGISTER_BEGIN();
    STREAM_ENUMASINT(color);
    STREAM(leftPost);
    STREAM(rightPost);
    STREAM(distance);
    STREAM(angle);
    STREAM(distanceValidity);
    STREAM(angleValidity);
    STREAM(rotation);
    STREAM(topLeft);
    STREAM(topRight);
    STREAM(bottomLeft);
    STREAM(bottomRight);
    STREAM_BASE(ConditionalBoundary);
    STREAM_REGISTER_FINISH();
  }
};

/**
* The class represents all detected landmark percepts.
*/
class LandmarksPercept : public Streamable
{
public:
  Flag flags[4]; /**< The array of up to 4 flags. */
  int numberOfFlags; /**< The number of flags actually stored in the array. */
  Goal goals[2]; /**< The array of up to 2 goals. */
  int numberOfGoals; /**< The number of goals actually stored in the array. */
  double viewAngle; /**< The estimated viewing angle in field coords. */
  bool viewAngleValid; /**< Tells the self locator if the view angle is valid. */
  Vector3<double> cameraOffset; /**< The camera offset relative to position of the robot's neck */
  unsigned long frameNumber; /**< The frame number when perceived. */
  /**
  * Constructor.
  * Resets the object.
  */
  LandmarksPercept();
  
  /**
  * The function resets the object, i.e. the numbers of flags and goals are set to 0.
  */
  void reset(unsigned long frameNumber);
  
  /**
  * The function adds a flag to the flag array.
  */
  void addFlag(Flag::FlagType type,
    const Vector2<double>& position,
    const ConditionalBoundary& boundary,
    const Vector2<int>& topLeft,
    const Vector2<int>& topRight,
    const Vector2<int>& bottomLeft,
    const Vector2<int>& bottomRight);
  
  /**
  * The function adds a flag to the flag array.
  */
  void addFlag(Flag::FlagType type,
    bool ownTeamColorIsBlue,
    const ConditionalBoundary& boundary,
    const Vector2<int>& topLeft,
    const Vector2<int>& topRight,
    const Vector2<int>& bottomLeft,
    const Vector2<int>& bottomRight);

  /**
  * Calculates distance and angle for each flag.
  */
  void estimateOffsetForFlags
  (
   const Vector2<double>& cameraOffset
   );

  /**
  * The function adds a goal to the goal array.
  */
  void addGoal(colorClass color,
    const ConditionalBoundary& boundary,
    const Vector2<int>& topLeft,
    const Vector2<int>& topRight,
    const Vector2<int>& bottomLeft,
    const Vector2<int>& bottomRight,
    double distance, 
    double angle, 
    double rotation,
    double distanceValidity, 
    double angleValidity);
  void addGoal(colorClass color,
    const ConditionalBoundary& boundary,
    const Vector2<int>& topLeft,
    const Vector2<int>& topRight,
    const Vector2<int>& bottomLeft,
    const Vector2<int>& bottomRight,
    const CameraMatrix& cameraMatrix);

  /**
  * Calculates distance and angle for each goal.
  */
  void estimateOffsetAndValiditiesForGoal
  (
   Goal& goal,
   const CameraMatrix& cameraMatrix
  );


  void serialize(In* in, Out* out)
  {
    STREAM_REGISTER_BEGIN();
    STREAM(frameNumber);
    STREAM_DYN_ARRAY(flags, numberOfFlags);
    STREAM_DYN_ARRAY(goals, numberOfGoals);
    STREAM(cameraOffset);
    STREAM(viewAngle);
    STREAM(viewAngleValid);
    STREAM_REGISTER_FINISH();
  }
};

#endif //__LandmarksPercept_h_

---