24 #include <fvmodels/relative_position/ball_trigo.h> 25 #include <utils/math/angle.h> 57 BallTrigoRelativePos::BallTrigoRelativePos(
unsigned int image_width,
58 unsigned int image_height,
60 float camera_offset_x,
61 float camera_offset_y,
62 float camera_base_pan,
63 float camera_base_tilt,
64 float horizontal_angle,
66 float ball_circumference)
68 __image_width = image_width;
69 __image_width_2 = __image_width / 2;
70 __image_height = image_height;
71 __image_height_2 = __image_height / 2;
72 __ball_circumference = ball_circumference;
73 __camera_height = camera_height;
74 __camera_offset_x = camera_offset_x;
75 __camera_offset_y = camera_offset_y;
76 __camera_base_pan = camera_base_pan;
77 __camera_base_tilt = camera_base_tilt;
78 __horizontal_angle =
deg2rad( horizontal_angle );
79 __vertical_angle =
deg2rad( vertical_angle );
81 __cirt_center.x = 0.0f;
82 __cirt_center.y = 0.0f;
86 __pan_rad_per_pixel = __horizontal_angle / (float)__image_width;
87 __tilt_rad_per_pixel = __vertical_angle / (float)__image_height;
88 __ball_radius = __ball_circumference / ( 2.0 * M_PI );
90 __ball_x = __ball_y = __bearing = __slope = __distance = 0.f;
95 BallTrigoRelativePos::get_distance()
const 102 BallTrigoRelativePos::get_bearing()
const 109 BallTrigoRelativePos::get_slope()
const 116 BallTrigoRelativePos::get_y()
const 123 BallTrigoRelativePos::get_x()
const 130 BallTrigoRelativePos::set_center(
float x,
float y)
140 __cirt_center.x = c.
x;
141 __cirt_center.y = c.
y;
146 BallTrigoRelativePos::set_radius(
float r)
152 BallTrigoRelativePos::set_pan_tilt(
float pan,
float tilt)
160 BallTrigoRelativePos::get_pan_tilt(
float *pan,
float *tilt)
const 168 BallTrigoRelativePos::get_name()
const 170 return "BallTrigoRelativePos";
175 BallTrigoRelativePos::reset()
180 BallTrigoRelativePos::calc()
184 __bearing = (((__cirt_center.x - __image_width_2) * __pan_rad_per_pixel
185 + __pan + __camera_base_pan));
188 __bearing = - (((__cirt_center.x - __image_width_2) * __pan_rad_per_pixel
189 + __pan + __camera_base_pan));
193 __slope = ((__image_height_2 - __cirt_center.y) * __tilt_rad_per_pixel
194 + __tilt + __camera_base_tilt);
196 float alpha = M_PI_2 - __slope;
198 float e = __camera_height - __ball_radius - __ball_radius * cos(alpha);
199 __distance = - (e * tan(alpha) + __ball_radius * sin(alpha));
201 __ball_x = cos( __bearing ) * __distance + __camera_offset_x;
202 __ball_y = sin( __bearing ) * __distance + __camera_offset_y;
207 BallTrigoRelativePos::is_pos_valid()
const 209 return __distance > 0;
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.