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 
33 using namespace std;
34 using namespace fawkes;
35 
36 namespace 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  */
53 FrontBallRelativePos::FrontBallRelativePos(unsigned int image_width,
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 
112 float
113 FrontBallRelativePos::get_distance() const
114 {
115  return distance_ball_motor;
116 }
117 
118 float
119 FrontBallRelativePos::get_bearing(void) const
120 {
121  return bearing;
122 }
123 
124 float
125 FrontBallRelativePos::get_slope() const
126 {
127  return slope;
128 }
129 
130 float
131 FrontBallRelativePos::get_y(void) const
132 {
133  return ball_y;
134 }
135 
136 float
137 FrontBallRelativePos::get_x(void) const
138 {
139  return ball_x;
140 }
141 
142 void
143 FrontBallRelativePos::set_center(float x, float y)
144 {
145  m_cirtCenter.x = x;
146  m_cirtCenter.y = y;
147 }
148 
149 void
150 FrontBallRelativePos::set_center(const center_in_roi_t &c)
151 {
152  m_cirtCenter.x = c.x;
153  m_cirtCenter.y = c.y;
154 }
155 
156 void
157 FrontBallRelativePos::set_radius(float r)
158 {
159  m_fRadius = r;
160 }
161 
162 /** Get the ball radius.
163  * @return ball radius
164  */
165 float
166 FrontBallRelativePos::get_radius() const
167 {
168  return m_fRadius;
169 }
170 
171 void
172 FrontBallRelativePos::set_pan_tilt(float pan, float tilt)
173 {
174  m_fPan = pan;
175  m_fTilt = tilt;
176 }
177 
178 void
179 FrontBallRelativePos::get_pan_tilt(float *pan, float *tilt) const
180 {
181  *pan = m_fPan;
182  *tilt = m_fTilt;
183 }
184 
185 const char *
186 FrontBallRelativePos::get_name() const
187 {
188  return "FrontBallRelativePos";
189 }
190 
191 /** Set horizontal viewing angle.
192  * @param angle_deg horizontal viewing angle in degree
193  */
194 void
195 FrontBallRelativePos::set_horizontal_angle(float angle_deg)
196 {
197  horizontal_angle = deg2rad(angle_deg);
198 }
199 
200 /** Set vertical viewing angle.
201  * @param angle_deg horizontal viewing angle in degree
202  */
203 void
204 FrontBallRelativePos::set_vertical_angle(float angle_deg)
205 {
206  vertical_angle = deg2rad(angle_deg);
207 }
208 
209 void
210 FrontBallRelativePos::reset()
211 {
212  last_available = false;
213  //kalman_filter->reset();
214 }
215 
216 void
217 FrontBallRelativePos::calc()
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 
261 bool
262 FrontBallRelativePos::is_pos_valid() const
263 {
264  return true;
265 }
266 
267 void
268 FrontBallRelativePos::calc_unfiltered()
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 /*
297 void
298 FrontBallRelativePos::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
fawkes
fawkes::deg2rad
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:40
firevision::center_in_roi_t::x
float x
x in pixels
Definition: types.h:50
firevision::center_in_roi_t::y
float y
y in pixels
Definition: types.h:51
firevision::center_in_roi_t
Center in ROI.
Definition: types.h:42