relvelo.cpp

00001 
00002 /***************************************************************************
00003  *  relvelo.cpp - Implementation of velocity model based on relative ball
00004  *                 positions and relative robot velocity
00005  *
00006  *  Created: Tue Oct 04 15:54:27 2005
00007  *  Copyright  2005  Tim Niemueller [www.niemueller.de]
00008  *
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version. A runtime exception applies to
00015  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00016  *
00017  *  This program is distributed in the hope that it will be useful,
00018  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00019  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00020  *  GNU Library General Public License for more details.
00021  *
00022  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00023  */
00024 
00025 #include <models/velocity/relvelo.h>
00026 
00027 #include <utils/system/console_colors.h>
00028 #include <utils/time/time.h>
00029 
00030 #include <cmath>
00031 #include <cstdlib>
00032 
00033 using namespace std;
00034 
00035 namespace firevision {
00036 #if 0 /* just to make Emacs auto-indent happy */
00037 }
00038 #endif
00039 
00040 /** @class VelocityFromRelative <models/velocity/relvelo.h>
00041  * Calculate velocity from relative positions.
00042  */
00043 
00044 /** Constructor.
00045  * @param model relative position model
00046  * @param max_history_length maximum history length
00047  * @param calc_interval calculation interval
00048  */
00049 VelocityFromRelative::VelocityFromRelative(RelativePositionModel* model,
00050                                            unsigned int max_history_length,
00051                                            unsigned int calc_interval)
00052 {
00053   this->relative_pos_model = model;
00054   this->max_history_length = max_history_length;
00055   this->calc_interval      = calc_interval;
00056 
00057   //kalman_enabled = true;
00058 
00059   robot_rel_vel_x = robot_rel_vel_y = 0.f;
00060 
00061   velocity_x = velocity_y = 0.f;
00062 
00063   /*
00064   // initial variance for ball pos
00065   var_proc_x = 300;
00066   var_proc_y =  50;
00067   var_meas_x = 300;
00068   var_meas_y =  50;
00069 
00070   // initial variance for ball pos
00071   kalman_filter = new kalmanFilter2Dim();
00072   CMatrix<float> initialStateVarianceBall(2,2);
00073   initialStateVarianceBall[0][0] = var_meas_x;
00074   initialStateVarianceBall[1][0] =    0.0;
00075   initialStateVarianceBall[0][1] =    0.0;
00076   initialStateVarianceBall[1][1] = var_meas_y;
00077   kalman_filter->setInitialStateCovariance( initialStateVarianceBall ); 
00078 
00079   // process noise for ball pos kf, initial estimates, refined in calc()
00080   kalman_filter->setProcessCovariance( var_proc_x, var_proc_y );
00081   kalman_filter->setMeasurementCovariance( var_meas_x, var_meas_y );
00082   */
00083 
00084   avg_vx_sum = 0.f;
00085   avg_vx_num = 0;
00086 
00087   avg_vy_sum = 0.f;
00088   avg_vy_num = 0;
00089 
00090   ball_history.clear();
00091 
00092 }
00093 
00094 
00095 /** Destructor. */
00096 VelocityFromRelative::~VelocityFromRelative()
00097 {
00098 }
00099 
00100 
00101 void
00102 VelocityFromRelative::setPanTilt(float pan, float tilt)
00103 {
00104 }
00105 
00106 
00107 void
00108 VelocityFromRelative::setRobotPosition(float x, float y, float ori, timeval t)
00109 {
00110 }
00111 
00112 
00113 void
00114 VelocityFromRelative::setRobotVelocity(float rel_vel_x, float rel_vel_y, timeval t)
00115 {
00116   robot_rel_vel_x = rel_vel_x;
00117   robot_rel_vel_y = rel_vel_y;
00118   robot_rel_vel_t.tv_sec = t.tv_sec;
00119   robot_rel_vel_t.tv_usec = t.tv_usec;
00120 }
00121 
00122 void
00123 VelocityFromRelative::setTime(timeval t)
00124 {
00125   now.tv_sec  = t.tv_sec;
00126   now.tv_usec = t.tv_usec;
00127 }
00128 
00129 
00130 void
00131 VelocityFromRelative::setTimeNow()
00132 {
00133   gettimeofday(&now, NULL);
00134 }
00135 
00136 
00137 void
00138 VelocityFromRelative::getTime(long int *sec, long int *usec)
00139 {
00140   *sec  = now.tv_sec;
00141   *usec = now.tv_usec;
00142 }
00143 
00144 
00145 void
00146 VelocityFromRelative::getVelocity(float *vel_x, float *vel_y)
00147 {
00148   if (vel_x != NULL) {
00149     *vel_x = velocity_x;
00150   }
00151   if (vel_y != NULL) {
00152     *vel_y = velocity_y;
00153   }
00154 }
00155 
00156 
00157 float
00158 VelocityFromRelative::getVelocityX()
00159 {
00160   return velocity_x;
00161 }
00162 
00163 
00164 float
00165 VelocityFromRelative::getVelocityY()
00166 {
00167   return velocity_y;
00168 }
00169 
00170 
00171 void
00172 VelocityFromRelative::calc()
00173 {
00174   /*
00175   char user_input = toupper( getkey() );
00176 
00177   if ( ! relative_pos_model->isPosValid() ) {
00178     return;
00179   }
00180 
00181   if (user_input == 'P') {
00182     cout << "Enter new kalman process variance values (X Y):" << flush;
00183     cin >> var_proc_x >> var_proc_y;
00184   } else if (user_input == 'M') {
00185     cout << "Enter new kalman measurement variance values (X Y):" << flush;
00186     cin >> var_meas_x >> var_meas_y;
00187   } else if (user_input == 'R') {
00188     cout << "Reset" << endl;
00189     reset();
00190   } else if (user_input == 'C') {
00191     cout << "Current kalman measurement variance (X Y) = ("
00192          << var_meas_x << " " << var_meas_y << ")" << endl
00193          << "Current kalman process variance (X Y)     = ("
00194          << var_proc_x << " " << var_proc_y << ")" << endl;
00195   } else if (user_input == 'K') {
00196     kalman_enabled = ! kalman_enabled;
00197     if ( kalman_enabled ) {
00198       cout << "Kalman filtering enabled" << endl;
00199       kalman_filter->reset();
00200     } else {
00201       cout << "Kalman filtering disabled" << endl;
00202     }
00203   }
00204   */
00205 
00206   // Gather needed data
00207   cur_ball_x = relative_pos_model->get_x();
00208   cur_ball_y = relative_pos_model->get_y();
00209   cur_ball_dist = relative_pos_model->get_distance();
00210 
00211   if ( isnan(cur_ball_x) || isinf(cur_ball_x) ||
00212        isnan(cur_ball_y) || isinf(cur_ball_y) ||
00213        isnan(cur_ball_dist) || isinf(cur_ball_dist) ) {
00214     // cout << cred << "relative position model returned nan/inf value(s)!" << cnormal << endl;
00215     return;
00216   }
00217 
00218   // if we project the last ball position by the velocity we calculated
00219   // at that time we can compare this to the current position and estimate
00220   // an error from this information
00221   if (last_available) {
00222     proj_time_diff_sec = fawkes::time_diff_sec(now, last_time);
00223     proj_x = last_x + velocity_x * proj_time_diff_sec;
00224     proj_y = last_y + velocity_y * proj_time_diff_sec;
00225     last_proj_error_x = cur_ball_x - proj_x;
00226     last_proj_error_y = cur_ball_y - proj_y;
00227     last_available = false;
00228   } else {
00229     last_proj_error_x = cur_ball_x;
00230     last_proj_error_y = cur_ball_x;
00231   }
00232 
00233 
00234   // newest entry first
00235   vel_postime_t *vpt = (vel_postime_t *)malloc(sizeof(vel_postime_t));;
00236   vpt->x             = cur_ball_x;
00237   vpt->y             = cur_ball_y;
00238   vpt->t.tv_sec      = now.tv_sec;
00239   vpt->t.tv_usec     = now.tv_usec;
00240   ball_history.push_front( vpt );
00241     
00242 
00243   if (ball_history.size() >= 2) {
00244 
00245     // we need at least two entries
00246     // take the last robot velocity, then find the corresponding ball_pos entry
00247     // in the history and an entry about 100ms away to extrapolate the
00248     // ball velocity, then correct this by the robot's velocity we got
00249 
00250     if ( fawkes::time_diff_sec(robot_rel_vel_t, vel_last_time) != 0 ) {
00251       // We have a new robot position data, calculate new velocity
00252 
00253       vel_last_time.tv_sec  = robot_rel_vel_t.tv_sec;
00254       vel_last_time.tv_usec = robot_rel_vel_t.tv_usec;
00255 
00256       f_diff_sec = HUGE;
00257       float time_diff;
00258       vel_postime_t *young = NULL;
00259       vel_postime_t *old   = NULL;
00260       unsigned int  step = 0;
00261       for (bh_it = ball_history.begin(); bh_it != ball_history.end(); ++bh_it) {
00262         // Find the ball pos history entry closest in time (but still younger) to
00263         // the new position data
00264         time_diff = fawkes::time_diff_sec((*bh_it)->t, robot_rel_vel_t);
00265         if ( (time_diff > 0) && (time_diff < f_diff_sec) ) {
00266           f_diff_sec = time_diff;
00267           young = (*bh_it);
00268         } else {
00269           // Now find second time
00270           if (step != calc_interval) {
00271             ++step;
00272           } else {
00273             // Found a second time
00274             old = *bh_it;
00275             ++bh_it;
00276             break;
00277           }
00278         }
00279       }
00280 
00281       if ((young != NULL) && (old != NULL)) {
00282         // we found two valid times
00283 
00284         diff_x = young->x - old->x;
00285         diff_y = young->y - old->y;
00286       
00287         f_diff_sec = fawkes::time_diff_sec(young->t, old->t);
00288 
00289         velocity_x = diff_x / f_diff_sec;
00290         velocity_y = diff_y / f_diff_sec;
00291 
00292         //cout << "f_diff_sec=" << f_diff_sec << "  vx=" << velocity_x << "  vy=" << velocity_y << endl;
00293         
00294         velocity_x += robot_rel_vel_x;
00295         velocity_y += robot_rel_vel_y;
00296 
00297         velocity_x -= last_proj_error_x * proj_time_diff_sec;
00298         velocity_y -= last_proj_error_y * proj_time_diff_sec;
00299 
00300         //cout << "vx+rx=" << velocity_x << "  vy+ry=" << velocity_y << endl;
00301         
00302         /*
00303         cout << endl
00304              << "VELOCITY CALCULATION" << endl
00305              << "  History size  : " << ball_history.size() << endl
00306              << "  Ball position" << endl
00307              << "    young       : (" << young->x << ", " << young->y << ")" << endl
00308              << "    old         : (" << old->x << ", " << old->y << ")" << endl
00309              << "    difference  : " << diff_x << ", " << diff_y << ")" << endl
00310              << "  Time" << endl
00311              << "    current     :  " << young->t.tv_sec << " sec, " << young->t.tv_usec << " usec" << endl
00312              << "    old         :  " << old->t.tv_sec << " sec, " << old->t.tv_usec << " usec" << endl
00313              << "    difference  :  " << f_diff_sec << " sec" << endl
00314              << "  Projection" << endl
00315              << "  proj error    : (" << last_proj_error_x << "," << last_proj_error_y << ")" << endl
00316              << "  Velocity" << endl
00317              << "    robot       : (" << robot_rel_vel_x << ", " << robot_rel_vel_y << ")" << endl
00318              << "    Ball" << endl
00319              << "      raw       : (" << velocity_x - robot_rel_vel_x << ", " << velocity_y - robot_rel_vel_y << ")" << endl
00320              << "      corrected : (" << velocity_x << ", " << velocity_y << ")" << endl;
00321         */
00322 
00323         /*
00324         if ( kalman_enabled ) {
00325           applyKalmanFilter();
00326         }
00327         */
00328 
00329         last_x = cur_ball_x;
00330         last_y = cur_ball_y;
00331         last_time.tv_sec = now.tv_sec;
00332         last_time.tv_usec = now.tv_usec;
00333         last_available = true;
00334 
00335         /*
00336         cout << "      filtered  : (" << clightpurple << velocity_x << cnormal
00337              << ", " << clightpurple << velocity_y << cnormal << ")" << endl
00338              << endl;
00339         */
00340 
00341         // erase old history entries
00342         if (bh_it != ball_history.end()) {
00343           ball_history.erase(bh_it, ball_history.end());
00344         }
00345       } else {
00346         // cout << "did not find matching young and old record" << endl;
00347         velocity_x = 0.f;
00348         velocity_y = 0.f;
00349       }
00350     } else {
00351       // we did not get a new robot position, keep old velocities for 2 seconds
00352       if (fawkes::time_diff_sec(now, vel_last_time) > 2) {
00353         // cout << "did not get new robot position for more than 2 sec, resetting" << endl;
00354         velocity_x = 0.f;
00355         velocity_y = 0.f;
00356       }
00357     }
00358   } else {
00359     // cout << "history too short" << endl;
00360     velocity_x = 0.f;
00361     velocity_y = 0.f;
00362   }
00363 
00364   if (ball_history.size() > max_history_length) {
00365     bh_it = ball_history.begin();
00366     for (unsigned int i = 0; i < max_history_length; ++i) {
00367       ++bh_it;
00368     }
00369     ball_history.erase(bh_it, ball_history.end());
00370   }
00371 
00372 }
00373 
00374 
00375 void
00376 VelocityFromRelative::reset()
00377 {
00378   /*
00379   if (kalman_enabled) {
00380     kalman_filter->reset();
00381   }
00382   */
00383   avg_vx_sum = 0.f;
00384   avg_vx_num = 0;
00385   avg_vy_sum = 0.f;
00386   avg_vy_num = 0;
00387   velocity_x = 0.f;
00388   velocity_y = 0.f;
00389   ball_history.clear();
00390 }
00391 
00392 
00393 const char *
00394 VelocityFromRelative::getName() const
00395 {
00396   return "VelocityModel::VelocityFromRelative";
00397 }
00398 
00399 
00400 coordsys_type_t
00401 VelocityFromRelative::getCoordinateSystem()
00402 {
00403   return COORDSYS_ROBOT_CART;
00404 }
00405 
00406 /*
00407 void
00408 VelocityFromRelative::applyKalmanFilter()
00409 {
00410   /
00411   avg_vx_sum += velocity_x;
00412   avg_vy_sum += velocity_y;
00413 
00414   ++avg_vx_num;
00415   ++avg_vy_num;
00416 
00417   avg_vx = avg_vx_sum / avg_vx_num;
00418   avg_vy = avg_vy_sum / avg_vy_num;
00419 
00420   age_factor = (fawkes::time_diff_sec(now, robot_rel_vel_t) + f_diff_sec);
00421 
00422   rx = (velocity_x - avg_vx) * age_factor;
00423   ry = (velocity_y - avg_vy) * age_factor;
00424 
00425   kalman_filter->setProcessCovariance( rx * rx, ry * ry );
00426 
00427   rx = (velocity_x - avg_vx) * cur_ball_dist;
00428   ry = (velocity_y - avg_vy) * cur_ball_dist;
00429 
00430   kalman_filter->setMeasurementCovariance( rx * rx, ry * ry );
00431   /
00432 
00433   kalman_filter->setProcessCovariance( var_proc_x * cur_ball_dist,
00434                                        var_proc_y * cur_ball_dist);
00435   kalman_filter->setMeasurementCovariance( var_meas_x * cur_ball_dist,
00436                                            var_meas_y * cur_ball_dist );
00437 
00438   kalman_filter->setMeasurementX( velocity_x );
00439   kalman_filter->setMeasurementY( velocity_y );
00440   kalman_filter->doCalculation();
00441 
00442   velocity_x = kalman_filter->getStateX();
00443   velocity_y = kalman_filter->getStateY();
00444 
00445   velocity_x = round( velocity_x * 10 ) / 10;
00446   velocity_y = round( velocity_y * 10 ) / 10;
00447 
00448   if (isnan(velocity_x) || isinf(velocity_x) ||
00449       isnan(velocity_y) || isinf(velocity_y) ) {
00450     reset();
00451   }
00452 
00453 }
00454 */
00455 
00456 } // end namespace firevision

Generated on Tue Feb 22 13:31:17 2011 for Fawkes API by  doxygen 1.4.7