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 <cmath>
28 #include <iostream>
29 #include <fvmodels/relative_position/front_ball.h>
30 #include <utils/math/angle.h>
31 
32 using namespace std;
33 using namespace fawkes;
34 
35 namespace firevision {
36 #if 0 /* just to make Emacs auto-indent happy */
37 }
38 #endif
39 
40 /** @class FrontBallRelativePos <fvmodels/relative_position/front_ball.h>
41  * Relative ball position model for front vision.
42  */
43 
44 /** Constructor.
45  * @param image_width width of image in pixels
46  * @param image_height height of image in pixels
47  * @param camera_height height of camera in meters
48  * @param camera_offset_x camera offset of the motor axis in x direction
49  * @param camera_offset_y camera offset of the motor axis in y direction
50  * @param camera_ori camera orientation compared to the robot
51  * @param horizontal_angle horizontal viewing angle (in degree)
52  * @param vertical_angle vertical viewing angle (in degree)
53  * @param ball_circumference ball circumference
54  */
55 FrontBallRelativePos::FrontBallRelativePos(unsigned int image_width,
56  unsigned int image_height,
57  float camera_height,
58  float camera_offset_x,
59  float camera_offset_y,
60  float camera_ori,
61  float horizontal_angle,
62  float vertical_angle,
63  float ball_circumference
64  )
65 {
66 
67  this->image_width = image_width;
68  this->image_height = image_height;
69  this->ball_circumference = ball_circumference;
70  this->horizontal_angle = deg2rad( horizontal_angle );
71  this->vertical_angle = deg2rad( vertical_angle );
72  this->camera_orientation = deg2rad( camera_ori );
73  this->camera_height = camera_height;
74  this->camera_offset_x = camera_offset_x;
75  this->camera_offset_y = camera_offset_y;
76 
77  m_fRadius = 0.0f;
78  m_cirtCenter.x = 0.0f;
79  m_cirtCenter.y = 0.0f;
80  m_fPan = 0.0f;
81  m_fTilt = 0.0f;
82 
83  avg_x = avg_y = avg_x_sum = avg_y_sum = 0.f;
84  avg_x_num = avg_y_num = 0;
85 
86  m_fPanRadPerPixel = this->horizontal_angle / this->image_width;
87  m_fTiltRadPerPixel = this->vertical_angle / this->image_height;
88  m_fBallRadius = this->ball_circumference / ( 2 * M_PI );
89 
90  ball_x = ball_y = bearing = slope = distance_ball_motor = distance_ball_cam = 0.f;
91 
92  DEFAULT_X_VARIANCE = 1500.f;
93  DEFAULT_Y_VARIANCE = 1000.f;
94 
95  var_proc_x = 1500;
96  var_proc_y = 1000;
97  var_meas_x = 1500;
98  var_meas_y = 1000;
99 
100  /*
101  // initial variance for ball pos kf
102  kalman_filter = new kalmanFilter2Dim();
103  CMatrix<float> initialStateVarianceBall(2,2);
104  initialStateVarianceBall[0][0] = DEFAULT_X_VARIANCE;
105  initialStateVarianceBall[1][0] = 0.00;
106  initialStateVarianceBall[0][1] = 0.00;
107  initialStateVarianceBall[1][1] = DEFAULT_Y_VARIANCE;
108  kalman_filter->setInitialStateCovariance( initialStateVarianceBall );
109 
110  // process noise for ball pos kf
111  kalman_filter->setProcessCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
112  kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
113  */
114 }
115 
116 
117 float
118 FrontBallRelativePos::get_distance() const
119 {
120  return distance_ball_motor;
121 }
122 
123 
124 float
125 FrontBallRelativePos::get_bearing(void) const
126 {
127  return bearing;
128 }
129 
130 
131 float
132 FrontBallRelativePos::get_slope() const
133 {
134  return slope;
135 }
136 
137 
138 float
139 FrontBallRelativePos::get_y(void) const
140 {
141  return ball_y;
142 }
143 
144 
145 float
146 FrontBallRelativePos::get_x(void) const
147 {
148  return ball_x;
149 }
150 
151 
152 void
153 FrontBallRelativePos::set_center(float x, float y)
154 {
155  m_cirtCenter.x = x;
156  m_cirtCenter.y = y;
157 }
158 
159 
160 void
161 FrontBallRelativePos::set_center(const center_in_roi_t& c)
162 {
163  m_cirtCenter.x = c.x;
164  m_cirtCenter.y = c.y;
165 }
166 
167 
168 void
169 FrontBallRelativePos::set_radius(float r)
170 {
171  m_fRadius = r;
172 }
173 
174 
175 /** Get the ball radius.
176  * @return ball radius
177  */
178 float
179 FrontBallRelativePos::get_radius() const
180 {
181  return m_fRadius;
182 }
183 
184 
185 void
186 FrontBallRelativePos::set_pan_tilt(float pan, float tilt)
187 {
188  m_fPan = pan;
189  m_fTilt = tilt;
190 }
191 
192 
193 void
194 FrontBallRelativePos::get_pan_tilt(float *pan, float *tilt) const
195 {
196  *pan = m_fPan;
197  *tilt = m_fTilt;
198 }
199 
200 
201 const char *
202 FrontBallRelativePos::get_name() const
203 {
204  return "FrontBallRelativePos";
205 }
206 
207 
208 /** Set horizontal viewing angle.
209  * @param angle_deg horizontal viewing angle in degree
210  */
211 void
212 FrontBallRelativePos::set_horizontal_angle(float angle_deg)
213 {
214  horizontal_angle = deg2rad( angle_deg );
215 }
216 
217 
218 /** Set vertical viewing angle.
219  * @param angle_deg horizontal viewing angle in degree
220  */
221 void
222 FrontBallRelativePos::set_vertical_angle(float angle_deg)
223 {
224  vertical_angle = deg2rad( angle_deg );
225 }
226 
227 
228 void
229 FrontBallRelativePos::reset()
230 {
231  last_available = false;
232  //kalman_filter->reset();
233 }
234 
235 void
236 FrontBallRelativePos::calc()
237 {
238 
239  /*
240  char user_input = toupper( getkey() );
241 
242  if (user_input == 'P') {
243  cout << "Enter new kalman process variance values (X Y):" << flush;
244  cin >> var_proc_x >> var_proc_y;
245  } else if (user_input == 'M') {
246  cout << "Enter new kalman measurement variance values (X Y):" << flush;
247  cin >> var_meas_x >> var_meas_y;
248  } else if (user_input == 'R') {
249  cout << "Reset" << endl;
250  reset();
251  }
252  */
253 
254  float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
255 
256  /* projection of air-line-distance on the ground (Pythagoras) */
257  distance_ball_cam = sqrt( tmp * tmp -
258  (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius) );
259 
260 
261 #ifdef OLD_COORD_SYS
262  /* Bearing shall be clockwise positive. */
263  bearing = (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
264 #else
265  /* Bearing shall be counter-clockwise positive. */
266  bearing = - (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
267 #endif
268 
269  /* Slope shall be downward negative */
270  slope = - ((m_cirtCenter.y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
271 
272  ball_x = cos( bearing ) * distance_ball_cam + camera_offset_x;
273  ball_y = sin( bearing ) * distance_ball_cam + camera_offset_y;
274 
275  // applyKalmanFilter();
276 
277  distance_ball_motor = sqrt( ball_x * ball_x + ball_y * ball_y );
278 
279 }
280 
281 
282 bool
283 FrontBallRelativePos::is_pos_valid() const
284 {
285  return true;
286 }
287 
288 
289 void
290 FrontBallRelativePos::calc_unfiltered()
291 {
292  float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
293 
294  /* projection of air-line-distance on the ground
295  (Pythagoras) */
296  distance_ball_cam = sqrt( tmp * tmp -
297  (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius) );
298 
299 
300 #ifdef OLD_COORD_SYS
301  /* Bearing shall be clockwise positive. */
302  bearing = (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
303 #else
304  /* Bearing shall be counter-clockwise positive. */
305  bearing = - (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
306 #endif
307 
308  // invert sign, because slope downward shall be negative
309  slope = - ((m_cirtCenter.y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
310 
311 
312  ball_x = cos( bearing ) * distance_ball_cam + camera_offset_x;
313  ball_y = sin( bearing ) * distance_ball_cam + camera_offset_y;
314 
315  distance_ball_motor = sqrt( ball_x * ball_x + ball_y * ball_y );
316 
317 }
318 
319 
320 /*
321 void
322 FrontBallRelativePos::applyKalmanFilter()
323 {
324 
325  kalman_filter->setMeasurementCovariance( var_meas_x, var_meas_y );
326  kalman_filter->setProcessCovariance( var_proc_x, var_proc_y );
327 
328  last_x = ball_x;
329  last_y = ball_y;
330 
331  kalman_filter->setMeasurementX( ball_x );
332  kalman_filter->setMeasurementY( ball_y );
333  kalman_filter->doCalculation();
334 
335  ball_x = kalman_filter->getStateX();
336  ball_y = kalman_filter->getStateY();
337 
338  if ( isnan( ball_x ) || isinf( ball_x ) ||
339  isnan( ball_y ) || isinf( ball_y ) ) {
340  // Kalman is wedged, use unfiltered result and reset filter
341  kalman_filter->reset();
342  ball_x = last_x;
343  ball_y = last_y;
344  }
345 
346 }
347 */
348 
349 } // end namespace firevision
Fawkes library namespace.
STL namespace.
float x
x in pixels
Definition: types.h:40
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
Center in ROI.
Definition: types.h:39
float y
y in pixels
Definition: types.h:41