Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * boxrelative.cpp - Implementation of the relative box position model 00004 * 00005 * Created: Fri Jun 03 22:56:22 2005 00006 * Copyright 2005 Hu Yuxiao <Yuxiao.Hu@rwth-aachen.de> 00007 * 2005-2006 Tim Niemueller [www.niemueller.de] 00008 * 2005 Martin Heracles <Martin.Heracles@rwth-aachen.de> 00009 * 00010 ****************************************************************************/ 00011 00012 /* This program is free software; you can redistribute it and/or modify 00013 * it under the terms of the GNU General Public License as published by 00014 * the Free Software Foundation; either version 2 of the License, or 00015 * (at your option) any later version. A runtime exception applies to 00016 * this software (see LICENSE.GPL_WRE file mentioned below for details). 00017 * 00018 * This program is distributed in the hope that it will be useful, 00019 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00020 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00021 * GNU Library General Public License for more details. 00022 * 00023 * Read the full text in the LICENSE.GPL_WRE file in the doc directory. 00024 */ 00025 00026 #include <cmath> 00027 #include <models/relative_position/box_relative.h> 00028 #include <utils/math/angle.h> 00029 00030 #include <iostream> 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 BoxRelative <models/relative_position/box_relative.h> 00041 * Relative (beer) box position model. 00042 * Model used in Bremen to get world champions :-) 00043 */ 00044 00045 /** Constructor. 00046 * @param image_width width of image in pixels 00047 * @param image_height height of image in pixels 00048 * @param camera_height height of camera in meters 00049 * @param camera_offset_x camera offset of the motor axis in x direction 00050 * @param camera_offset_y camera offset of the motor axis in y direction 00051 * @param camera_ori camera orientation compared to the robot 00052 * @param horizontal_angle horizontal viewing angle (in degree) 00053 * @param vertical_angle vertical viewing angle (in degree) 00054 */ 00055 BoxRelative::BoxRelative(unsigned int image_width, 00056 unsigned int image_height, 00057 float camera_height, 00058 float camera_offset_x, float camera_offset_y, 00059 float camera_ori, 00060 float horizontal_angle, float vertical_angle 00061 ) 00062 { 00063 00064 this->image_width = image_width; 00065 this->image_height = image_height; 00066 this->horizontal_angle = deg2rad( horizontal_angle ); 00067 this->vertical_angle = deg2rad( vertical_angle ); 00068 this->camera_orientation = deg2rad( camera_ori ); 00069 this->camera_height = camera_height; 00070 this->camera_offset_x = camera_offset_x; 00071 this->camera_offset_y = camera_offset_y; 00072 00073 center.x = center.y = 0.f; 00074 pan = 0.0f; 00075 tilt = 0.0f; 00076 00077 pan_rad_per_pixel = this->horizontal_angle / this->image_width; 00078 tilt_rad_per_pixel = this->vertical_angle / this->image_height; 00079 00080 box_x = box_y = bearing = slope = distance_box_motor = distance_box_cam = 0.f; 00081 00082 DEFAULT_X_VARIANCE = 1500.f; 00083 DEFAULT_Y_VARIANCE = 1000.f; 00084 00085 /* 00086 var_proc_x = 1500; 00087 var_proc_y = 1000; 00088 var_meas_x = 1500; 00089 var_meas_y = 1000; 00090 00091 // initial variance for box pos kf 00092 kalman_filter = new kalmanFilter2Dim(); 00093 CMatrix<float> initialStateVarianceBox(2,2); 00094 initialStateVarianceBox[0][0] = DEFAULT_X_VARIANCE; 00095 initialStateVarianceBox[1][0] = 0.00; 00096 initialStateVarianceBox[0][1] = 0.00; 00097 initialStateVarianceBox[1][1] = DEFAULT_Y_VARIANCE; 00098 kalman_filter->setInitialStateCovariance( initialStateVarianceBox ); 00099 00100 // process noise for box pos kf 00101 kalman_filter->setProcessCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE ); 00102 kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE ); 00103 */ 00104 } 00105 00106 00107 /* Get the distance to the box - NOT IMPLEMENTED! 00108 * Was not needed, matching with laser data. 00109 * @return 0 00110 */ 00111 float 00112 BoxRelative::get_distance() const 00113 { 00114 return distance_box_motor; 00115 } 00116 00117 00118 float 00119 BoxRelative::get_bearing(void) const 00120 { 00121 return bearing; 00122 } 00123 00124 00125 float 00126 BoxRelative::get_slope() const 00127 { 00128 return slope; 00129 } 00130 00131 00132 /* Get relative Y distance in local cartesian coordinate system - NOT IMPLEMENTED! 00133 * Was not needed, matching with laser data. 00134 * @return 0 00135 */ 00136 float 00137 BoxRelative::get_y(void) const 00138 { 00139 return box_y; 00140 } 00141 00142 00143 /* Get relative X distance in local cartesian coordinate system - NOT IMPLEMENTED! 00144 * Was not needed, matching with laser data. 00145 * @return 0 00146 */ 00147 float 00148 BoxRelative::get_x(void) const 00149 { 00150 return box_x; 00151 } 00152 00153 void 00154 BoxRelative::set_radius(float r) 00155 { 00156 } 00157 00158 00159 void 00160 BoxRelative::set_center(float x, float y) 00161 { 00162 center.x = x; 00163 center.y = y; 00164 } 00165 00166 00167 void 00168 BoxRelative::set_center(const center_in_roi_t& c) 00169 { 00170 center.x = c.x; 00171 center.y = c.y; 00172 } 00173 00174 00175 void 00176 BoxRelative::set_pan_tilt(float pan, float tilt) 00177 { 00178 this->pan = pan; 00179 this->tilt = tilt; 00180 } 00181 00182 00183 void 00184 BoxRelative::get_pan_tilt(float *pan, float *tilt) const 00185 { 00186 *pan = this->pan; 00187 *tilt = this->tilt; 00188 } 00189 00190 00191 const char * 00192 BoxRelative::get_name() const 00193 { 00194 return "BoxRelative"; 00195 } 00196 00197 00198 /** Set the horizontal viewing angle. 00199 * @param angle_deg horizontal viewing angle in degrees 00200 */ 00201 void 00202 BoxRelative::set_horizontal_angle(float angle_deg) 00203 { 00204 horizontal_angle = deg2rad( angle_deg ); 00205 } 00206 00207 00208 /** Set the vertical viewing angle. 00209 * @param angle_deg vertical viewing angle in degrees 00210 */ 00211 void 00212 BoxRelative::set_vertical_angle(float angle_deg) 00213 { 00214 vertical_angle = deg2rad( angle_deg ); 00215 } 00216 00217 00218 void 00219 BoxRelative::reset() 00220 { 00221 last_available = false; 00222 // kalman_filter->reset(); 00223 } 00224 00225 void 00226 BoxRelative::calc() 00227 { 00228 00229 /* 00230 char user_input = toupper( getkey() ); 00231 00232 if (user_input == 'P') { 00233 cout << "Enter new kalman process variance values (X Y):" << flush; 00234 cin >> var_proc_x >> var_proc_y; 00235 } else if (user_input == 'M') { 00236 cout << "Enter new kalman measurement variance values (X Y):" << flush; 00237 cin >> var_meas_x >> var_meas_y; 00238 } else if (user_input == 'R') { 00239 cout << "Reset" << endl; 00240 reset(); 00241 } 00242 */ 00243 00244 00245 calc_unfiltered(); 00246 // applyKalmanFilter(); 00247 00248 } 00249 00250 00251 bool 00252 BoxRelative::is_pos_valid() const 00253 { 00254 return true; 00255 } 00256 00257 00258 void 00259 BoxRelative::calc_unfiltered() 00260 { 00261 /* Pan to the right is positive. Therefore we add it, 00262 because bearing to the right shall be positive */ 00263 bearing = ((center.x - image_width/2) * pan_rad_per_pixel + pan + camera_orientation); 00264 00265 // invert sign, because slope downward shall be negative 00266 slope = -((center.y - image_height / 2) * tilt_rad_per_pixel - tilt); 00267 00268 distance_box_cam = camera_height * tan(M_PI / 2 + slope); 00269 distance_box_motor = distance_box_cam - camera_offset_x; 00270 00271 /* 00272 cout << "pan:" << pan << " tilt:" << tilt 00273 << " bearing: " << bearing << " slope:" << slope 00274 << " dist->cam:" << distance_box_cam 00275 << " dist->motor:" << distance_box_motor 00276 << endl; 00277 */ 00278 00279 box_x = cos( bearing ) * distance_box_cam + camera_offset_x; 00280 box_y = sin( bearing ) * distance_box_cam + camera_offset_y; 00281 } 00282 00283 00284 /* 00285 void 00286 BoxRelative::applyKalmanFilter() 00287 { 00288 00289 kalman_filter->setMeasurementCovariance( var_meas_x, var_meas_y ); 00290 kalman_filter->setProcessCovariance( var_proc_x, var_proc_y ); 00291 00292 last_x = box_x; 00293 last_y = box_y; 00294 00295 kalman_filter->setMeasurementX( box_x ); 00296 kalman_filter->setMeasurementY( box_y ); 00297 kalman_filter->doCalculation(); 00298 00299 box_x = kalman_filter->getStateX(); 00300 box_y = kalman_filter->getStateY(); 00301 00302 if ( isnan( box_x ) || isinf( box_x ) || 00303 isnan( box_y ) || isinf( box_y ) ) { 00304 // Kalman is wedged, use unfiltered result and reset filter 00305 kalman_filter->reset(); 00306 box_x = last_x; 00307 box_y = last_y; 00308 } 00309 00310 } 00311 */ 00312 00313 } // end namespace firevision