Fawkes API Fawkes Development Version

omni_relative.cpp

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