Fawkes API Fawkes Development Version
front_ball.cpp
1
2/***************************************************************************
3 * front_ball.cpp - Implementation of the relative ball position model for
4 * the front vision
5 *
6 * Created: Fri Jun 03 22:56:22 2005
7 * Copyright 2005 Hu Yuxiao <Yuxiao.Hu@rwth-aachen.de>
8 * 2005-2006 Tim Niemueller [www.niemueller.de]
9 * 2005 Martin Heracles <Martin.Heracles@rwth-aachen.de>
10 *
11 ****************************************************************************/
12
13/* This program is free software; you can redistribute it and/or modify
14 * it under the terms of the GNU General Public License as published by
15 * the Free Software Foundation; either version 2 of the License, or
16 * (at your option) any later version. A runtime exception applies to
17 * this software (see LICENSE.GPL_WRE file mentioned below for details).
18 *
19 * This program is distributed in the hope that it will be useful,
20 * but WITHOUT ANY WARRANTY; without even the implied warranty of
21 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
22 * GNU Library General Public License for more details.
23 *
24 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
25 */
26
27#include <fvmodels/relative_position/front_ball.h>
28#include <utils/math/angle.h>
29
30#include <cmath>
31#include <iostream>
32
33using namespace std;
34using namespace fawkes;
35
36namespace firevision {
37
38/** @class FrontBallRelativePos <fvmodels/relative_position/front_ball.h>
39 * Relative ball position model for front vision.
40 */
41
42/** Constructor.
43 * @param image_width width of image in pixels
44 * @param image_height height of image in pixels
45 * @param camera_height height of camera in meters
46 * @param camera_offset_x camera offset of the motor axis in x direction
47 * @param camera_offset_y camera offset of the motor axis in y direction
48 * @param camera_ori camera orientation compared to the robot
49 * @param horizontal_angle horizontal viewing angle (in degree)
50 * @param vertical_angle vertical viewing angle (in degree)
51 * @param ball_circumference ball circumference
52 */
54 unsigned int image_height,
55 float camera_height,
56 float camera_offset_x,
57 float camera_offset_y,
58 float camera_ori,
59 float horizontal_angle,
60 float vertical_angle,
61 float ball_circumference)
62{
63 this->image_width = image_width;
64 this->image_height = image_height;
65 this->ball_circumference = ball_circumference;
66 this->horizontal_angle = deg2rad(horizontal_angle);
67 this->vertical_angle = deg2rad(vertical_angle);
68 this->camera_orientation = deg2rad(camera_ori);
69 this->camera_height = camera_height;
70 this->camera_offset_x = camera_offset_x;
71 this->camera_offset_y = camera_offset_y;
72
73 m_fRadius = 0.0f;
74 m_cirtCenter.x = 0.0f;
75 m_cirtCenter.y = 0.0f;
76 m_fPan = 0.0f;
77 m_fTilt = 0.0f;
78
79 avg_x = avg_y = avg_x_sum = avg_y_sum = 0.f;
80 avg_x_num = avg_y_num = 0;
81
82 m_fPanRadPerPixel = this->horizontal_angle / this->image_width;
83 m_fTiltRadPerPixel = this->vertical_angle / this->image_height;
84 m_fBallRadius = this->ball_circumference / (2 * M_PI);
85
86 ball_x = ball_y = bearing = slope = distance_ball_motor = distance_ball_cam = 0.f;
87
88 DEFAULT_X_VARIANCE = 1500.f;
89 DEFAULT_Y_VARIANCE = 1000.f;
90
91 var_proc_x = 1500;
92 var_proc_y = 1000;
93 var_meas_x = 1500;
94 var_meas_y = 1000;
95
96 /*
97 // initial variance for ball pos kf
98 kalman_filter = new kalmanFilter2Dim();
99 CMatrix<float> initialStateVarianceBall(2,2);
100 initialStateVarianceBall[0][0] = DEFAULT_X_VARIANCE;
101 initialStateVarianceBall[1][0] = 0.00;
102 initialStateVarianceBall[0][1] = 0.00;
103 initialStateVarianceBall[1][1] = DEFAULT_Y_VARIANCE;
104 kalman_filter->setInitialStateCovariance( initialStateVarianceBall );
105
106 // process noise for ball pos kf
107 kalman_filter->setProcessCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
108 kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
109 */
110}
111
112float
114{
115 return distance_ball_motor;
116}
117
118float
120{
121 return bearing;
122}
123
124float
126{
127 return slope;
128}
129
130float
132{
133 return ball_y;
134}
135
136float
138{
139 return ball_x;
140}
141
142void
144{
145 m_cirtCenter.x = x;
146 m_cirtCenter.y = y;
147}
148
149void
151{
152 m_cirtCenter.x = c.x;
153 m_cirtCenter.y = c.y;
154}
155
156void
158{
159 m_fRadius = r;
160}
161
162/** Get the ball radius.
163 * @return ball radius
164 */
165float
167{
168 return m_fRadius;
169}
170
171void
173{
174 m_fPan = pan;
175 m_fTilt = tilt;
176}
177
178void
179FrontBallRelativePos::get_pan_tilt(float *pan, float *tilt) const
180{
181 *pan = m_fPan;
182 *tilt = m_fTilt;
183}
184
185const char *
187{
188 return "FrontBallRelativePos";
189}
190
191/** Set horizontal viewing angle.
192 * @param angle_deg horizontal viewing angle in degree
193 */
194void
196{
197 horizontal_angle = deg2rad(angle_deg);
198}
199
200/** Set vertical viewing angle.
201 * @param angle_deg horizontal viewing angle in degree
202 */
203void
205{
206 vertical_angle = deg2rad(angle_deg);
207}
208
209void
211{
212 last_available = false;
213 //kalman_filter->reset();
214}
215
216void
218{
219 /*
220 char user_input = toupper( getkey() );
221
222 if (user_input == 'P') {
223 cout << "Enter new kalman process variance values (X Y):" << flush;
224 cin >> var_proc_x >> var_proc_y;
225 } else if (user_input == 'M') {
226 cout << "Enter new kalman measurement variance values (X Y):" << flush;
227 cin >> var_meas_x >> var_meas_y;
228 } else if (user_input == 'R') {
229 cout << "Reset" << endl;
230 reset();
231 }
232 */
233
234 float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
235
236 /* projection of air-line-distance on the ground (Pythagoras) */
237 distance_ball_cam =
238 sqrt(tmp * tmp - (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius));
239
240#ifdef OLD_COORD_SYS
241 /* Bearing shall be clockwise positive. */
242 bearing =
243 (((m_cirtCenter.x - image_width / 2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
244#else
245 /* Bearing shall be counter-clockwise positive. */
246 bearing =
247 -(((m_cirtCenter.x - image_width / 2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
248#endif
249
250 /* Slope shall be downward negative */
251 slope = -((m_cirtCenter.y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
252
253 ball_x = cos(bearing) * distance_ball_cam + camera_offset_x;
254 ball_y = sin(bearing) * distance_ball_cam + camera_offset_y;
255
256 // applyKalmanFilter();
257
258 distance_ball_motor = sqrt(ball_x * ball_x + ball_y * ball_y);
259}
260
261bool
263{
264 return true;
265}
266
267void
269{
270 float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
271
272 /* projection of air-line-distance on the ground
273 (Pythagoras) */
274 distance_ball_cam =
275 sqrt(tmp * tmp - (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius));
276
277#ifdef OLD_COORD_SYS
278 /* Bearing shall be clockwise positive. */
279 bearing =
280 (((m_cirtCenter.x - image_width / 2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
281#else
282 /* Bearing shall be counter-clockwise positive. */
283 bearing =
284 -(((m_cirtCenter.x - image_width / 2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
285#endif
286
287 // invert sign, because slope downward shall be negative
288 slope = -((m_cirtCenter.y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
289
290 ball_x = cos(bearing) * distance_ball_cam + camera_offset_x;
291 ball_y = sin(bearing) * distance_ball_cam + camera_offset_y;
292
293 distance_ball_motor = sqrt(ball_x * ball_x + ball_y * ball_y);
294}
295
296/*
297void
298FrontBallRelativePos::applyKalmanFilter()
299{
300
301 kalman_filter->setMeasurementCovariance( var_meas_x, var_meas_y );
302 kalman_filter->setProcessCovariance( var_proc_x, var_proc_y );
303
304 last_x = ball_x;
305 last_y = ball_y;
306
307 kalman_filter->setMeasurementX( ball_x );
308 kalman_filter->setMeasurementY( ball_y );
309 kalman_filter->doCalculation();
310
311 ball_x = kalman_filter->getStateX();
312 ball_y = kalman_filter->getStateY();
313
314 if ( isnan( ball_x ) || isinf( ball_x ) ||
315 isnan( ball_y ) || isinf( ball_y ) ) {
316 // Kalman is wedged, use unfiltered result and reset filter
317 kalman_filter->reset();
318 ball_x = last_x;
319 ball_y = last_y;
320 }
321
322}
323*/
324
325} // end namespace firevision
virtual void set_pan_tilt(float pan=0.0f, float tilt=0.0f)
Set camera pan and tilt.
Definition: front_ball.cpp:172
virtual float get_distance() const
Get distance to object.
Definition: front_ball.cpp:113
virtual float get_x() const
Get relative X coordinate of object.
Definition: front_ball.cpp:137
virtual void set_vertical_angle(float angle_deg)
Set vertical viewing angle.
Definition: front_ball.cpp:204
virtual void reset()
Reset all data.
Definition: front_ball.cpp:210
virtual const char * get_name() const
Get name of relative position model.
Definition: front_ball.cpp:186
virtual void set_radius(float r)
Set radius of a found circle.
Definition: front_ball.cpp:157
virtual void calc()
Calculate position data.
Definition: front_ball.cpp:217
FrontBallRelativePos(unsigned int image_width, unsigned int image_height, float camera_height, float camera_offset_x, float camera_offset_y, float camera_ori, float horizontal_angle, float vertical_angle, float ball_circumference)
Constructor.
Definition: front_ball.cpp:53
virtual void set_horizontal_angle(float angle_deg)
Set horizontal viewing angle.
Definition: front_ball.cpp:195
virtual float get_bearing() const
Get bearing (horizontal angle) to object.
Definition: front_ball.cpp:119
virtual float get_radius() const
Get the ball radius.
Definition: front_ball.cpp:166
virtual bool is_pos_valid() const
Check if position is valid.
Definition: front_ball.cpp:262
virtual void calc_unfiltered()
Calculate data unfiltered.
Definition: front_ball.cpp:268
virtual void set_center(float x, float y)
Set center of a found circle.
Definition: front_ball.cpp:143
virtual float get_y() const
Get relative Y coordinate of object.
Definition: front_ball.cpp:131
virtual void get_pan_tilt(float *pan, float *tilt) const
Get camera pan tilt.
Definition: front_ball.cpp:179
virtual float get_slope() const
Get slope (vertical angle) to object.
Definition: front_ball.cpp:125
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
Center in ROI.
Definition: types.h:38
float y
y in pixels
Definition: types.h:40
float x
x in pixels
Definition: types.h:39