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 /** @class FrontBallRelativePos <models/relative_position/front_ball.h>
00036  * Relative ball position model for front vision.
00037  */
00038 
00039 /** Constructor.
00040  * @param image_width width of image in pixels
00041  * @param image_height height of image in pixels
00042  * @param camera_height height of camera in meters
00043  * @param camera_offset_x camera offset of the motor axis in x direction
00044  * @param camera_offset_y camera offset of the motor axis in y direction
00045  * @param camera_ori camera orientation compared to the robot
00046  * @param horizontal_angle horizontal viewing angle (in degree)
00047  * @param vertical_angle vertical viewing angle (in degree)
00048  * @param ball_circumference ball circumference
00049  */
00050 FrontBallRelativePos::FrontBallRelativePos(unsigned int image_width,
00051                                            unsigned int image_height,
00052                                            float camera_height,
00053                                            float camera_offset_x,
00054                                            float camera_offset_y,
00055                                            float camera_ori,
00056                                            float horizontal_angle,
00057                                            float vertical_angle,
00058                                            float ball_circumference
00059                                            )
00060 {
00061
00062   this->image_width        = image_width;
00063   this->image_height       = image_height;
00064   this->ball_circumference = ball_circumference;
00065   this->horizontal_angle   = deg2rad( horizontal_angle );
00066   this->vertical_angle     = deg2rad( vertical_angle   );
00067   this->camera_orientation = deg2rad( camera_ori       );
00068   this->camera_height      = camera_height;
00069   this->camera_offset_x    = camera_offset_x;
00070   this->camera_offset_y    = camera_offset_y;
00071
00072   m_fRadius         = 0.0f;
00073   m_cirtCenter.x    = 0.0f;
00074   m_cirtCenter.y    = 0.0f;
00075   m_fPan            = 0.0f;
00076   m_fTilt           = 0.0f;
00077
00078   avg_x = avg_y = avg_x_sum = avg_y_sum = 0.f;
00079   avg_x_num = avg_y_num = 0;
00080
00081   m_fPanRadPerPixel  = this->horizontal_angle   / this->image_width;
00082   m_fTiltRadPerPixel = this->vertical_angle     / this->image_height;
00083   m_fBallRadius      = this->ball_circumference / ( 2 * M_PI );
00084
00085   ball_x = ball_y = bearing = slope = distance_ball_motor = distance_ball_cam = 0.f;
00086
00087   DEFAULT_X_VARIANCE = 1500.f;
00088   DEFAULT_Y_VARIANCE = 1000.f;
00089
00090   var_proc_x = 1500;
00091   var_proc_y = 1000;
00092   var_meas_x = 1500;
00093   var_meas_y = 1000;
00094
00095   /*
00096   // initial variance for ball pos kf
00097   kalman_filter = new kalmanFilter2Dim();
00098   CMatrix<float> initialStateVarianceBall(2,2);
00099   initialStateVarianceBall[0][0] = DEFAULT_X_VARIANCE;
00100   initialStateVarianceBall[1][0] = 0.00;
00101   initialStateVarianceBall[0][1] = 0.00;
00102   initialStateVarianceBall[1][1] = DEFAULT_Y_VARIANCE;
00103   kalman_filter->setInitialStateCovariance( initialStateVarianceBall ); 
00104 
00105   // process noise for ball pos kf
00106   kalman_filter->setProcessCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
00107   kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
00108   */
00109 }
00110
00111
00112 float
00113 FrontBallRelativePos::get_distance() const 
00114 {
00115   return distance_ball_motor;
00116 }
00117
00118
00119 float
00120 FrontBallRelativePos::get_bearing(void) const
00121 {
00122   return bearing;
00123 }
00124
00125
00126 float
00127 FrontBallRelativePos::get_slope() const
00128 {
00129   return slope;
00130 }
00131
00132
00133 float
00134 FrontBallRelativePos::get_y(void) const
00135 {
00136   return ball_y;
00137 }
00138
00139
00140 float
00141 FrontBallRelativePos::get_x(void) const
00142 {
00143   return ball_x;
00144 }
00145
00146
00147 void
00148 FrontBallRelativePos::set_center(float x, float y)
00149 {
00150   m_cirtCenter.x = x;
00151   m_cirtCenter.y = y;
00152 }
00153
00154
00155 void
00156 FrontBallRelativePos::set_center(const center_in_roi_t& c)
00157 {
00158   m_cirtCenter.x = c.x;
00159   m_cirtCenter.y = c.y;
00160 }
00161
00162
00163 void
00164 FrontBallRelativePos::set_radius(float r)
00165 {
00166   m_fRadius = r;
00167 }
00168
00169 
00170 /** Get the ball radius.
00171  * @return ball radius
00172  */
00173 float
00174 FrontBallRelativePos::get_radius() const
00175 {
00176   return m_fRadius;
00177 }
00178
00179
00180 void
00181 FrontBallRelativePos::set_pan_tilt(float pan, float tilt)
00182 {
00183   m_fPan = pan;
00184   m_fTilt = tilt;
00185 }
00186
00187
00188 void
00189 FrontBallRelativePos::get_pan_tilt(float *pan, float *tilt) const
00190 {
00191   *pan  = m_fPan;
00192   *tilt = m_fTilt;
00193 }
00194
00195
00196 const char *
00197 FrontBallRelativePos::get_name() const
00198 {
00199   return "FrontBallRelativePos";
00200 }
00201
00202 
00203 /** Set horizontal viewing angle.
00204  * @param angle_deg horizontal viewing angle in degree
00205  */
00206 void
00207 FrontBallRelativePos::set_horizontal_angle(float angle_deg)
00208 {
00209   horizontal_angle = deg2rad( angle_deg );
00210 }
00211
00212 
00213 /** Set vertical viewing angle.
00214  * @param angle_deg horizontal viewing angle in degree
00215  */
00216 void
00217 FrontBallRelativePos::set_vertical_angle(float angle_deg)
00218 {
00219   vertical_angle = deg2rad( angle_deg );
00220 }
00221
00222
00223 void
00224 FrontBallRelativePos::reset()
00225 {
00226   last_available = false;
00227   //kalman_filter->reset();
00228 }
00229
00230 void
00231 FrontBallRelativePos::calc()
00232 {
00233
00234   /*
00235   char user_input = toupper( getkey() );
00236 
00237   if (user_input == 'P') {
00238     cout << "Enter new kalman process variance values (X Y):" << flush;
00239     cin >> var_proc_x >> var_proc_y;
00240   } else if (user_input == 'M') {
00241     cout << "Enter new kalman measurement variance values (X Y):" << flush;
00242     cin >> var_meas_x >> var_meas_y;
00243   } else if (user_input == 'R') {
00244     cout << "Reset" << endl;
00245     reset();
00246   }
00247   */
00248
00249   float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
00250
00251   /* projection of air-line-distance on the ground (Pythagoras) */
00252   distance_ball_cam = sqrt( tmp * tmp    -
00253                             (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius)  );
00254
00255
00256 #ifdef OLD_COORD_SYS
00257   /* Bearing shall be clockwise positive. */
00258   bearing =   (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
00259 #else
00260   /* Bearing shall be counter-clockwise positive. */
00261   bearing = - (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
00262 #endif
00263 
00264   /* Slope shall be downward negative */
00265   slope   = - ((m_cirtCenter.y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
00266
00267   ball_x = cos( bearing ) * distance_ball_cam + camera_offset_x;
00268   ball_y = sin( bearing ) * distance_ball_cam + camera_offset_y;
00269
00270   // applyKalmanFilter();
00271
00272   distance_ball_motor = sqrt( ball_x * ball_x  +  ball_y * ball_y );
00273
00274 }
00275
00276
00277 bool
00278 FrontBallRelativePos::is_pos_valid() const
00279 {
00280   return true;
00281 }
00282
00283
00284 void
00285 FrontBallRelativePos::calc_unfiltered()
00286 {
00287   float tmp = m_fBallRadius / sin(m_fRadius * m_fPanRadPerPixel);
00288
00289   /* projection of air-line-distance on the ground
00290      (Pythagoras) */
00291   distance_ball_cam = sqrt( tmp                             * tmp                            -
00292                             (camera_height - m_fBallRadius) * (camera_height - m_fBallRadius)  );
00293
00294
00295 #ifdef OLD_COORD_SYS
00296   /* Bearing shall be clockwise positive. */
00297   bearing =   (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
00298 #else
00299   /* Bearing shall be counter-clockwise positive. */
00300   bearing = - (((m_cirtCenter.x - image_width/2) * m_fPanRadPerPixel + m_fPan + camera_orientation));
00301 #endif
00302 
00303   // invert sign, because slope downward shall be negative
00304   slope   = - ((m_cirtCenter.y - image_height / 2) * m_fTiltRadPerPixel - m_fTilt);
00305
00306
00307   ball_x = cos( bearing ) * distance_ball_cam + camera_offset_x;
00308   ball_y = sin( bearing ) * distance_ball_cam + camera_offset_y;
00309
00310   distance_ball_motor = sqrt( ball_x * ball_x  +  ball_y * ball_y );
00311
00312 }
00313
00314
00315 /*
00316 void
00317 FrontBallRelativePos::applyKalmanFilter()
00318 {
00319 
00320   kalman_filter->setMeasurementCovariance( var_meas_x, var_meas_y );
00321   kalman_filter->setProcessCovariance( var_proc_x, var_proc_y );
00322 
00323   last_x = ball_x;
00324   last_y = ball_y;
00325 
00326   kalman_filter->setMeasurementX( ball_x );
00327   kalman_filter->setMeasurementY( ball_y );
00328   kalman_filter->doCalculation();
00329 
00330   ball_x = kalman_filter->getStateX();
00331   ball_y = kalman_filter->getStateY();
00332 
00333   if ( isnan( ball_x ) || isinf( ball_x ) ||
00334        isnan( ball_y ) || isinf( ball_y ) ) {
00335     // Kalman is wedged, use unfiltered result and reset filter
00336     kalman_filter->reset();
00337     ball_x = last_x;
00338     ball_y = last_y;
00339   }
00340 
00341 }
00342 */