cornerhorizon.cpp

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