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 CornerHorizon::operator*()
109 {
110  return coord;
111 }
112 
113 upoint_t *CornerHorizon::operator->()
114 {
115  return &coord;
116 }
117 
118 /** Calculate horizon point. */
119 void
120 CornerHorizon::calculate()
121 {
122  float phi = normalize_mirror_rad(pose_ori + pan);
123 
124  float corner_x, corner_y;
125 
126  if ((phi > 0) && (phi <= M_PI_HALF)) {
127  corner_x = field_length / 2 + field_border;
128  corner_y = field_width / 2 + field_border;
129  } else if ((phi > M_PI_HALF) && (phi <= M_PI)) {
130  corner_x = -(field_length / 2 + field_border);
131  corner_y = field_width / 2 + field_border;
132  } else if ((phi <= 0) && (phi > -M_PI_HALF)) {
133  corner_x = field_length / 2 + field_border;
134  corner_y = -(field_width / 2 + field_border);
135  } else /* if (phi <= - M_PI_HALF) */ {
136  corner_x = -(field_length / 2 + field_border);
137  corner_y = -(field_width / 2 + field_border);
138  }
139 
140  float d_x = corner_x - pose_x;
141  float d_y = corner_y - pose_y;
142 
143  float d = sqrt(d_x * d_x + d_y * d_y);
144 
145  float alpha = atan2f(d, camera_height);
146  float beta = M_PI_HALF - alpha;
147 
148  int hor = (int)roundf((beta + tilt) * tilt_pixel_per_rad);
149 
150  if ((unsigned int)abs(hor) >= (image_height / 2)) {
151  if (hor < 0) {
152  hor = -(image_height / 2);
153  } else {
154  hor = image_height / 2;
155  }
156  }
157 
158  horizon = image_height / 2 + hor;
159 
160  /*
161  cout << "Calculated: " << endl
162  << " phi=" << phi << endl
163  << " corner_x=" << corner_x << endl
164  << " corner_y=" << corner_y << endl
165  << " d_x=" << d_x << endl
166  << " d_y=" << d_y << endl
167  << " d=" << d << endl
168  << " alpha=" << alpha << endl
169  << " beta=" << beta << endl
170  << " hor=" << hor << endl
171  << " horizon=" << horizon << endl
172  << " pan_pixel_per_rad=" << pan_pixel_per_rad << endl
173  << " tilt_pixel_per_rad=" << tilt_pixel_per_rad << endl;
174  */
175 }
176 
177 upoint_t *
178 CornerHorizon::operator++()
179 {
180  if (!calculated) {
181  calculate();
182  calculated = true;
183  }
184 
185  coord.x = (*model)->x;
186  coord.y = (*model)->y;
187 
188  do {
189  ++(*model);
190  } while (((*model)->y < horizon) && (!model->finished()));
191 
192  if (((*model)->y < horizon) || model->finished()) {
193  // finished
194  //cout << "1 (" << coord.x << "," << coord.y << ")" << endl;
195  return &coord;
196  } else {
197  coord.x = (*model)->x;
198  coord.y = (*model)->y;
199  //cout << "2 (" << coord.x << "," << coord.y << ")" << endl;
200  return &coord;
201  }
202 }
203 
204 upoint_t *
205 CornerHorizon::operator++(int)
206 {
207  if (!calculated) {
208  calculate();
209  calculated = true;
210  }
211  memcpy(&tmp_coord, &coord, sizeof(upoint_t));
212 
213  do {
214  ++(*model);
215  } while (((*model)->y < horizon) && !model->finished());
216 
217  if (((*model)->y >= horizon) && !model->finished()) {
218  coord.x = (*model)->x;
219  coord.y = (*model)->y;
220  //cout << "3 (" << coord.x << "," << coord.y << ")" << endl;
221  }
222 
223  return &tmp_coord;
224 }
225 
226 bool
227 CornerHorizon::finished()
228 {
229  return model->finished();
230 }
231 
232 void
233 CornerHorizon::reset()
234 {
235  calculated = false;
236  coord.x = coord.y = 0;
237  model->reset();
238 }
239 
240 const char *
241 CornerHorizon::get_name()
242 {
243  return "ScanlineModel::CornerHorizon";
244 }
245 
246 unsigned int
247 CornerHorizon::get_margin()
248 {
249  return model->get_margin();
250 }
251 
252 /** Get the horizon point.
253  * @return y coordinate of the horizon point.
254  */
255 unsigned int
256 CornerHorizon::getHorizon()
257 {
258  return horizon;
259 }
260 
261 void
262 CornerHorizon::set_robot_pose(float x, float y, float ori)
263 {
264  pose_x = x;
265  pose_y = y;
266  pose_ori = ori;
267 }
268 
269 void
270 CornerHorizon::set_pan_tilt(float pan, float tilt)
271 {
272  this->pan = pan;
273  this->tilt = tilt;
274 }
275 
276 } // end namespace firevision
fawkes::upoint_t
Point with cartesian coordinates as unsigned integers.
Definition: types.h:34
firevision::ScanlineModel
Definition: scanlinemodel.h:58
fawkes
fawkes::deg2rad
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:42
fawkes::normalize_mirror_rad
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:78