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 
29 #include <cstdlib>
30 #include <cstring>
31 
32 using namespace fawkes;
33 
34 namespace firevision {
35 
36 const float CornerHorizon::M_PI_HALF = M_PI / 2.f;
37 
38 /** @class CornerHorizon <fvmodels/scanlines/cornerhorizon.h>
39  * Cut of arbitrary scanline models at an artificial horizon.
40  * The artificial horizon is calculated by the highest corner that is visible
41  * in the image. From that the Y coordinate in the image is used and everything
42  * above that point is ignored from the scanline grid.
43  *
44  * This class was written in a one-night hacking sensation at RoboLudens 2006
45  * in Eindhoven. For that time it is pretty readable code and we are using it
46  * since then. Cool!
47  *
48  * @author Tim Niemueller
49  * @author Stefan Schiffer
50  * @author Christoph Mies
51  */
52 
53 /** Constructor.
54  * @param model Model to apply the artificial horizon on. This model is deleted on
55  * the destruction of the CornerHorizon instance so you can forget about it in the
56  * using application.
57  * @param field_length length of soccer field
58  * @param field_width width of soccer field
59  * @param field_border size of border around the field (i.e. distance between the
60  * outer white line and the physical field end)
61  * @param image_width image width in pixels
62  * @param image_height image height in pixels
63  * @param camera_height height of camera above ground
64  * @param camera_ori orientation of camera on the robot in degrees
65  * @param horizontal_angle horizontal viewing angle in degrees
66  * @param vertical_angle vertical viewing angle in degrees
67  */
68 CornerHorizon::CornerHorizon(ScanlineModel *model,
69  float field_length,
70  float field_width,
71  float field_border,
72  unsigned int image_width,
73  unsigned int image_height,
74  float camera_height,
75  float camera_ori,
76  float horizontal_angle,
77  float vertical_angle)
78 {
79  this->model = model;
80 
81  this->field_length = field_length;
82  this->field_width = field_width;
83  this->field_border = field_border;
84 
85  this->image_width = image_width;
86  this->image_height = image_height;
87  this->horizontal_angle = deg2rad(horizontal_angle);
88  this->vertical_angle = deg2rad(vertical_angle);
89  this->camera_ori = deg2rad(camera_ori);
90  this->camera_height = camera_height;
91 
92  pan_pixel_per_rad = this->image_width / this->horizontal_angle;
93  tilt_pixel_per_rad = this->image_height / this->vertical_angle;
94 
95  calculated = false;
96 
97  coord.x = coord.y = 0;
98 }
99 
100 /** Destructor.
101  * Not that this deletes the supplied model!
102  */
103 CornerHorizon::~CornerHorizon()
104 {
105  delete model;
106 }
107 
108 upoint_t
109 CornerHorizon::operator*()
110 {
111  return coord;
112 }
113 
114 upoint_t *
115 CornerHorizon::operator->()
116 {
117  return &coord;
118 }
119 
120 /** Calculate horizon point. */
121 void
122 CornerHorizon::calculate()
123 {
124  float phi = normalize_mirror_rad(pose_ori + pan);
125 
126  float corner_x, corner_y;
127 
128  if ((phi > 0) && (phi <= M_PI_HALF)) {
129  corner_x = field_length / 2 + field_border;
130  corner_y = field_width / 2 + field_border;
131  } else if ((phi > M_PI_HALF) && (phi <= M_PI)) {
132  corner_x = -(field_length / 2 + field_border);
133  corner_y = field_width / 2 + field_border;
134  } else if ((phi <= 0) && (phi > -M_PI_HALF)) {
135  corner_x = field_length / 2 + field_border;
136  corner_y = -(field_width / 2 + field_border);
137  } else /* if (phi <= - M_PI_HALF) */ {
138  corner_x = -(field_length / 2 + field_border);
139  corner_y = -(field_width / 2 + field_border);
140  }
141 
142  float d_x = corner_x - pose_x;
143  float d_y = corner_y - pose_y;
144 
145  float d = sqrt(d_x * d_x + d_y * d_y);
146 
147  float alpha = atan2f(d, camera_height);
148  float beta = M_PI_HALF - alpha;
149 
150  int hor = (int)roundf((beta + tilt) * tilt_pixel_per_rad);
151 
152  if ((unsigned int)abs(hor) >= (image_height / 2)) {
153  if (hor < 0) {
154  hor = -(image_height / 2);
155  } else {
156  hor = image_height / 2;
157  }
158  }
159 
160  horizon = image_height / 2 + hor;
161 
162  /*
163  cout << "Calculated: " << endl
164  << " phi=" << phi << endl
165  << " corner_x=" << corner_x << endl
166  << " corner_y=" << corner_y << endl
167  << " d_x=" << d_x << endl
168  << " d_y=" << d_y << endl
169  << " d=" << d << endl
170  << " alpha=" << alpha << endl
171  << " beta=" << beta << endl
172  << " hor=" << hor << endl
173  << " horizon=" << horizon << endl
174  << " pan_pixel_per_rad=" << pan_pixel_per_rad << endl
175  << " tilt_pixel_per_rad=" << tilt_pixel_per_rad << endl;
176  */
177 }
178 
179 upoint_t *
180 CornerHorizon::operator++()
181 {
182  if (!calculated) {
183  calculate();
184  calculated = true;
185  }
186 
187  coord.x = (*model)->x;
188  coord.y = (*model)->y;
189 
190  do {
191  ++(*model);
192  } while (((*model)->y < horizon) && (!model->finished()));
193 
194  if (((*model)->y < horizon) || model->finished()) {
195  // finished
196  //cout << "1 (" << coord.x << "," << coord.y << ")" << endl;
197  return &coord;
198  } else {
199  coord.x = (*model)->x;
200  coord.y = (*model)->y;
201  //cout << "2 (" << coord.x << "," << coord.y << ")" << endl;
202  return &coord;
203  }
204 }
205 
206 upoint_t *
207 CornerHorizon::operator++(int)
208 {
209  if (!calculated) {
210  calculate();
211  calculated = true;
212  }
213  memcpy(&tmp_coord, &coord, sizeof(upoint_t));
214 
215  do {
216  ++(*model);
217  } while (((*model)->y < horizon) && !model->finished());
218 
219  if (((*model)->y >= horizon) && !model->finished()) {
220  coord.x = (*model)->x;
221  coord.y = (*model)->y;
222  //cout << "3 (" << coord.x << "," << coord.y << ")" << endl;
223  }
224 
225  return &tmp_coord;
226 }
227 
228 bool
229 CornerHorizon::finished()
230 {
231  return model->finished();
232 }
233 
234 void
235 CornerHorizon::reset()
236 {
237  calculated = false;
238  coord.x = coord.y = 0;
239  model->reset();
240 }
241 
242 const char *
243 CornerHorizon::get_name()
244 {
245  return "ScanlineModel::CornerHorizon";
246 }
247 
248 unsigned int
249 CornerHorizon::get_margin()
250 {
251  return model->get_margin();
252 }
253 
254 /** Get the horizon point.
255  * @return y coordinate of the horizon point.
256  */
257 unsigned int
258 CornerHorizon::getHorizon()
259 {
260  return horizon;
261 }
262 
263 void
264 CornerHorizon::set_robot_pose(float x, float y, float ori)
265 {
266  pose_x = x;
267  pose_y = y;
268  pose_ori = ori;
269 }
270 
271 void
272 CornerHorizon::set_pan_tilt(float pan, float tilt)
273 {
274  this->pan = pan;
275  this->tilt = tilt;
276 }
277 
278 } // end namespace firevision
fawkes::upoint_t
Point with cartesian coordinates as unsigned integers.
Definition: types.h:35
firevision::ScanlineModel
Scanline model interface.
Definition: scanlinemodel.h:53
fawkes
Fawkes library namespace.
fawkes::deg2rad
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
fawkes::normalize_mirror_rad
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72