Fawkes API  Fawkes Development Version
cornerhorizon.cpp
1 
2 /***************************************************************************
3  * cornerhorizon.cpp - Implementation of the corner horizon
4  *
5  * Created: Fri Apr 07 04:37:25 2006
6  * Copyright 2005-2006 Tim Niemueller [www.niemueller.de]
7  * 2006 Stefan Schiffer
8  * 2006 Christoph Mies
9  *
10  ****************************************************************************/
11 
12 /* This program is free software; you can redistribute it and/or modify
13  * it under the terms of the GNU General Public License as published by
14  * the Free Software Foundation; either version 2 of the License, or
15  * (at your option) any later version. A runtime exception applies to
16  * this software (see LICENSE.GPL_WRE file mentioned below for details).
17  *
18  * This program is distributed in the hope that it will be useful,
19  * but WITHOUT ANY WARRANTY; without even the implied warranty of
20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21  * GNU Library General Public License for more details.
22  *
23  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
24  */
25 
26 #include <fvmodels/scanlines/cornerhorizon.h>
27 #include <utils/math/angle.h>
28 #include <cstdlib>
29 #include <cstring>
30 
31 using namespace fawkes;
32 
33 namespace firevision {
34 #if 0 /* just to make Emacs auto-indent happy */
35 }
36 #endif
37 
38 const float CornerHorizon::M_PI_HALF = M_PI / 2.f;
39 
40 /** @class CornerHorizon <fvmodels/scanlines/cornerhorizon.h>
41  * Cut of arbitrary scanline models at an artificial horizon.
42  * The artificial horizon is calculated by the highest corner that is visible
43  * in the image. From that the Y coordinate in the image is used and everything
44  * above that point is ignored from the scanline grid.
45  *
46  * This class was written in a one-night hacking sensation at RoboLudens 2006
47  * in Eindhoven. For that time it is pretty readable code and we are using it
48  * since then. Cool!
49  *
50  * @author Tim Niemueller
51  * @author Stefan Schiffer
52  * @author Christoph Mies
53  */
54 
55 /** Constructor.
56  * @param model Model to apply the artificial horizon on. This model is deleted on
57  * the destruction of the CornerHorizon instance so you can forget about it in the
58  * using application.
59  * @param field_length length of soccer field
60  * @param field_width width of soccer field
61  * @param field_border size of border around the field (i.e. distance between the
62  * outer white line and the physical field end)
63  * @param image_width image width in pixels
64  * @param image_height image height in pixels
65  * @param camera_height height of camera above ground
66  * @param camera_ori orientation of camera on the robot in degrees
67  * @param horizontal_angle horizontal viewing angle in degrees
68  * @param vertical_angle vertical viewing angle in degrees
69  */
70 CornerHorizon::CornerHorizon(ScanlineModel *model,
71  float field_length, float field_width, float field_border,
72  unsigned int image_width, unsigned int image_height,
73  float camera_height, float camera_ori,
74  float horizontal_angle, float vertical_angle
75  )
76 {
77  this->model = model;
78 
79  this->field_length = field_length;
80  this->field_width = field_width;
81  this->field_border = field_border;
82 
83  this->image_width = image_width;
84  this->image_height = image_height;
85  this->horizontal_angle = deg2rad( horizontal_angle );
86  this->vertical_angle = deg2rad( vertical_angle );
87  this->camera_ori = deg2rad( camera_ori );
88  this->camera_height = camera_height;
89 
90  pan_pixel_per_rad = this->image_width / this->horizontal_angle;
91  tilt_pixel_per_rad = this->image_height / this->vertical_angle;
92 
93  calculated = false;
94 
95  coord.x = coord.y = 0;
96 }
97 
98 
99 /** Destructor.
100  * Not that this deletes the supplied model!
101  */
102 CornerHorizon::~CornerHorizon()
103 {
104  delete model;
105 }
106 
107 
108 upoint_t
109 CornerHorizon::operator*()
110 {
111  return coord;
112 }
113 
114 
115 upoint_t*
116 CornerHorizon::operator->()
117 {
118  return &coord;
119 }
120 
121 
122 /** Calculate horizon point. */
123 void
124 CornerHorizon::calculate()
125 {
126 
127  float phi = normalize_mirror_rad( pose_ori + pan );
128 
129  float corner_x, corner_y;
130 
131  if ( (phi > 0) && (phi <= M_PI_HALF) ) {
132  corner_x = field_length / 2 + field_border;
133  corner_y = field_width / 2 + field_border;
134  } else if ( (phi > M_PI_HALF) && (phi <= M_PI) ) {
135  corner_x = - (field_length / 2 + field_border );
136  corner_y = field_width / 2 + field_border;
137  } else if ( (phi <= 0) && (phi > - M_PI_HALF) ) {
138  corner_x = field_length / 2 + field_border;
139  corner_y = - (field_width / 2 + field_border);
140  } else /* if (phi <= - M_PI_HALF) */ {
141  corner_x = - (field_length / 2 + field_border );
142  corner_y = - (field_width / 2 + field_border);
143  }
144 
145  float d_x = corner_x - pose_x;
146  float d_y = corner_y - pose_y;
147 
148  float d = sqrt( d_x * d_x + d_y * d_y );
149 
150  float alpha = atan2f( d, camera_height );
151  float beta = M_PI_HALF - alpha;
152 
153  int hor = (int)round((beta + tilt) * tilt_pixel_per_rad);
154 
155  if ((unsigned int)abs(hor) >= (image_height / 2)) {
156  if ( hor < 0 ) {
157  hor = - ( image_height / 2 );
158  } else {
159  hor = image_height / 2;
160  }
161  }
162 
163  horizon = image_height / 2 + hor;
164 
165  /*
166  cout << "Calculated: " << endl
167  << " phi=" << phi << endl
168  << " corner_x=" << corner_x << endl
169  << " corner_y=" << corner_y << endl
170  << " d_x=" << d_x << endl
171  << " d_y=" << d_y << endl
172  << " d=" << d << endl
173  << " alpha=" << alpha << endl
174  << " beta=" << beta << endl
175  << " hor=" << hor << endl
176  << " horizon=" << horizon << endl
177  << " pan_pixel_per_rad=" << pan_pixel_per_rad << endl
178  << " tilt_pixel_per_rad=" << tilt_pixel_per_rad << endl;
179  */
180 
181 }
182 
183 
184 upoint_t *
185 CornerHorizon::operator++()
186 {
187  if ( ! calculated) {
188  calculate();
189  calculated = true;
190  }
191 
192  coord.x = (*model)->x;
193  coord.y = (*model)->y;
194 
195  do {
196  ++(*model);
197  } while ( ((*model)->y < horizon) && ( ! model->finished()) );
198 
199  if ( ((*model)->y < horizon) || model->finished() ) {
200  // finished
201  //cout << "1 (" << coord.x << "," << coord.y << ")" << endl;
202  return &coord;
203  } else {
204  coord.x = (*model)->x;
205  coord.y = (*model)->y;
206  //cout << "2 (" << coord.x << "," << coord.y << ")" << endl;
207  return &coord;
208  }
209 }
210 
211 
212 upoint_t *
213 CornerHorizon::operator++(int)
214 {
215  if ( ! calculated) {
216  calculate();
217  calculated = true;
218  }
219  memcpy(&tmp_coord, &coord, sizeof(upoint_t));
220 
221  do {
222  ++(*model);
223  } while ( ((*model)->y < horizon) && ! model->finished() );
224 
225  if ( ((*model)->y >= horizon) && ! model->finished() ) {
226  coord.x = (*model)->x;
227  coord.y = (*model)->y;
228  //cout << "3 (" << coord.x << "," << coord.y << ")" << endl;
229  }
230 
231  return &tmp_coord;
232 }
233 
234 
235 bool
236 CornerHorizon::finished()
237 {
238  return model->finished();
239 }
240 
241 
242 void
243 CornerHorizon::reset()
244 {
245  calculated = false;
246  coord.x = coord.y = 0;
247  model->reset();
248 }
249 
250 
251 const char *
252 CornerHorizon::get_name()
253 {
254  return "ScanlineModel::CornerHorizon";
255 }
256 
257 
258 unsigned int
259 CornerHorizon::get_margin()
260 {
261  return model->get_margin();
262 }
263 
264 
265 /** Get the horizon point.
266  * @return y coordinate of the horizon point.
267  */
268 unsigned int
269 CornerHorizon::getHorizon()
270 {
271  return horizon;
272 }
273 
274 
275 void
276 CornerHorizon::set_robot_pose(float x, float y, float ori)
277 {
278  pose_x = x;
279  pose_y = y;
280  pose_ori = ori;
281 }
282 
283 
284 void
285 CornerHorizon::set_pan_tilt(float pan, float tilt)
286 {
287  this->pan = pan;
288  this->tilt = tilt;
289 }
290 
291 } // end namespace firevision
Scanline model interface.
Definition: scanlinemodel.h:55
Fawkes library namespace.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:75
Point with cartesian coordinates as unsigned integers.
Definition: types.h:34
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37