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
32using namespace fawkes;
33
34namespace firevision {
35
36const 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 */
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 */
104{
105 delete model;
106}
107
110{
111 return coord;
112}
113
114upoint_t *
116{
117 return &coord;
118}
119
120/** Calculate horizon point. */
121void
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
179upoint_t *
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
206upoint_t *
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
228bool
230{
231 return model->finished();
232}
233
234void
236{
237 calculated = false;
238 coord.x = coord.y = 0;
239 model->reset();
240}
241
242const char *
244{
245 return "ScanlineModel::CornerHorizon";
246}
247
248unsigned int
250{
251 return model->get_margin();
252}
253
254/** Get the horizon point.
255 * @return y coordinate of the horizon point.
256 */
257unsigned int
259{
260 return horizon;
261}
262
263void
264CornerHorizon::set_robot_pose(float x, float y, float ori)
265{
266 pose_x = x;
267 pose_y = y;
268 pose_ori = ori;
269}
270
271void
272CornerHorizon::set_pan_tilt(float pan, float tilt)
273{
274 this->pan = pan;
275 this->tilt = tilt;
276}
277
278} // end namespace firevision
virtual ~CornerHorizon()
Destructor.
bool finished()
Check if all desired points have been processed.
void reset()
Reset model.
unsigned int getHorizon()
Get the horizon point.
fawkes::upoint_t * operator++()
Postfix ++ operator.
CornerHorizon(ScanlineModel *model, float field_length, float field_width, float field_border, unsigned int image_width, unsigned int image_height, float camera_height, float camera_ori, float horizontal_angle, float vertical_angle)
Constructor.
unsigned int get_margin()
Get margin around points.
fawkes::upoint_t operator*()
Get the current coordinate.
void set_pan_tilt(float pan, float tilt)
Set camera's pan/tilt values.
void set_robot_pose(float x, float y, float ori)
Set the robot's pose.
const char * get_name()
Get name of scanline model.
void calculate()
Calculate horizon point.
fawkes::upoint_t * operator->()
Get pointer to current point.
Scanline model interface.
Definition: scanlinemodel.h:53
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:72
Point with cartesian coordinates as unsigned integers.
Definition: types.h:35
unsigned int x
x coordinate
Definition: types.h:36
unsigned int y
y coordinate
Definition: types.h:37