1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
/** 
* @file GoalPercept.h
*
* Declaration of class GoalPercept
*/

#ifndef _GoalPercept_h_
#define _GoalPercept_h_

#include "Tools/Math/Vector2.h"
#include "Tools/Math/Vector3.h"
#include "Tools/ColorClasses.h"
#include <Tools/DataStructures/Printable.h>
#include <Tools/DataStructures/Serializer.h>

class GoalPercept : public naoth::Printable
{ 
public:
  GoalPercept()
    : 
    angleToSeenGoal(0),
    horizonScan(false),
    numberOfSeenPosts(0)
  {}

  class GoalPost
  {
  public:
    GoalPost() : 
      color(ColorClasses::none),
      type(unknownPost), 
      positionReliable(false), 
      seenHeight(0), 
      seenWidth(0)
    {}
    
    ~GoalPost(){}

    /* possible types of goalposts */
    enum PostType 
    {
      rightPost, 
      leftPost, 
      unknownPost
    }; 

    /* the posts base point in the image */
    Vector2i basePoint;

    /* the posts base point in the image */
    Vector2i topPoint;

    /* color of the post */
    ColorClasses::Color color;

    /* posts type */
    PostType type;

    /* indicates whether the base point was seen inside the field */
    bool positionReliable;

    /* the seen height of the post */
    double seenHeight;

    double seenWidth;

    /* the posts base point on the ground */
    Vector2d position;

    /* the posts up/down direction in the image*/
    Vector2d directionInImage;

    static const char* getPostTypeName(PostType type)
    {
      switch(type) 
      {
        case rightPost:   return "right post";
        case leftPost:    return "left post"; 
        case unknownPost: return "unknown post"; 
        default:          return "unknown post type!"; 
      };
    }//end getPostTypeName

    bool operator() (const GoalPost& postOne, const GoalPost& postTwo) 
    { 
      return (postOne.seenHeight > postTwo.seenHeight);
    }

  };//end class GoalPost


public:
  /* maximum number of posts */
  const static unsigned int MAXNUMBEROFPOSTS = 6;

  /* angle to the centroid of the goal-colored pixel in the image */
  double angleToSeenGoal;

  /* estimated position of the goal-centroid relative to the robot */
  Vector3d goalCentroid;

  // the goal was found at the horizon
  bool horizonScan; 

private:
  /* number of seen posts */
  unsigned int numberOfSeenPosts;

  /* array of recognized posts in arbitrary order */
  GoalPost post[MAXNUMBEROFPOSTS];


public:

  void add(const GoalPost& goalPost)
  {
    if(numberOfSeenPosts < MAXNUMBEROFPOSTS)
    {
      post[numberOfSeenPosts] = goalPost;
      numberOfSeenPosts++;
    }
  }


  void reset()
  {
    numberOfSeenPosts = 0;
    angleToSeenGoal = 0; 
    horizonScan = false;
  }


  virtual void print(std::ostream& stream) const
  {
    stream << "angleToSeenGoal=" << angleToSeenGoal << std::endl;
    stream << "goalCentroid=" << goalCentroid << std::endl;
    stream << "horizonScan=" << (horizonScan?"true":"false") << std::endl;
    
    for(unsigned int n=0; n<numberOfSeenPosts; n++)
    {
      stream << "=====Post " << n << "=====" << std::endl;
      stream << "post type= " << GoalPost::getPostTypeName(post[n].type) << std::endl;
      stream << "basePoint=(" << post[n].basePoint.x << ";" << post[n].basePoint.y << ")" << std::endl;
      stream << "position=(" << post[n].position.x << ";" << post[n].position.y << ")" << std::endl;
      stream << "color=" << ColorClasses::getColorName(post[n].color) << std::endl;
      stream << "reliable=" << (post[n].positionReliable? "true" : "false") << std::endl;
    }//end for
  }//end print


  //////////////////////////////////////
  // getters
  int getNumberOfSeenPosts() const { return (int)numberOfSeenPosts; }
  const GoalPost& getPost(unsigned int i) const { ASSERT(i < numberOfSeenPosts); return post[i]; }
  GoalPost& getPost(unsigned int i) { ASSERT(i < numberOfSeenPosts); return post[i]; }

private:
  friend class naoth::Serializer<GoalPercept>;
};//end GoalPercept

class GoalPerceptTop : public GoalPercept
{
public:
  virtual ~GoalPerceptTop() {}
};

namespace naoth
{
  template<>
  class Serializer<GoalPercept>
  {
  public:
    static void serialize(const GoalPercept& representation, std::ostream& stream);
    static void deserialize(std::istream& stream, GoalPercept& representation);
  };

  template<>
  class Serializer<GoalPerceptTop> : public Serializer<GoalPercept>
  {};
}

#endif // _GoalPercept_h_