00001
00002 /***************************************************************************
00003  *  omni_relative.cpp - Implementation of the relative ball model
00004  *                      for the omni cam
00005  *
00006  *  Generated: Thu Mar 23 22:00:15 2006
00007  *  Copyright  2006  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 <cmath>
00026 #include <iostream>
00027 #include <models/relative_position/omni_relative.h>
00028 
00029 /** @class OmniRelative <models/relative_position/omni_relative.h>
00030  * Omni vision relative position model.
00031  */
00032 
00033 /** Constructor.
00034  * @param mirror_model mirror model
00035  */
00036 OmniRelative::OmniRelative(MirrorModel *mirror_model)
00037 {
00038   this->mirror_model = mirror_model;
00039
00040   avg_x = avg_y = avg_x_sum = avg_y_sum = 0.f;
00041   avg_x_num = avg_y_num = 0;
00042
00043   ball_x = ball_y = bearing = slope = distance_ball_motor = distance_ball_cam = 0.f;
00044
00045   // cannot calculate yet
00046   slope = 0;
00047
00048   DEFAULT_X_VARIANCE = 36.f;
00049   DEFAULT_Y_VARIANCE = 25.f;
00050
00051   /*
00052   // initial variance for ball pos kf
00053   kalman_filter = new kalmanFilter2Dim();
00054   CMatrix<float> initialStateVarianceBall(2,2);
00055   initialStateVarianceBall[0][0] = DEFAULT_X_VARIANCE;
00056   initialStateVarianceBall[1][0] = 0.00;
00057   initialStateVarianceBall[0][1] = 0.00;
00058   initialStateVarianceBall[1][1] = DEFAULT_Y_VARIANCE;
00059   kalman_filter->setInitialStateCovariance( initialStateVarianceBall ); 
00060 
00061   // process noise for ball pos kf
00062   kalman_filter->setProcessCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
00063   kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
00064   */
00065
00066 }
00067
00068
00069 float
00070 OmniRelative::get_distance() const 
00071 {
00072   return distance_ball_motor;
00073 }
00074
00075
00076 float
00077 OmniRelative::get_bearing(void) const
00078 {
00079   return bearing;
00080 }
00081
00082
00083 float
00084 OmniRelative::get_slope() const
00085 {
00086   return slope;
00087 }
00088
00089
00090 float
00091 OmniRelative::get_y(void) const
00092 {
00093   return ball_y;
00094 }
00095
00096
00097 float
00098 OmniRelative::get_x(void) const
00099 {
00100   return ball_x;
00101 }
00102
00103
00104 void
00105 OmniRelative::set_center(float x, float y)
00106 {
00107   image_x = (unsigned int)roundf(x);
00108   image_y = (unsigned int)roundf(y);
00109 }
00110
00111
00112 void
00113 OmniRelative::set_center(const center_in_roi_t& c)
00114 {
00115 }
00116
00117
00118 void
00119 OmniRelative::set_radius(float r)
00120 {
00121 }
00122
00123 
00124 /** Get radius.
00125  * @return always 0
00126  */
00127 float
00128 OmniRelative::get_radius() const
00129 {
00130   return 0;
00131 }
00132
00133
00134 void
00135 OmniRelative::set_pan_tilt(float pan, float tilt)
00136 {
00137 }
00138
00139
00140 void
00141 OmniRelative::get_pan_tilt(float *pan, float *tilt) const
00142 {
00143 }
00144
00145
00146 const char *
00147 OmniRelative::get_name() const
00148 {
00149   return "OmniRelative";
00150 }
00151
00152
00153 void
00154 OmniRelative::reset()
00155 {
00156   last_available = false;
00157   //kalman_filter->reset();
00158 }
00159
00160
00161 void
00162 OmniRelative::calc()
00163 {
00164
00165   if ( mirror_model->isValidPoint( image_x, image_y ) ) {
00166     fawkes::polar_coord_2d_t rel_pos = mirror_model->getWorldPointRelative( image_x, image_y );
00167
00168     distance_ball_cam = rel_pos.r;
00169     bearing           = rel_pos.phi;
00170
00171     // assumes camera to be centered on robot
00172     ball_x = cos( bearing ) * distance_ball_cam;
00173     ball_y = sin( bearing ) * distance_ball_cam;
00174
00175     // applyKalmanFilter();
00176
00177     distance_ball_motor = sqrt( ball_x * ball_x  +  ball_y * ball_y );
00178   }
00179 }
00180
00181
00182 bool
00183 OmniRelative::is_pos_valid() const
00184 {
00185   return mirror_model->isValidPoint( image_x, image_y );
00186 }
00187
00188
00189 void
00190 OmniRelative::calc_unfiltered()
00191 {
00192
00193   fawkes::polar_coord_2d_t rel_pos = mirror_model->getWorldPointRelative( image_x, image_y );
00194
00195   distance_ball_cam = rel_pos.r;
00196   bearing           = rel_pos.phi;
00197
00198   // cannot calculate yet
00199   slope = 0;
00200
00201   // assumes camera to be centered on robot
00202   ball_x = cos( bearing ) * distance_ball_cam;
00203   ball_y = sin( bearing ) * distance_ball_cam;
00204
00205   distance_ball_motor = sqrt( ball_x * ball_x  +  ball_y * ball_y );
00206 }
00207
00208
00209 /*
00210 void
00211 OmniRelative::applyKalmanFilter()
00212 {
00213 
00214   if (last_available) {
00215     avg_x_sum += ball_x - last_x;
00216     avg_y_sum += ball_y - last_y;
00217 
00218     ++avg_x_num;
00219     ++avg_y_num;
00220 
00221     avg_x = avg_x_sum / avg_x_num;
00222     avg_y = avg_y_sum / avg_y_num;
00223 
00224     rx = (ball_x - avg_x) * distance_ball_cam;
00225     ry = (ball_y - avg_y) * distance_ball_cam;
00226 
00227     kalman_filter->setMeasurementCovariance( rx * rx, ry * ry );
00228   } else {
00229     kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
00230   }
00231 
00232   last_x = ball_x;
00233   last_y = ball_y;
00234 
00235   kalman_filter->setMeasurementX( ball_x );
00236   kalman_filter->setMeasurementY( ball_y );
00237   kalman_filter->doCalculation();
00238 
00239   ball_x = kalman_filter->getStateX();
00240   ball_y = kalman_filter->getStateY();
00241 
00242   if ( isnan( ball_x ) || isinf( ball_x ) ||
00243        isnan( ball_y ) || isinf( ball_y ) ) {
00244     // Kalman is wedged, use unfiltered result and reset filter
00245     kalman_filter->reset();
00246     ball_x = last_x;
00247     ball_y = last_y;
00248   }
00249 
00250 }
00251 */