27 #include <fvmodels/relative_position/box_relative.h> 28 #include <utils/math/angle.h> 55 BoxRelative::BoxRelative(
unsigned int image_width,
56 unsigned int image_height,
58 float camera_offset_x,
float camera_offset_y,
60 float horizontal_angle,
float vertical_angle
64 this->image_width = image_width;
65 this->image_height = image_height;
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;
73 center.x = center.y = 0.f;
77 pan_rad_per_pixel = this->horizontal_angle / this->image_width;
78 tilt_rad_per_pixel = this->vertical_angle / this->image_height;
80 box_x = box_y = bearing = slope = distance_box_motor = distance_box_cam = 0.f;
82 DEFAULT_X_VARIANCE = 1500.f;
83 DEFAULT_Y_VARIANCE = 1000.f;
112 BoxRelative::get_distance()
const 114 return distance_box_motor;
119 BoxRelative::get_bearing(
void)
const 126 BoxRelative::get_slope()
const 137 BoxRelative::get_y(
void)
const 148 BoxRelative::get_x(
void)
const 154 BoxRelative::set_radius(
float r)
160 BoxRelative::set_center(
float x,
float y)
176 BoxRelative::set_pan_tilt(
float pan,
float tilt)
184 BoxRelative::get_pan_tilt(
float *pan,
float *tilt)
const 192 BoxRelative::get_name()
const 194 return "BoxRelative";
202 BoxRelative::set_horizontal_angle(
float angle_deg)
204 horizontal_angle =
deg2rad( angle_deg );
212 BoxRelative::set_vertical_angle(
float angle_deg)
214 vertical_angle =
deg2rad( angle_deg );
221 last_available =
false;
252 BoxRelative::is_pos_valid()
const 259 BoxRelative::calc_unfiltered()
263 bearing = ((center.x - image_width/2) * pan_rad_per_pixel + pan + camera_orientation);
266 slope = -((center.y - image_height / 2) * tilt_rad_per_pixel - tilt);
268 distance_box_cam = camera_height * tan(M_PI / 2 + slope);
269 distance_box_motor = distance_box_cam - camera_offset_x;
279 box_x = cos( bearing ) * distance_box_cam + camera_offset_x;
280 box_y = sin( bearing ) * distance_box_cam + camera_offset_y;
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.