26 #include <fvmodels/scanlines/cornerhorizon.h> 27 #include <utils/math/angle.h> 34 namespace firevision {
36 const 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;
103 CornerHorizon::~CornerHorizon()
120 CornerHorizon::calculate()
124 float corner_x, corner_y;
126 if ((phi > 0) && (phi <= M_PI_HALF)) {
127 corner_x = field_length / 2 + field_border;
128 corner_y = field_width / 2 + field_border;
129 }
else if ((phi > M_PI_HALF) && (phi <= M_PI)) {
130 corner_x = -(field_length / 2 + field_border);
131 corner_y = field_width / 2 + field_border;
132 }
else if ((phi <= 0) && (phi > -M_PI_HALF)) {
133 corner_x = field_length / 2 + field_border;
134 corner_y = -(field_width / 2 + field_border);
136 corner_x = -(field_length / 2 + field_border);
137 corner_y = -(field_width / 2 + field_border);
140 float d_x = corner_x - pose_x;
141 float d_y = corner_y - pose_y;
143 float d = sqrt(d_x * d_x + d_y * d_y);
145 float alpha = atan2f(d, camera_height);
146 float beta = M_PI_HALF - alpha;
148 int hor = (int)roundf((beta + tilt) * tilt_pixel_per_rad);
150 if ((
unsigned int)abs(hor) >= (image_height / 2)) {
152 hor = -(image_height / 2);
154 hor = image_height / 2;
158 horizon = image_height / 2 + hor;
178 CornerHorizon::operator++()
185 coord.x = (*model)->x;
186 coord.y = (*model)->y;
190 }
while (((*model)->y < horizon) && (!model->finished()));
192 if (((*model)->y < horizon) || model->finished()) {
197 coord.x = (*model)->x;
198 coord.y = (*model)->y;
205 CornerHorizon::operator++(
int)
211 memcpy(&tmp_coord, &coord,
sizeof(
upoint_t));
215 }
while (((*model)->y < horizon) && !model->finished());
217 if (((*model)->y >= horizon) && !model->finished()) {
218 coord.x = (*model)->x;
219 coord.y = (*model)->y;
227 CornerHorizon::finished()
229 return model->finished();
233 CornerHorizon::reset()
236 coord.x = coord.y = 0;
241 CornerHorizon::get_name()
243 return "ScanlineModel::CornerHorizon";
247 CornerHorizon::get_margin()
249 return model->get_margin();
256 CornerHorizon::getHorizon()
262 CornerHorizon::set_robot_pose(
float x,
float y,
float ori)
270 CornerHorizon::set_pan_tilt(
float pan,
float tilt)
Scanline model interface.
Fawkes library namespace.
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Point with cartesian coordinates as unsigned integers.
float deg2rad(float deg)
Convert an angle given in degrees to radians.