29 #include <fvmodels/relative_position/front_ball.h> 30 #include <utils/math/angle.h> 55 FrontBallRelativePos::FrontBallRelativePos(
unsigned int image_width,
56 unsigned int image_height,
58 float camera_offset_x,
59 float camera_offset_y,
61 float horizontal_angle,
63 float ball_circumference
67 this->image_width = image_width;
68 this->image_height = image_height;
69 this->ball_circumference = ball_circumference;
70 this->horizontal_angle =
deg2rad( horizontal_angle );
71 this->vertical_angle =
deg2rad( vertical_angle );
72 this->camera_orientation =
deg2rad( camera_ori );
73 this->camera_height = camera_height;
74 this->camera_offset_x = camera_offset_x;
75 this->camera_offset_y = camera_offset_y;
78 m_cirtCenter.x = 0.0f;
79 m_cirtCenter.y = 0.0f;
83 avg_x = avg_y = avg_x_sum = avg_y_sum = 0.f;
84 avg_x_num = avg_y_num = 0;
86 m_fPanRadPerPixel = this->horizontal_angle / this->image_width;
87 m_fTiltRadPerPixel = this->vertical_angle / this->image_height;
88 m_fBallRadius = this->ball_circumference / ( 2 * M_PI );
90 ball_x = ball_y = bearing = slope = distance_ball_motor = distance_ball_cam = 0.f;
92 DEFAULT_X_VARIANCE = 1500.f;
93 DEFAULT_Y_VARIANCE = 1000.f;
118 FrontBallRelativePos::get_distance()
const 120 return distance_ball_motor;
125 FrontBallRelativePos::get_bearing(
void)
const 132 FrontBallRelativePos::get_slope()
const 139 FrontBallRelativePos::get_y(
void)
const 146 FrontBallRelativePos::get_x(
void)
const 153 FrontBallRelativePos::set_center(
float x,
float y)
163 m_cirtCenter.x = c.
x;
164 m_cirtCenter.y = c.
y;
169 FrontBallRelativePos::set_radius(
float r)
179 FrontBallRelativePos::get_radius()
const 186 FrontBallRelativePos::set_pan_tilt(
float pan,
float tilt)
194 FrontBallRelativePos::get_pan_tilt(
float *pan,
float *tilt)
const 202 FrontBallRelativePos::get_name()
const 204 return "FrontBallRelativePos";
212 FrontBallRelativePos::set_horizontal_angle(
float angle_deg)
214 horizontal_angle =
deg2rad( angle_deg );
222 FrontBallRelativePos::set_vertical_angle(
float angle_deg)
224 vertical_angle =
deg2rad( angle_deg );
229 FrontBallRelativePos::reset()
231 last_available =
false;
236 FrontBallRelativePos::calc()
254 float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
257 distance_ball_cam = sqrt( tmp * tmp -
258 (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius) );
263 bearing = (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
266 bearing = - (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
270 slope = - ((m_cirtCenter.y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
272 ball_x = cos( bearing ) * distance_ball_cam + camera_offset_x;
273 ball_y = sin( bearing ) * distance_ball_cam + camera_offset_y;
277 distance_ball_motor = sqrt( ball_x * ball_x + ball_y * ball_y );
283 FrontBallRelativePos::is_pos_valid()
const 290 FrontBallRelativePos::calc_unfiltered()
292 float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
296 distance_ball_cam = sqrt( tmp * tmp -
297 (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius) );
302 bearing = (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
305 bearing = - (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
309 slope = - ((m_cirtCenter.y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
312 ball_x = cos( bearing ) * distance_ball_cam + camera_offset_x;
313 ball_y = sin( bearing ) * distance_ball_cam + camera_offset_y;
315 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.