Fawkes API Fawkes Development Version

front_ball.cpp

00001 
00002 /***************************************************************************
00003  *  front_ball.cpp - Implementation of the relative ball position model for
00004  *                   the front vision
00005  *
00006  *  Created: Fri Jun 03 22:56:22 2005
00007  *  Copyright  2005       Hu Yuxiao      <Yuxiao.Hu@rwth-aachen.de>
00008  *             2005-2006  Tim Niemueller [www.niemueller.de]
00009  *             2005       Martin Heracles <Martin.Heracles@rwth-aachen.de>
00010  *
00011  ****************************************************************************/
00012 
00013 /*  This program is free software; you can redistribute it and/or modify
00014  *  it under the terms of the GNU General Public License as published by
00015  *  the Free Software Foundation; either version 2 of the License, or
00016  *  (at your option) any later version. A runtime exception applies to
00017  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00018  *
00019  *  This program is distributed in the hope that it will be useful,
00020  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00021  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022  *  GNU Library General Public License for more details.
00023  *
00024  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00025  */
00026 
00027 #include <cmath>
00028 #include <iostream>
00029 #include <models/relative_position/front_ball.h>
00030 #include <utils/math/angle.h>
00031 
00032 using namespace std;
00033 using namespace fawkes;
00034 
00035 namespace firevision {
00036 #if 0 /* just to make Emacs auto-indent happy */
00037 }
00038 #endif
00039 
00040 /** @class FrontBallRelativePos <models/relative_position/front_ball.h>
00041  * Relative ball position model for front vision.
00042  */
00043 
00044 /** Constructor.
00045  * @param image_width width of image in pixels
00046  * @param image_height height of image in pixels
00047  * @param camera_height height of camera in meters
00048  * @param camera_offset_x camera offset of the motor axis in x direction
00049  * @param camera_offset_y camera offset of the motor axis in y direction
00050  * @param camera_ori camera orientation compared to the robot
00051  * @param horizontal_angle horizontal viewing angle (in degree)
00052  * @param vertical_angle vertical viewing angle (in degree)
00053  * @param ball_circumference ball circumference
00054  */
00055 FrontBallRelativePos::FrontBallRelativePos(unsigned int image_width,
00056                                            unsigned int image_height,
00057                                            float camera_height,
00058                                            float camera_offset_x,
00059                                            float camera_offset_y,
00060                                            float camera_ori,
00061                                            float horizontal_angle,
00062                                            float vertical_angle,
00063                                            float ball_circumference
00064                                            )
00065 {
00066 
00067   this->image_width        = image_width;
00068   this->image_height       = image_height;
00069   this->ball_circumference = ball_circumference;
00070   this->horizontal_angle   = deg2rad( horizontal_angle );
00071   this->vertical_angle     = deg2rad( vertical_angle   );
00072   this->camera_orientation = deg2rad( camera_ori       );
00073   this->camera_height      = camera_height;
00074   this->camera_offset_x    = camera_offset_x;
00075   this->camera_offset_y    = camera_offset_y;
00076 
00077   m_fRadius         = 0.0f;
00078   m_cirtCenter.x    = 0.0f;
00079   m_cirtCenter.y    = 0.0f;
00080   m_fPan            = 0.0f;
00081   m_fTilt           = 0.0f;
00082 
00083   avg_x = avg_y = avg_x_sum = avg_y_sum = 0.f;
00084   avg_x_num = avg_y_num = 0;
00085 
00086   m_fPanRadPerPixel  = this->horizontal_angle   / this->image_width;
00087   m_fTiltRadPerPixel = this->vertical_angle     / this->image_height;
00088   m_fBallRadius      = this->ball_circumference / ( 2 * M_PI );
00089 
00090   ball_x = ball_y = bearing = slope = distance_ball_motor = distance_ball_cam = 0.f;
00091 
00092   DEFAULT_X_VARIANCE = 1500.f;
00093   DEFAULT_Y_VARIANCE = 1000.f;
00094 
00095   var_proc_x = 1500;
00096   var_proc_y = 1000;
00097   var_meas_x = 1500;
00098   var_meas_y = 1000;
00099 
00100   /*
00101   // initial variance for ball pos kf
00102   kalman_filter = new kalmanFilter2Dim();
00103   CMatrix<float> initialStateVarianceBall(2,2);
00104   initialStateVarianceBall[0][0] = DEFAULT_X_VARIANCE;
00105   initialStateVarianceBall[1][0] = 0.00;
00106   initialStateVarianceBall[0][1] = 0.00;
00107   initialStateVarianceBall[1][1] = DEFAULT_Y_VARIANCE;
00108   kalman_filter->setInitialStateCovariance( initialStateVarianceBall ); 
00109 
00110   // process noise for ball pos kf
00111   kalman_filter->setProcessCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
00112   kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
00113   */
00114 }
00115 
00116 
00117 float
00118 FrontBallRelativePos::get_distance() const 
00119 {
00120   return distance_ball_motor;
00121 }
00122 
00123 
00124 float
00125 FrontBallRelativePos::get_bearing(void) const
00126 {
00127   return bearing;
00128 }
00129 
00130 
00131 float
00132 FrontBallRelativePos::get_slope() const
00133 {
00134   return slope;
00135 }
00136 
00137 
00138 float
00139 FrontBallRelativePos::get_y(void) const
00140 {
00141   return ball_y;
00142 }
00143 
00144 
00145 float
00146 FrontBallRelativePos::get_x(void) const
00147 {
00148   return ball_x;
00149 }
00150 
00151 
00152 void
00153 FrontBallRelativePos::set_center(float x, float y)
00154 {
00155   m_cirtCenter.x = x;
00156   m_cirtCenter.y = y;
00157 }
00158 
00159 
00160 void
00161 FrontBallRelativePos::set_center(const center_in_roi_t& c)
00162 {
00163   m_cirtCenter.x = c.x;
00164   m_cirtCenter.y = c.y;
00165 }
00166 
00167 
00168 void
00169 FrontBallRelativePos::set_radius(float r)
00170 {
00171   m_fRadius = r;
00172 }
00173 
00174 
00175 /** Get the ball radius.
00176  * @return ball radius
00177  */
00178 float
00179 FrontBallRelativePos::get_radius() const
00180 {
00181   return m_fRadius;
00182 }
00183 
00184 
00185 void
00186 FrontBallRelativePos::set_pan_tilt(float pan, float tilt)
00187 {
00188   m_fPan = pan;
00189   m_fTilt = tilt;
00190 }
00191 
00192 
00193 void
00194 FrontBallRelativePos::get_pan_tilt(float *pan, float *tilt) const
00195 {
00196   *pan  = m_fPan;
00197   *tilt = m_fTilt;
00198 }
00199 
00200 
00201 const char *
00202 FrontBallRelativePos::get_name() const
00203 {
00204   return "FrontBallRelativePos";
00205 }
00206 
00207 
00208 /** Set horizontal viewing angle.
00209  * @param angle_deg horizontal viewing angle in degree
00210  */
00211 void
00212 FrontBallRelativePos::set_horizontal_angle(float angle_deg)
00213 {
00214   horizontal_angle = deg2rad( angle_deg );
00215 }
00216 
00217 
00218 /** Set vertical viewing angle.
00219  * @param angle_deg horizontal viewing angle in degree
00220  */
00221 void
00222 FrontBallRelativePos::set_vertical_angle(float angle_deg)
00223 {
00224   vertical_angle = deg2rad( angle_deg );
00225 }
00226 
00227 
00228 void
00229 FrontBallRelativePos::reset()
00230 {
00231   last_available = false;
00232   //kalman_filter->reset();
00233 }
00234 
00235 void
00236 FrontBallRelativePos::calc()
00237 {
00238 
00239   /*
00240   char user_input = toupper( getkey() );
00241 
00242   if (user_input == 'P') {
00243     cout << "Enter new kalman process variance values (X Y):" << flush;
00244     cin >> var_proc_x >> var_proc_y;
00245   } else if (user_input == 'M') {
00246     cout << "Enter new kalman measurement variance values (X Y):" << flush;
00247     cin >> var_meas_x >> var_meas_y;
00248   } else if (user_input == 'R') {
00249     cout << "Reset" << endl;
00250     reset();
00251   }
00252   */
00253 
00254   float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
00255   
00256   /* projection of air-line-distance on the ground (Pythagoras) */
00257   distance_ball_cam = sqrt( tmp * tmp    -
00258                             (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius)  );
00259 
00260 
00261 #ifdef OLD_COORD_SYS
00262   /* Bearing shall be clockwise positive. */
00263   bearing =   (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
00264 #else
00265   /* Bearing shall be counter-clockwise positive. */
00266   bearing = - (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
00267 #endif
00268 
00269   /* Slope shall be downward negative */
00270   slope   = - ((m_cirtCenter.y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
00271 
00272   ball_x = cos( bearing ) * distance_ball_cam + camera_offset_x;
00273   ball_y = sin( bearing ) * distance_ball_cam + camera_offset_y;
00274 
00275   // applyKalmanFilter();
00276 
00277   distance_ball_motor = sqrt( ball_x * ball_x  +  ball_y * ball_y );
00278 
00279 }
00280 
00281 
00282 bool
00283 FrontBallRelativePos::is_pos_valid() const
00284 {
00285   return true;
00286 }
00287 
00288 
00289 void
00290 FrontBallRelativePos::calc_unfiltered()
00291 {
00292   float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
00293   
00294   /* projection of air-line-distance on the ground
00295      (Pythagoras) */
00296   distance_ball_cam = sqrt( tmp                             * tmp                            -
00297                             (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius)  );
00298 
00299 
00300 #ifdef OLD_COORD_SYS
00301   /* Bearing shall be clockwise positive. */
00302   bearing =   (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
00303 #else
00304   /* Bearing shall be counter-clockwise positive. */
00305   bearing = - (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
00306 #endif
00307 
00308   // invert sign, because slope downward shall be negative
00309   slope   = - ((m_cirtCenter.y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
00310 
00311 
00312   ball_x = cos( bearing ) * distance_ball_cam + camera_offset_x;
00313   ball_y = sin( bearing ) * distance_ball_cam + camera_offset_y;
00314 
00315   distance_ball_motor = sqrt( ball_x * ball_x  +  ball_y * ball_y );
00316 
00317 }
00318 
00319 
00320 /*
00321 void
00322 FrontBallRelativePos::applyKalmanFilter()
00323 {
00324 
00325   kalman_filter->setMeasurementCovariance( var_meas_x, var_meas_y );
00326   kalman_filter->setProcessCovariance( var_proc_x, var_proc_y );
00327 
00328   last_x = ball_x;
00329   last_y = ball_y;
00330 
00331   kalman_filter->setMeasurementX( ball_x );
00332   kalman_filter->setMeasurementY( ball_y );
00333   kalman_filter->doCalculation();
00334 
00335   ball_x = kalman_filter->getStateX();
00336   ball_y = kalman_filter->getStateY();
00337 
00338   if ( isnan( ball_x ) || isinf( ball_x ) ||
00339        isnan( ball_y ) || isinf( ball_y ) ) {
00340     // Kalman is wedged, use unfiltered result and reset filter
00341     kalman_filter->reset();
00342     ball_x = last_x;
00343     ball_y = last_y;
00344   }
00345 
00346 }
00347 */
00348 
00349 } // end namespace firevision
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends