00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include <cmath>
00028 #include <iostream>
00029 #include <models/relative_position/front_ball.h>
00030 #include <utils/math/angle.h>
00031
00032 using namespace std;
00033 using namespace fawkes;
00034
00035 namespace firevision {
00036 #if 0
00037 }
00038 #endif
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055 FrontBallRelativePos::FrontBallRelativePos(unsigned int image_width,
00056 unsigned int image_height,
00057 float camera_height,
00058 float camera_offset_x,
00059 float camera_offset_y,
00060 float camera_ori,
00061 float horizontal_angle,
00062 float vertical_angle,
00063 float ball_circumference
00064 )
00065 {
00066
00067 this->image_width = image_width;
00068 this->image_height = image_height;
00069 this->ball_circumference = ball_circumference;
00070 this->horizontal_angle = deg2rad( horizontal_angle );
00071 this->vertical_angle = deg2rad( vertical_angle );
00072 this->camera_orientation = deg2rad( camera_ori );
00073 this->camera_height = camera_height;
00074 this->camera_offset_x = camera_offset_x;
00075 this->camera_offset_y = camera_offset_y;
00076
00077 m_fRadius = 0.0f;
00078 m_cirtCenter.x = 0.0f;
00079 m_cirtCenter.y = 0.0f;
00080 m_fPan = 0.0f;
00081 m_fTilt = 0.0f;
00082
00083 avg_x = avg_y = avg_x_sum = avg_y_sum = 0.f;
00084 avg_x_num = avg_y_num = 0;
00085
00086 m_fPanRadPerPixel = this->horizontal_angle / this->image_width;
00087 m_fTiltRadPerPixel = this->vertical_angle / this->image_height;
00088 m_fBallRadius = this->ball_circumference / ( 2 * M_PI );
00089
00090 ball_x = ball_y = bearing = slope = distance_ball_motor = distance_ball_cam = 0.f;
00091
00092 DEFAULT_X_VARIANCE = 1500.f;
00093 DEFAULT_Y_VARIANCE = 1000.f;
00094
00095 var_proc_x = 1500;
00096 var_proc_y = 1000;
00097 var_meas_x = 1500;
00098 var_meas_y = 1000;
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114 }
00115
00116
00117 float
00118 FrontBallRelativePos::get_distance() const
00119 {
00120 return distance_ball_motor;
00121 }
00122
00123
00124 float
00125 FrontBallRelativePos::get_bearing(void) const
00126 {
00127 return bearing;
00128 }
00129
00130
00131 float
00132 FrontBallRelativePos::get_slope() const
00133 {
00134 return slope;
00135 }
00136
00137
00138 float
00139 FrontBallRelativePos::get_y(void) const
00140 {
00141 return ball_y;
00142 }
00143
00144
00145 float
00146 FrontBallRelativePos::get_x(void) const
00147 {
00148 return ball_x;
00149 }
00150
00151
00152 void
00153 FrontBallRelativePos::set_center(float x, float y)
00154 {
00155 m_cirtCenter.x = x;
00156 m_cirtCenter.y = y;
00157 }
00158
00159
00160 void
00161 FrontBallRelativePos::set_center(const center_in_roi_t& c)
00162 {
00163 m_cirtCenter.x = c.x;
00164 m_cirtCenter.y = c.y;
00165 }
00166
00167
00168 void
00169 FrontBallRelativePos::set_radius(float r)
00170 {
00171 m_fRadius = r;
00172 }
00173
00174
00175
00176
00177
00178 float
00179 FrontBallRelativePos::get_radius() const
00180 {
00181 return m_fRadius;
00182 }
00183
00184
00185 void
00186 FrontBallRelativePos::set_pan_tilt(float pan, float tilt)
00187 {
00188 m_fPan = pan;
00189 m_fTilt = tilt;
00190 }
00191
00192
00193 void
00194 FrontBallRelativePos::get_pan_tilt(float *pan, float *tilt) const
00195 {
00196 *pan = m_fPan;
00197 *tilt = m_fTilt;
00198 }
00199
00200
00201 const char *
00202 FrontBallRelativePos::get_name() const
00203 {
00204 return "FrontBallRelativePos";
00205 }
00206
00207
00208
00209
00210
00211 void
00212 FrontBallRelativePos::set_horizontal_angle(float angle_deg)
00213 {
00214 horizontal_angle = deg2rad( angle_deg );
00215 }
00216
00217
00218
00219
00220
00221 void
00222 FrontBallRelativePos::set_vertical_angle(float angle_deg)
00223 {
00224 vertical_angle = deg2rad( angle_deg );
00225 }
00226
00227
00228 void
00229 FrontBallRelativePos::reset()
00230 {
00231 last_available = false;
00232
00233 }
00234
00235 void
00236 FrontBallRelativePos::calc()
00237 {
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254 float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
00255
00256
00257 distance_ball_cam = sqrt( tmp * tmp -
00258 (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius) );
00259
00260
00261 #ifdef OLD_COORD_SYS
00262
00263 bearing = (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
00264 #else
00265
00266 bearing = - (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
00267 #endif
00268
00269
00270 slope = - ((m_cirtCenter.y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
00271
00272 ball_x = cos( bearing ) * distance_ball_cam + camera_offset_x;
00273 ball_y = sin( bearing ) * distance_ball_cam + camera_offset_y;
00274
00275
00276
00277 distance_ball_motor = sqrt( ball_x * ball_x + ball_y * ball_y );
00278
00279 }
00280
00281
00282 bool
00283 FrontBallRelativePos::is_pos_valid() const
00284 {
00285 return true;
00286 }
00287
00288
00289 void
00290 FrontBallRelativePos::calc_unfiltered()
00291 {
00292 float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
00293
00294
00295
00296 distance_ball_cam = sqrt( tmp * tmp -
00297 (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius) );
00298
00299
00300 #ifdef OLD_COORD_SYS
00301
00302 bearing = (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
00303 #else
00304
00305 bearing = - (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
00306 #endif
00307
00308
00309 slope = - ((m_cirtCenter.y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
00310
00311
00312 ball_x = cos( bearing ) * distance_ball_cam + camera_offset_x;
00313 ball_y = sin( bearing ) * distance_ball_cam + camera_offset_y;
00314
00315 distance_ball_motor = sqrt( ball_x * ball_x + ball_y * ball_y );
00316
00317 }
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349 }