00001
00002 /***************************************************************************
00003  *  cornerhorizon.cpp - Implementation of the corner horizon
00004  *
00005  *  Generated: 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 const float CornerHorizon::M_PI_HALF = M_PI / 2.f;
00034 
00035 /** @class CornerHorizon <models/scanlines/cornerhorizon.h>
00036  * Cut of arbitrary scanline models at an artificial horizon.
00037  * The artificial horizon is calculated by the highest corner that is visible
00038  * in the image. From that the Y coordinate in the image is used and everything
00039  * above that point is ignored from the scanline grid.
00040  *
00041  * This class was written in a one-night hacking sensation at RoboLudens 2006
00042  * in Eindhoven. For that time it is pretty readable code and we are using it
00043  * since then. Cool!
00044  *
00045  * @author Tim Niemueller
00046  * @author Stefan Schiffer
00047  * @author Christoph Mies
00048  */
00049 
00050 /** Constructor.
00051  * @param model Model to apply the artificial horizon on. This model is deleted on
00052  * the destruction of the CornerHorizon instance so you can forget about it in the
00053  * using application.
00054  * @param field_length length of soccer field
00055  * @param field_width width of soccer field
00056  * @param field_border size of border around the field (i.e. distance between the
00057  * outer white line and the physical field end)
00058  * @param image_width image width in pixels
00059  * @param image_height image height in pixels
00060  * @param camera_height height of camera above ground
00061  * @param camera_ori orientation of camera on the robot in degrees
00062  * @param horizontal_angle horizontal viewing angle in degrees
00063  * @param vertical_angle vertical viewing angle in degrees
00064  */
00065 CornerHorizon::CornerHorizon(ScanlineModel *model,
00066                              float field_length, float field_width, float field_border,
00067                              unsigned int image_width, unsigned int image_height,
00068                              float camera_height, float camera_ori,
00069                              float horizontal_angle, float vertical_angle
00070                              )
00071 {
00072   this->model = model;
00073
00074   this->field_length = field_length;
00075   this->field_width  = field_width;
00076   this->field_border = field_border;
00077
00078   this->image_width        = image_width;
00079   this->image_height       = image_height;
00080   this->horizontal_angle   = deg2rad( horizontal_angle );
00081   this->vertical_angle     = deg2rad( vertical_angle   );
00082   this->camera_ori         = deg2rad( camera_ori       );
00083   this->camera_height      = camera_height;
00084
00085   pan_pixel_per_rad  = this->image_width  / this->horizontal_angle;
00086   tilt_pixel_per_rad = this->image_height / this->vertical_angle;
00087
00088   calculated = false;
00089
00090   coord.x = coord.y = 0;
00091 }
00092
00093 
00094 /** Destructor.
00095  * Not that this deletes the supplied model!
00096  */
00097 CornerHorizon::~CornerHorizon()
00098 {
00099   delete model;
00100 }
00101
00102
00103 point_t
00104 CornerHorizon::operator*()
00105 {
00106   return coord;
00107 }
00108
00109
00110 point_t*
00111 CornerHorizon::operator->()
00112 {
00113   return &coord;
00114 }
00115
00116 
00117 /** Calculate horizon point. */
00118 void
00119 CornerHorizon::calculate()
00120 {
00121
00122   float phi = normalize_mirror_rad( pose_ori + pan );
00123
00124   float corner_x, corner_y;
00125
00126   if ( (phi > 0) && (phi <= M_PI_HALF) ) {
00127     corner_x = field_length / 2 + field_border;
00128     corner_y = field_width  / 2 + field_border;
00129   } else if ( (phi > M_PI_HALF) && (phi <= M_PI) ) {
00130     corner_x = - (field_length / 2 + field_border );
00131     corner_y = field_width / 2 + field_border;
00132   } else if ( (phi <= 0) && (phi > - M_PI_HALF) ) {
00133     corner_x = field_length / 2 + field_border;
00134     corner_y = - (field_width / 2 + field_border);
00135   } else /* if (phi <= - M_PI_HALF) */ {
00136     corner_x = - (field_length / 2 + field_border );
00137     corner_y = - (field_width / 2 + field_border);
00138   }
00139
00140   float d_x = corner_x - pose_x;
00141   float d_y = corner_y - pose_y;
00142
00143   float d = sqrt( d_x * d_x + d_y * d_y );
00144
00145   float alpha = atan2f( d, camera_height );
00146   float beta  = M_PI_HALF - alpha;
00147
00148   int hor = (int)round((beta + tilt) * tilt_pixel_per_rad);
00149
00150   if ((unsigned int)abs(hor) >= (image_height / 2)) {
00151     if ( hor < 0 ) {
00152       hor = - ( image_height / 2 );
00153     } else {
00154       hor = image_height / 2;
00155     }
00156   }
00157
00158   horizon = image_height / 2 + hor;
00159
00160   /*
00161   cout << "Calculated: " << endl
00162        << "  phi=" << phi << endl
00163        << "  corner_x=" << corner_x << endl
00164        << "  corner_y=" << corner_y << endl
00165        << "  d_x=" << d_x << endl
00166        << "  d_y=" << d_y << endl
00167        << "  d=" << d << endl
00168        << "  alpha=" << alpha << endl
00169        << "  beta=" << beta << endl
00170        << "  hor=" << hor << endl
00171        << "  horizon=" << horizon << endl
00172        << "  pan_pixel_per_rad=" << pan_pixel_per_rad << endl
00173        << "  tilt_pixel_per_rad=" << tilt_pixel_per_rad << endl;
00174   */
00175
00176 }
00177
00178
00179 point_t *
00180 CornerHorizon::operator++()
00181 {
00182   if ( ! calculated) {
00183     calculate();
00184     calculated = true;
00185   }
00186
00187   coord.x = (*model)->x;
00188   coord.y = (*model)->y;
00189
00190   do {
00191     ++(*model);
00192   } while ( ((*model)->y < horizon) && ( ! model->finished()) );
00193
00194   if ( ((*model)->y < horizon) || model->finished() ) {
00195     // finished
00196     //cout << "1 (" << coord.x << "," << coord.y << ")" << endl;
00197     return &coord;
00198   } else {
00199     coord.x = (*model)->x;
00200     coord.y = (*model)->y;
00201     //cout << "2 (" << coord.x << "," << coord.y << ")" << endl;
00202     return &coord;
00203   }
00204 }
00205
00206
00207 point_t *
00208 CornerHorizon::operator++(int)
00209 {
00210   if ( ! calculated) {
00211     calculate();
00212     calculated = true;
00213   }
00214   memcpy(&tmp_coord, &coord, sizeof(point_t));
00215
00216   do {
00217     ++(*model);
00218   } while ( ((*model)->y < horizon) && ! model->finished() );
00219
00220   if ( ((*model)->y >= horizon) && ! model->finished() ) {
00221     coord.x = (*model)->x;
00222     coord.y = (*model)->y;
00223     //cout << "3 (" << coord.x << "," << coord.y << ")" << endl;
00224   }
00225
00226   return &tmp_coord;
00227 }
00228
00229
00230 bool
00231 CornerHorizon::finished()
00232 {
00233   return model->finished();
00234 }
00235
00236
00237 void
00238 CornerHorizon::reset()
00239 {
00240   calculated = false;
00241   coord.x = coord.y = 0;
00242   model->reset();
00243 }
00244
00245
00246 const char *
00247 CornerHorizon::get_name()
00248 {
00249   return "ScanlineModel::CornerHorizon";
00250 }
00251
00252
00253 unsigned int
00254 CornerHorizon::get_margin()
00255 {
00256   return model->get_margin();
00257 }
00258
00259 
00260 /** Get the horizon point.
00261  * @return y coordinate of the horizon point.
00262  */
00263 unsigned int
00264 CornerHorizon::getHorizon()
00265 {
00266   return horizon;
00267 }
00268
00269
00270 void
00271 CornerHorizon::set_robot_pose(float x, float y, float ori)
00272 {
00273   pose_x = x;
00274   pose_y = y;
00275   pose_ori = ori;
00276 }
00277
00278
00279 void
00280 CornerHorizon::set_pan_tilt(float pan, float tilt)
00281 {
00282   this->pan  = pan;
00283   this->tilt = tilt;
00284 }