26 #include <fvmodels/relative_position/box_relative.h>
27 #include <utils/math/angle.h>
35 namespace firevision {
52 BoxRelative::BoxRelative(
unsigned int image_width,
53 unsigned int image_height,
55 float camera_offset_x,
56 float camera_offset_y,
58 float horizontal_angle,
61 this->image_width = image_width;
62 this->image_height = image_height;
63 this->horizontal_angle =
deg2rad(horizontal_angle);
64 this->vertical_angle =
deg2rad(vertical_angle);
65 this->camera_orientation =
deg2rad(camera_ori);
66 this->camera_height = camera_height;
67 this->camera_offset_x = camera_offset_x;
68 this->camera_offset_y = camera_offset_y;
70 center.x = center.y = 0.f;
74 pan_rad_per_pixel = this->horizontal_angle / this->image_width;
75 tilt_rad_per_pixel = this->vertical_angle / this->image_height;
77 box_x = box_y = bearing = slope = distance_box_motor = distance_box_cam = 0.f;
79 DEFAULT_X_VARIANCE = 1500.f;
80 DEFAULT_Y_VARIANCE = 1000.f;
108 BoxRelative::get_distance()
const
110 return distance_box_motor;
114 BoxRelative::get_bearing(
void)
const
120 BoxRelative::get_slope()
const
130 BoxRelative::get_y(
void)
const
140 BoxRelative::get_x(
void)
const
146 BoxRelative::set_radius(
float r)
151 BoxRelative::set_center(
float x,
float y)
165 BoxRelative::set_pan_tilt(
float pan,
float tilt)
172 BoxRelative::get_pan_tilt(
float *pan,
float *tilt)
const
179 BoxRelative::get_name()
const
181 return "BoxRelative";
188 BoxRelative::set_horizontal_angle(
float angle_deg)
190 horizontal_angle =
deg2rad(angle_deg);
197 BoxRelative::set_vertical_angle(
float angle_deg)
199 vertical_angle =
deg2rad(angle_deg);
205 last_available =
false;
232 BoxRelative::is_pos_valid()
const
238 BoxRelative::calc_unfiltered()
242 bearing = ((center.x - image_width / 2) * pan_rad_per_pixel + pan + camera_orientation);
245 slope = -((center.y - image_height / 2) * tilt_rad_per_pixel - tilt);
247 distance_box_cam = camera_height * tan(M_PI / 2 + slope);
248 distance_box_motor = distance_box_cam - camera_offset_x;
258 box_x = cos(bearing) * distance_box_cam + camera_offset_x;
259 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.