27 #include <fvmodels/relative_position/front_ball.h>
28 #include <utils/math/angle.h>
36 namespace firevision {
53 FrontBallRelativePos::FrontBallRelativePos(
unsigned int image_width,
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;
113 FrontBallRelativePos::get_distance()
const
115 return distance_ball_motor;
119 FrontBallRelativePos::get_bearing(
void)
const
125 FrontBallRelativePos::get_slope()
const
131 FrontBallRelativePos::get_y(
void)
const
137 FrontBallRelativePos::get_x(
void)
const
143 FrontBallRelativePos::set_center(
float x,
float y)
152 m_cirtCenter.x = c.
x;
153 m_cirtCenter.y = c.
y;
157 FrontBallRelativePos::set_radius(
float r)
166 FrontBallRelativePos::get_radius()
const
172 FrontBallRelativePos::set_pan_tilt(
float pan,
float tilt)
179 FrontBallRelativePos::get_pan_tilt(
float *pan,
float *tilt)
const
186 FrontBallRelativePos::get_name()
const
188 return "FrontBallRelativePos";
195 FrontBallRelativePos::set_horizontal_angle(
float angle_deg)
197 horizontal_angle =
deg2rad(angle_deg);
204 FrontBallRelativePos::set_vertical_angle(
float angle_deg)
206 vertical_angle =
deg2rad(angle_deg);
210 FrontBallRelativePos::reset()
212 last_available =
false;
217 FrontBallRelativePos::calc()
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);
262 FrontBallRelativePos::is_pos_valid()
const
268 FrontBallRelativePos::calc_unfiltered()
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);
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.