cornerhorizon.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include <models/scanlines/cornerhorizon.h>
00027 #include <utils/math/angle.h>
00028 #include <cstdlib>
00029 #include <cstring>
00030
00031 using namespace fawkes;
00032
00033 namespace firevision {
00034 #if 0
00035 }
00036 #endif
00037
00038 const float CornerHorizon::M_PI_HALF = M_PI / 2.f;
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070 CornerHorizon::CornerHorizon(ScanlineModel *model,
00071 float field_length, float field_width, float field_border,
00072 unsigned int image_width, unsigned int image_height,
00073 float camera_height, float camera_ori,
00074 float horizontal_angle, float vertical_angle
00075 )
00076 {
00077 this->model = model;
00078
00079 this->field_length = field_length;
00080 this->field_width = field_width;
00081 this->field_border = field_border;
00082
00083 this->image_width = image_width;
00084 this->image_height = image_height;
00085 this->horizontal_angle = deg2rad( horizontal_angle );
00086 this->vertical_angle = deg2rad( vertical_angle );
00087 this->camera_ori = deg2rad( camera_ori );
00088 this->camera_height = camera_height;
00089
00090 pan_pixel_per_rad = this->image_width / this->horizontal_angle;
00091 tilt_pixel_per_rad = this->image_height / this->vertical_angle;
00092
00093 calculated = false;
00094
00095 coord.x = coord.y = 0;
00096 }
00097
00098
00099
00100
00101
00102 CornerHorizon::~CornerHorizon()
00103 {
00104 delete model;
00105 }
00106
00107
00108 point_t
00109 CornerHorizon::operator*()
00110 {
00111 return coord;
00112 }
00113
00114
00115 point_t*
00116 CornerHorizon::operator->()
00117 {
00118 return &coord;
00119 }
00120
00121
00122
00123 void
00124 CornerHorizon::calculate()
00125 {
00126
00127 float phi = normalize_mirror_rad( pose_ori + pan );
00128
00129 float corner_x, corner_y;
00130
00131 if ( (phi > 0) && (phi <= M_PI_HALF) ) {
00132 corner_x = field_length / 2 + field_border;
00133 corner_y = field_width / 2 + field_border;
00134 } else if ( (phi > M_PI_HALF) && (phi <= M_PI) ) {
00135 corner_x = - (field_length / 2 + field_border );
00136 corner_y = field_width / 2 + field_border;
00137 } else if ( (phi <= 0) && (phi > - M_PI_HALF) ) {
00138 corner_x = field_length / 2 + field_border;
00139 corner_y = - (field_width / 2 + field_border);
00140 } else {
00141 corner_x = - (field_length / 2 + field_border );
00142 corner_y = - (field_width / 2 + field_border);
00143 }
00144
00145 float d_x = corner_x - pose_x;
00146 float d_y = corner_y - pose_y;
00147
00148 float d = sqrt( d_x * d_x + d_y * d_y );
00149
00150 float alpha = atan2f( d, camera_height );
00151 float beta = M_PI_HALF - alpha;
00152
00153 int hor = (int)round((beta + tilt) * tilt_pixel_per_rad);
00154
00155 if ((unsigned int)abs(hor) >= (image_height / 2)) {
00156 if ( hor < 0 ) {
00157 hor = - ( image_height / 2 );
00158 } else {
00159 hor = image_height / 2;
00160 }
00161 }
00162
00163 horizon = image_height / 2 + hor;
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181 }
00182
00183
00184 point_t *
00185 CornerHorizon::operator++()
00186 {
00187 if ( ! calculated) {
00188 calculate();
00189 calculated = true;
00190 }
00191
00192 coord.x = (*model)->x;
00193 coord.y = (*model)->y;
00194
00195 do {
00196 ++(*model);
00197 } while ( ((*model)->y < horizon) && ( ! model->finished()) );
00198
00199 if ( ((*model)->y < horizon) || model->finished() ) {
00200
00201
00202 return &coord;
00203 } else {
00204 coord.x = (*model)->x;
00205 coord.y = (*model)->y;
00206
00207 return &coord;
00208 }
00209 }
00210
00211
00212 point_t *
00213 CornerHorizon::operator++(int)
00214 {
00215 if ( ! calculated) {
00216 calculate();
00217 calculated = true;
00218 }
00219 memcpy(&tmp_coord, &coord, sizeof(point_t));
00220
00221 do {
00222 ++(*model);
00223 } while ( ((*model)->y < horizon) && ! model->finished() );
00224
00225 if ( ((*model)->y >= horizon) && ! model->finished() ) {
00226 coord.x = (*model)->x;
00227 coord.y = (*model)->y;
00228
00229 }
00230
00231 return &tmp_coord;
00232 }
00233
00234
00235 bool
00236 CornerHorizon::finished()
00237 {
00238 return model->finished();
00239 }
00240
00241
00242 void
00243 CornerHorizon::reset()
00244 {
00245 calculated = false;
00246 coord.x = coord.y = 0;
00247 model->reset();
00248 }
00249
00250
00251 const char *
00252 CornerHorizon::get_name()
00253 {
00254 return "ScanlineModel::CornerHorizon";
00255 }
00256
00257
00258 unsigned int
00259 CornerHorizon::get_margin()
00260 {
00261 return model->get_margin();
00262 }
00263
00264
00265
00266
00267
00268 unsigned int
00269 CornerHorizon::getHorizon()
00270 {
00271 return horizon;
00272 }
00273
00274
00275 void
00276 CornerHorizon::set_robot_pose(float x, float y, float ori)
00277 {
00278 pose_x = x;
00279 pose_y = y;
00280 pose_ori = ori;
00281 }
00282
00283
00284 void
00285 CornerHorizon::set_pan_tilt(float pan, float tilt)
00286 {
00287 this->pan = pan;
00288 this->tilt = tilt;
00289 }
00290
00291 }