27#include <fvmodels/relative_position/front_ball.h>
28#include <utils/math/angle.h>
54 unsigned int image_height,
56 float camera_offset_x,
57 float camera_offset_y,
59 float horizontal_angle,
61 float ball_circumference)
63 this->image_width = image_width;
64 this->image_height = image_height;
65 this->ball_circumference = ball_circumference;
66 this->horizontal_angle =
deg2rad(horizontal_angle);
67 this->vertical_angle =
deg2rad(vertical_angle);
68 this->camera_orientation =
deg2rad(camera_ori);
69 this->camera_height = camera_height;
70 this->camera_offset_x = camera_offset_x;
71 this->camera_offset_y = camera_offset_y;
74 m_cirtCenter.
x = 0.0f;
75 m_cirtCenter.
y = 0.0f;
79 avg_x = avg_y = avg_x_sum = avg_y_sum = 0.f;
80 avg_x_num = avg_y_num = 0;
82 m_fPanRadPerPixel = this->horizontal_angle / this->image_width;
83 m_fTiltRadPerPixel = this->vertical_angle / this->image_height;
84 m_fBallRadius = this->ball_circumference / (2 * M_PI);
86 ball_x = ball_y = bearing = slope = distance_ball_motor = distance_ball_cam = 0.f;
88 DEFAULT_X_VARIANCE = 1500.f;
89 DEFAULT_Y_VARIANCE = 1000.f;
115 return distance_ball_motor;
152 m_cirtCenter.
x = c.
x;
153 m_cirtCenter.
y = c.
y;
188 return "FrontBallRelativePos";
197 horizontal_angle =
deg2rad(angle_deg);
206 vertical_angle =
deg2rad(angle_deg);
212 last_available =
false;
234 float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
238 sqrt(tmp * tmp - (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius));
243 (((m_cirtCenter.
x - image_width / 2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
247 -(((m_cirtCenter.
x - image_width / 2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
251 slope = -((m_cirtCenter.
y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
253 ball_x = cos(bearing) * distance_ball_cam + camera_offset_x;
254 ball_y = sin(bearing) * distance_ball_cam + camera_offset_y;
258 distance_ball_motor = sqrt(ball_x * ball_x + ball_y * ball_y);
270 float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
275 sqrt(tmp * tmp - (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius));
280 (((m_cirtCenter.
x - image_width / 2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
284 -(((m_cirtCenter.
x - image_width / 2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
288 slope = -((m_cirtCenter.
y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
290 ball_x = cos(bearing) * distance_ball_cam + camera_offset_x;
291 ball_y = sin(bearing) * distance_ball_cam + camera_offset_y;
293 distance_ball_motor = sqrt(ball_x * ball_x + ball_y * ball_y);
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 float get_x() const
Get relative X coordinate of object.
virtual void set_vertical_angle(float angle_deg)
Set vertical viewing angle.
virtual void reset()
Reset all data.
virtual const char * get_name() const
Get name of relative position model.
virtual void set_radius(float r)
Set radius of a found circle.
virtual void calc()
Calculate position data.
FrontBallRelativePos(unsigned int image_width, unsigned int image_height, float camera_height, float camera_offset_x, float camera_offset_y, float camera_ori, float horizontal_angle, float vertical_angle, float ball_circumference)
Constructor.
virtual void set_horizontal_angle(float angle_deg)
Set horizontal viewing angle.
virtual float get_bearing() const
Get bearing (horizontal angle) to object.
virtual float get_radius() const
Get the ball radius.
virtual bool is_pos_valid() const
Check if position is valid.
virtual void calc_unfiltered()
Calculate data unfiltered.
virtual void set_center(float x, float y)
Set center of a found circle.
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 float get_slope() const
Get slope (vertical angle) to object.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.