26#include <fvmodels/scanlines/cornerhorizon.h>
27#include <utils/math/angle.h>
36const float CornerHorizon::M_PI_HALF = M_PI / 2.f;
72 unsigned int image_width,
73 unsigned int image_height,
76 float horizontal_angle,
81 this->field_length = field_length;
82 this->field_width = field_width;
83 this->field_border = field_border;
85 this->image_width = image_width;
86 this->image_height = image_height;
87 this->horizontal_angle =
deg2rad(horizontal_angle);
88 this->vertical_angle =
deg2rad(vertical_angle);
89 this->camera_ori =
deg2rad(camera_ori);
90 this->camera_height = camera_height;
92 pan_pixel_per_rad = this->image_width / this->horizontal_angle;
93 tilt_pixel_per_rad = this->image_height / this->vertical_angle;
97 coord.
x = coord.
y = 0;
126 float corner_x, corner_y;
128 if ((phi > 0) && (phi <= M_PI_HALF)) {
129 corner_x = field_length / 2 + field_border;
130 corner_y = field_width / 2 + field_border;
131 }
else if ((phi > M_PI_HALF) && (phi <= M_PI)) {
132 corner_x = -(field_length / 2 + field_border);
133 corner_y = field_width / 2 + field_border;
134 }
else if ((phi <= 0) && (phi > -M_PI_HALF)) {
135 corner_x = field_length / 2 + field_border;
136 corner_y = -(field_width / 2 + field_border);
138 corner_x = -(field_length / 2 + field_border);
139 corner_y = -(field_width / 2 + field_border);
142 float d_x = corner_x - pose_x;
143 float d_y = corner_y - pose_y;
145 float d = sqrt(d_x * d_x + d_y * d_y);
147 float alpha = atan2f(d, camera_height);
148 float beta = M_PI_HALF - alpha;
150 int hor = (int)roundf((beta + tilt) * tilt_pixel_per_rad);
152 if ((
unsigned int)abs(hor) >= (image_height / 2)) {
154 hor = -(image_height / 2);
156 hor = image_height / 2;
160 horizon = image_height / 2 + hor;
187 coord.
x = (*model)->x;
188 coord.
y = (*model)->y;
192 }
while (((*model)->y < horizon) && (!model->finished()));
194 if (((*model)->y < horizon) || model->finished()) {
199 coord.
x = (*model)->x;
200 coord.
y = (*model)->y;
213 memcpy(&tmp_coord, &coord,
sizeof(
upoint_t));
217 }
while (((*model)->y < horizon) && !model->finished());
219 if (((*model)->y >= horizon) && !model->finished()) {
220 coord.
x = (*model)->x;
221 coord.
y = (*model)->y;
231 return model->finished();
238 coord.
x = coord.
y = 0;
245 return "ScanlineModel::CornerHorizon";
251 return model->get_margin();
virtual ~CornerHorizon()
Destructor.
bool finished()
Check if all desired points have been processed.
unsigned int getHorizon()
Get the horizon point.
fawkes::upoint_t * operator++()
Postfix ++ operator.
CornerHorizon(ScanlineModel *model, float field_length, float field_width, float field_border, unsigned int image_width, unsigned int image_height, float camera_height, float camera_ori, float horizontal_angle, float vertical_angle)
Constructor.
unsigned int get_margin()
Get margin around points.
fawkes::upoint_t operator*()
Get the current coordinate.
void set_pan_tilt(float pan, float tilt)
Set camera's pan/tilt values.
void set_robot_pose(float x, float y, float ori)
Set the robot's pose.
const char * get_name()
Get name of scanline model.
void calculate()
Calculate horizon point.
fawkes::upoint_t * operator->()
Get pointer to current point.
Scanline model interface.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Point with cartesian coordinates as unsigned integers.
unsigned int x
x coordinate
unsigned int y
y coordinate