24 #include <fvmodels/relative_position/ball_trigo.h> 25 #include <utils/math/angle.h> 32 namespace firevision {
54 BallTrigoRelativePos::BallTrigoRelativePos(
unsigned int image_width,
55 unsigned int image_height,
57 float camera_offset_x,
58 float camera_offset_y,
59 float camera_base_pan,
60 float camera_base_tilt,
61 float horizontal_angle,
63 float ball_circumference)
65 image_width_ = image_width;
66 image_width_2_ = image_width_ / 2;
67 image_height_ = image_height;
68 image_height_2_ = image_height_ / 2;
69 ball_circumference_ = ball_circumference;
70 camera_height_ = camera_height;
71 camera_offset_x_ = camera_offset_x;
72 camera_offset_y_ = camera_offset_y;
73 camera_base_pan_ = camera_base_pan;
74 camera_base_tilt_ = camera_base_tilt;
75 horizontal_angle_ =
deg2rad(horizontal_angle);
76 vertical_angle_ =
deg2rad(vertical_angle);
78 cirt_center_.x = 0.0f;
79 cirt_center_.y = 0.0f;
83 pan_rad_per_pixel_ = horizontal_angle_ / (float)image_width_;
84 tilt_rad_per_pixel_ = vertical_angle_ / (float)image_height_;
85 ball_radius_ = ball_circumference_ / (2.0 * M_PI);
87 ball_x_ = ball_y_ = bearing_ = slope_ = distance_ = 0.f;
91 BallTrigoRelativePos::get_distance()
const 97 BallTrigoRelativePos::get_bearing()
const 103 BallTrigoRelativePos::get_slope()
const 109 BallTrigoRelativePos::get_y()
const 115 BallTrigoRelativePos::get_x()
const 121 BallTrigoRelativePos::set_center(
float x,
float y)
130 cirt_center_.x = c.
x;
131 cirt_center_.y = c.
y;
135 BallTrigoRelativePos::set_radius(
float r)
140 BallTrigoRelativePos::set_pan_tilt(
float pan,
float tilt)
147 BallTrigoRelativePos::get_pan_tilt(
float *pan,
float *tilt)
const 154 BallTrigoRelativePos::get_name()
const 156 return "BallTrigoRelativePos";
160 BallTrigoRelativePos::reset()
165 BallTrigoRelativePos::calc()
169 bearing_ = (((cirt_center_.x - image_width_2_) * pan_rad_per_pixel_ + pan_ + camera_base_pan_));
172 bearing_ = -(((cirt_center_.x - image_width_2_) * pan_rad_per_pixel_ + pan_ + camera_base_pan_));
176 slope_ = ((image_height_2_ - cirt_center_.y) * tilt_rad_per_pixel_ + tilt_ + camera_base_tilt_);
178 float alpha = M_PI_2 - slope_;
180 float e = camera_height_ - ball_radius_ - ball_radius_ * cos(alpha);
181 distance_ = -(e * tan(alpha) + ball_radius_ * sin(alpha));
183 ball_x_ = cos(bearing_) * distance_ + camera_offset_x_;
184 ball_y_ = sin(bearing_) * distance_ + camera_offset_y_;
188 BallTrigoRelativePos::is_pos_valid()
const 190 return distance_ > 0;
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.