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