24#include <fvmodels/relative_position/ball_trigo.h>
25#include <utils/math/angle.h>
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;
130 cirt_center_.
x = c.
x;
131 cirt_center_.
y = c.
y;
156 return "BallTrigoRelativePos";
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_;
190 return distance_ > 0;
BallTrigoRelativePos(unsigned int image_width, unsigned int image_height, float camera_height, float camera_offset_x, float camera_offset_y, float camera_base_pan, float camera_base_tilt, float horizontal_angle, float vertical_angle, float ball_circumference)
Constructor.
virtual void reset()
Reset all data.
virtual void set_center(float x, float y)
Set center of a found circle.
virtual const char * get_name() const
Get name of relative position model.
virtual float get_x() const
Get relative X coordinate of object.
virtual void set_pan_tilt(float pan=0.0f, float tilt=0.0f)
Set camera pan and tilt.
virtual float get_distance() const
Get distance to object.
virtual void set_radius(float r)
Set radius of a found circle.
virtual float get_slope() const
Get slope (vertical angle) to object.
virtual float get_bearing() const
Get bearing (horizontal angle) to object.
virtual float get_y() const
Get relative Y coordinate of object.
virtual void get_pan_tilt(float *pan, float *tilt) const
Get camera pan tilt.
virtual void calc()
Calculate position data.
virtual bool is_pos_valid() const
Check if position is valid.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.