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()
109 CornerHorizon::operator*()
115 CornerHorizon::operator->()
122 CornerHorizon::calculate()
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;
180 CornerHorizon::operator++()
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;
207 CornerHorizon::operator++(
int)
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;
229 CornerHorizon::finished()
231 return model->finished();
235 CornerHorizon::reset()
238 coord.x = coord.y = 0;
243 CornerHorizon::get_name()
245 return "ScanlineModel::CornerHorizon";
249 CornerHorizon::get_margin()
251 return model->get_margin();
258 CornerHorizon::getHorizon()
264 CornerHorizon::set_robot_pose(
float x,
float y,
float ori)
272 CornerHorizon::set_pan_tilt(
float pan,
float tilt)