Fawkes API
Fawkes Development Version
|
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