26 #include <fvmodels/scanlines/cornerhorizon.h> 27 #include <utils/math/angle.h> 38 const float CornerHorizon::M_PI_HALF = M_PI / 2.f;
71 float field_length,
float field_width,
float field_border,
72 unsigned int image_width,
unsigned int image_height,
73 float camera_height,
float camera_ori,
74 float horizontal_angle,
float vertical_angle
79 this->field_length = field_length;
80 this->field_width = field_width;
81 this->field_border = field_border;
83 this->image_width = image_width;
84 this->image_height = image_height;
85 this->horizontal_angle =
deg2rad( horizontal_angle );
86 this->vertical_angle =
deg2rad( vertical_angle );
87 this->camera_ori =
deg2rad( camera_ori );
88 this->camera_height = camera_height;
90 pan_pixel_per_rad = this->image_width / this->horizontal_angle;
91 tilt_pixel_per_rad = this->image_height / this->vertical_angle;
95 coord.x = coord.y = 0;
102 CornerHorizon::~CornerHorizon()
109 CornerHorizon::operator*()
116 CornerHorizon::operator->()
124 CornerHorizon::calculate()
129 float corner_x, corner_y;
131 if ( (phi > 0) && (phi <= M_PI_HALF) ) {
132 corner_x = field_length / 2 + field_border;
133 corner_y = field_width / 2 + field_border;
134 }
else if ( (phi > M_PI_HALF) && (phi <= M_PI) ) {
135 corner_x = - (field_length / 2 + field_border );
136 corner_y = field_width / 2 + field_border;
137 }
else if ( (phi <= 0) && (phi > - M_PI_HALF) ) {
138 corner_x = field_length / 2 + field_border;
139 corner_y = - (field_width / 2 + field_border);
141 corner_x = - (field_length / 2 + field_border );
142 corner_y = - (field_width / 2 + field_border);
145 float d_x = corner_x - pose_x;
146 float d_y = corner_y - pose_y;
148 float d = sqrt( d_x * d_x + d_y * d_y );
150 float alpha = atan2f( d, camera_height );
151 float beta = M_PI_HALF - alpha;
153 int hor = (int)round((beta + tilt) * tilt_pixel_per_rad);
155 if ((
unsigned int)abs(hor) >= (image_height / 2)) {
157 hor = - ( image_height / 2 );
159 hor = image_height / 2;
163 horizon = image_height / 2 + hor;
185 CornerHorizon::operator++()
192 coord.x = (*model)->x;
193 coord.y = (*model)->y;
197 }
while ( ((*model)->y < horizon) && ( ! model->finished()) );
199 if ( ((*model)->y < horizon) || model->finished() ) {
204 coord.x = (*model)->x;
205 coord.y = (*model)->y;
213 CornerHorizon::operator++(
int)
219 memcpy(&tmp_coord, &coord,
sizeof(
upoint_t));
223 }
while ( ((*model)->y < horizon) && ! model->finished() );
225 if ( ((*model)->y >= horizon) && ! model->finished() ) {
226 coord.x = (*model)->x;
227 coord.y = (*model)->y;
236 CornerHorizon::finished()
238 return model->finished();
243 CornerHorizon::reset()
246 coord.x = coord.y = 0;
252 CornerHorizon::get_name()
254 return "ScanlineModel::CornerHorizon";
259 CornerHorizon::get_margin()
261 return model->get_margin();
269 CornerHorizon::getHorizon()
276 CornerHorizon::set_robot_pose(
float x,
float y,
float ori)
285 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.