Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * cornerhorizon.cpp - Implementation of the corner horizon 00004 * 00005 * Created: Fri Apr 07 04:37:25 2006 00006 * Copyright 2005-2006 Tim Niemueller [www.niemueller.de] 00007 * 2006 Stefan Schiffer 00008 * 2006 Christoph Mies 00009 * 00010 ****************************************************************************/ 00011 00012 /* This program is free software; you can redistribute it and/or modify 00013 * it under the terms of the GNU General Public License as published by 00014 * the Free Software Foundation; either version 2 of the License, or 00015 * (at your option) any later version. A runtime exception applies to 00016 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00017 * 00018 * This program is distributed in the hope that it will be useful, 00019 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00020 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00021 * GNU Library General Public License for more details. 00022 * 00023 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 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 /* just to make Emacs auto-indent happy */ 00035 } 00036 #endif 00037 00038 const float CornerHorizon::M_PI_HALF = M_PI / 2.f; 00039 00040 /** @class CornerHorizon <models/scanlines/cornerhorizon.h> 00041 * Cut of arbitrary scanline models at an artificial horizon. 00042 * The artificial horizon is calculated by the highest corner that is visible 00043 * in the image. From that the Y coordinate in the image is used and everything 00044 * above that point is ignored from the scanline grid. 00045 * 00046 * This class was written in a one-night hacking sensation at RoboLudens 2006 00047 * in Eindhoven. For that time it is pretty readable code and we are using it 00048 * since then. Cool! 00049 * 00050 * @author Tim Niemueller 00051 * @author Stefan Schiffer 00052 * @author Christoph Mies 00053 */ 00054 00055 /** Constructor. 00056 * @param model Model to apply the artificial horizon on. This model is deleted on 00057 * the destruction of the CornerHorizon instance so you can forget about it in the 00058 * using application. 00059 * @param field_length length of soccer field 00060 * @param field_width width of soccer field 00061 * @param field_border size of border around the field (i.e. distance between the 00062 * outer white line and the physical field end) 00063 * @param image_width image width in pixels 00064 * @param image_height image height in pixels 00065 * @param camera_height height of camera above ground 00066 * @param camera_ori orientation of camera on the robot in degrees 00067 * @param horizontal_angle horizontal viewing angle in degrees 00068 * @param vertical_angle vertical viewing angle in degrees 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 /** Destructor. 00100 * Not that this deletes the supplied model! 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 /** Calculate horizon point. */ 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 /* if (phi <= - M_PI_HALF) */ { 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 cout << "Calculated: " << endl 00167 << " phi=" << phi << endl 00168 << " corner_x=" << corner_x << endl 00169 << " corner_y=" << corner_y << endl 00170 << " d_x=" << d_x << endl 00171 << " d_y=" << d_y << endl 00172 << " d=" << d << endl 00173 << " alpha=" << alpha << endl 00174 << " beta=" << beta << endl 00175 << " hor=" << hor << endl 00176 << " horizon=" << horizon << endl 00177 << " pan_pixel_per_rad=" << pan_pixel_per_rad << endl 00178 << " tilt_pixel_per_rad=" << tilt_pixel_per_rad << endl; 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 // finished 00201 //cout << "1 (" << coord.x << "," << coord.y << ")" << endl; 00202 return &coord; 00203 } else { 00204 coord.x = (*model)->x; 00205 coord.y = (*model)->y; 00206 //cout << "2 (" << coord.x << "," << coord.y << ")" << endl; 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 //cout << "3 (" << coord.x << "," << coord.y << ")" << endl; 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 /** Get the horizon point. 00266 * @return y coordinate of the horizon point. 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 } // end namespace firevision