26#include <fvmodels/relative_position/box_relative.h>
27#include <utils/math/angle.h>
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;
110 return distance_box_motor;
181 return "BoxRelative";
190 horizontal_angle =
deg2rad(angle_deg);
199 vertical_angle =
deg2rad(angle_deg);
205 last_available =
false;
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;
virtual float get_y() const
Get relative Y coordinate of object.
virtual void set_center(float x, float y)
Set center of a found circle.
virtual void set_horizontal_angle(float angle_deg)
Set the horizontal viewing angle.
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.
virtual bool is_pos_valid() const
Check if position is valid.
virtual void reset()
Reset all data.
virtual void set_vertical_angle(float angle_deg)
Set the vertical viewing angle.
BoxRelative(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)
Constructor.
virtual float get_x() const
Get relative X coordinate of object.
virtual float get_distance() const
Get distance to object.
virtual float get_slope() const
Get slope (vertical angle) to object.
virtual void calc_unfiltered()
Calculate data unfiltered.
virtual void set_pan_tilt(float pan=0.0f, float tilt=0.0f)
Set camera pan and tilt.
virtual float get_bearing() const
Get bearing (horizontal angle) to object.
virtual void get_pan_tilt(float *pan, float *tilt) const
Get camera pan tilt.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.