00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include <models/relative_position/ball_trigo.h>
00025 #include <utils/math/angle.h>
00026
00027 #include <cmath>
00028
00029 using namespace std;
00030 using namespace fawkes;
00031
00032 namespace firevision {
00033 #if 0
00034 }
00035 #endif
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057 BallTrigoRelativePos::BallTrigoRelativePos(unsigned int image_width,
00058 unsigned int image_height,
00059 float camera_height,
00060 float camera_offset_x,
00061 float camera_offset_y,
00062 float camera_base_pan,
00063 float camera_base_tilt,
00064 float horizontal_angle,
00065 float vertical_angle,
00066 float ball_circumference)
00067 {
00068 __image_width = image_width;
00069 __image_width_2 = __image_width / 2;
00070 __image_height = image_height;
00071 __image_height_2 = __image_height / 2;
00072 __ball_circumference = ball_circumference;
00073 __camera_height = camera_height;
00074 __camera_offset_x = camera_offset_x;
00075 __camera_offset_y = camera_offset_y;
00076 __camera_base_pan = camera_base_pan;
00077 __camera_base_tilt = camera_base_tilt;
00078 __horizontal_angle = deg2rad( horizontal_angle );
00079 __vertical_angle = deg2rad( vertical_angle );
00080
00081 __cirt_center.x = 0.0f;
00082 __cirt_center.y = 0.0f;
00083 __pan = 0.0f;
00084 __tilt = 0.0f;
00085
00086 __pan_rad_per_pixel = __horizontal_angle / (float)__image_width;
00087 __tilt_rad_per_pixel = __vertical_angle / (float)__image_height;
00088 __ball_radius = __ball_circumference / ( 2.0 * M_PI );
00089
00090 __ball_x = __ball_y = __bearing = __slope = __distance = 0.f;
00091 }
00092
00093
00094 float
00095 BallTrigoRelativePos::get_distance() const
00096 {
00097 return __distance;
00098 }
00099
00100
00101 float
00102 BallTrigoRelativePos::get_bearing() const
00103 {
00104 return __bearing;
00105 }
00106
00107
00108 float
00109 BallTrigoRelativePos::get_slope() const
00110 {
00111 return __slope;
00112 }
00113
00114
00115 float
00116 BallTrigoRelativePos::get_y() const
00117 {
00118 return __ball_y;
00119 }
00120
00121
00122 float
00123 BallTrigoRelativePos::get_x() const
00124 {
00125 return __ball_x;
00126 }
00127
00128
00129 void
00130 BallTrigoRelativePos::set_center(float x, float y)
00131 {
00132 __cirt_center.x = x;
00133 __cirt_center.y = y;
00134 }
00135
00136
00137 void
00138 BallTrigoRelativePos::set_center(const center_in_roi_t& c)
00139 {
00140 __cirt_center.x = c.x;
00141 __cirt_center.y = c.y;
00142 }
00143
00144
00145 void
00146 BallTrigoRelativePos::set_radius(float r)
00147 {
00148 }
00149
00150
00151 void
00152 BallTrigoRelativePos::set_pan_tilt(float pan, float tilt)
00153 {
00154 __pan = pan;
00155 __tilt = tilt;
00156 }
00157
00158
00159 void
00160 BallTrigoRelativePos::get_pan_tilt(float *pan, float *tilt) const
00161 {
00162 *pan = __pan;
00163 *tilt = __tilt;
00164 }
00165
00166
00167 const char *
00168 BallTrigoRelativePos::get_name() const
00169 {
00170 return "BallTrigoRelativePos";
00171 }
00172
00173
00174 void
00175 BallTrigoRelativePos::reset()
00176 {
00177 }
00178
00179 void
00180 BallTrigoRelativePos::calc()
00181 {
00182 #ifdef OLD_COORD_SYS
00183
00184 __bearing = (((__cirt_center.x - __image_width_2) * __pan_rad_per_pixel
00185 + __pan + __camera_base_pan));
00186 #else
00187
00188 __bearing = - (((__cirt_center.x - __image_width_2) * __pan_rad_per_pixel
00189 + __pan + __camera_base_pan));
00190 #endif
00191
00192
00193 __slope = ((__image_height_2 - __cirt_center.y) * __tilt_rad_per_pixel
00194 + __tilt + __camera_base_tilt);
00195
00196 float alpha = M_PI_2 - __slope;
00197
00198 float e = __camera_height - __ball_radius - __ball_radius * cos(alpha);
00199 __distance = - (e * tan(alpha) + __ball_radius * sin(alpha));
00200
00201 __ball_x = cos( __bearing ) * __distance + __camera_offset_x;
00202 __ball_y = sin( __bearing ) * __distance + __camera_offset_y;
00203 }
00204
00205
00206 bool
00207 BallTrigoRelativePos::is_pos_valid() const
00208 {
00209 return __distance > 0;
00210 }
00211
00212 }