omni_relative.cpp
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 #include <cmath>
00026 #include <iostream>
00027 #include <models/relative_position/omni_relative.h>
00028
00029 namespace firevision {
00030 #if 0
00031 }
00032 #endif
00033
00034
00035
00036
00037
00038
00039
00040
00041 OmniRelative::OmniRelative(MirrorModel *mirror_model)
00042 {
00043 this->mirror_model = mirror_model;
00044
00045 avg_x = avg_y = avg_x_sum = avg_y_sum = 0.f;
00046 avg_x_num = avg_y_num = 0;
00047
00048 ball_x = ball_y = bearing = slope = distance_ball_motor = distance_ball_cam = 0.f;
00049
00050
00051 slope = 0;
00052
00053 DEFAULT_X_VARIANCE = 36.f;
00054 DEFAULT_Y_VARIANCE = 25.f;
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071 }
00072
00073
00074 float
00075 OmniRelative::get_distance() const
00076 {
00077 return distance_ball_motor;
00078 }
00079
00080
00081 float
00082 OmniRelative::get_bearing(void) const
00083 {
00084 return bearing;
00085 }
00086
00087
00088 float
00089 OmniRelative::get_slope() const
00090 {
00091 return slope;
00092 }
00093
00094
00095 float
00096 OmniRelative::get_y(void) const
00097 {
00098 return ball_y;
00099 }
00100
00101
00102 float
00103 OmniRelative::get_x(void) const
00104 {
00105 return ball_x;
00106 }
00107
00108
00109 void
00110 OmniRelative::set_center(float x, float y)
00111 {
00112 image_x = (unsigned int)roundf(x);
00113 image_y = (unsigned int)roundf(y);
00114 }
00115
00116
00117 void
00118 OmniRelative::set_center(const center_in_roi_t& c)
00119 {
00120 }
00121
00122
00123 void
00124 OmniRelative::set_radius(float r)
00125 {
00126 }
00127
00128
00129
00130
00131
00132 float
00133 OmniRelative::get_radius() const
00134 {
00135 return 0;
00136 }
00137
00138
00139 void
00140 OmniRelative::set_pan_tilt(float pan, float tilt)
00141 {
00142 }
00143
00144
00145 void
00146 OmniRelative::get_pan_tilt(float *pan, float *tilt) const
00147 {
00148 }
00149
00150
00151 const char *
00152 OmniRelative::get_name() const
00153 {
00154 return "OmniRelative";
00155 }
00156
00157
00158 void
00159 OmniRelative::reset()
00160 {
00161 last_available = false;
00162
00163 }
00164
00165
00166 void
00167 OmniRelative::calc()
00168 {
00169
00170 if ( mirror_model->isValidPoint( image_x, image_y ) ) {
00171 fawkes::polar_coord_2d_t rel_pos = mirror_model->getWorldPointRelative( image_x, image_y );
00172
00173 distance_ball_cam = rel_pos.r;
00174 bearing = rel_pos.phi;
00175
00176
00177 ball_x = cos( bearing ) * distance_ball_cam;
00178 ball_y = sin( bearing ) * distance_ball_cam;
00179
00180
00181
00182 distance_ball_motor = sqrt( ball_x * ball_x + ball_y * ball_y );
00183 }
00184 }
00185
00186
00187 bool
00188 OmniRelative::is_pos_valid() const
00189 {
00190 return mirror_model->isValidPoint( image_x, image_y );
00191 }
00192
00193
00194 void
00195 OmniRelative::calc_unfiltered()
00196 {
00197
00198 fawkes::polar_coord_2d_t rel_pos = mirror_model->getWorldPointRelative( image_x, image_y );
00199
00200 distance_ball_cam = rel_pos.r;
00201 bearing = rel_pos.phi;
00202
00203
00204 slope = 0;
00205
00206
00207 ball_x = cos( bearing ) * distance_ball_cam;
00208 ball_y = sin( bearing ) * distance_ball_cam;
00209
00210 distance_ball_motor = sqrt( ball_x * ball_x + ball_y * ball_y );
00211 }
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258 }