00001
00002 /***************************************************************************
00003  *  boxrelative.cpp - Implementation of the relative box position model
00004  *
00005  *  Generated: 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 /** @class BoxRelative <models/relative_position/box_relative.h>
00036  * Relative (beer) box position model.
00037  * Model used in Bremen to get world champions :-)
00038  */
00039 
00040 /** Constructor.
00041  * @param image_width width of image in pixels
00042  * @param image_height height of image in pixels
00043  * @param camera_height height of camera in meters
00044  * @param camera_offset_x camera offset of the motor axis in x direction
00045  * @param camera_offset_y camera offset of the motor axis in y direction
00046  * @param camera_ori camera orientation compared to the robot
00047  * @param horizontal_angle horizontal viewing angle (in degree)
00048  * @param vertical_angle vertical viewing angle (in degree)
00049  */
00050 BoxRelative::BoxRelative(unsigned int image_width,
00051                          unsigned int image_height,
00052                          float camera_height,
00053                          float camera_offset_x, float camera_offset_y,
00054                          float camera_ori,
00055                          float horizontal_angle, float vertical_angle
00056                          )
00057 {
00058
00059   this->image_width        = image_width;
00060   this->image_height       = image_height;
00061   this->horizontal_angle   = deg2rad( horizontal_angle );
00062   this->vertical_angle     = deg2rad( vertical_angle   );
00063   this->camera_orientation = deg2rad( camera_ori       );
00064   this->camera_height      = camera_height;
00065   this->camera_offset_x    = camera_offset_x;
00066   this->camera_offset_y    = camera_offset_y;
00067
00068   center.x = center.y = 0.f;
00069   pan            = 0.0f;
00070   tilt           = 0.0f;
00071
00072   pan_rad_per_pixel  = this->horizontal_angle   / this->image_width;
00073   tilt_rad_per_pixel = this->vertical_angle     / this->image_height;
00074
00075   box_x = box_y = bearing = slope = distance_box_motor = distance_box_cam = 0.f;
00076
00077   DEFAULT_X_VARIANCE = 1500.f;
00078   DEFAULT_Y_VARIANCE = 1000.f;
00079
00080   /*
00081   var_proc_x = 1500;
00082   var_proc_y = 1000;
00083   var_meas_x = 1500;
00084   var_meas_y = 1000;
00085 
00086   // initial variance for box pos kf
00087   kalman_filter = new kalmanFilter2Dim();
00088   CMatrix<float> initialStateVarianceBox(2,2);
00089   initialStateVarianceBox[0][0] = DEFAULT_X_VARIANCE;
00090   initialStateVarianceBox[1][0] = 0.00;
00091   initialStateVarianceBox[0][1] = 0.00;
00092   initialStateVarianceBox[1][1] = DEFAULT_Y_VARIANCE;
00093   kalman_filter->setInitialStateCovariance( initialStateVarianceBox ); 
00094 
00095   // process noise for box pos kf
00096   kalman_filter->setProcessCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
00097   kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
00098   */
00099 }
00100
00101
00102 /* Get the distance to the box - NOT IMPLEMENTED!
00103  * Was not needed, matching with laser data.
00104  * @return 0
00105  */
00106 float
00107 BoxRelative::get_distance() const 
00108 {
00109   return distance_box_motor;
00110 }
00111
00112
00113 float
00114 BoxRelative::get_bearing(void) const
00115 {
00116   return bearing;
00117 }
00118
00119
00120 float
00121 BoxRelative::get_slope() const
00122 {
00123   return slope;
00124 }
00125
00126
00127 /* Get relative Y distance in local cartesian coordinate system - NOT IMPLEMENTED!
00128  * Was not needed, matching with laser data.
00129  * @return 0
00130  */
00131 float
00132 BoxRelative::get_y(void) const
00133 {
00134   return box_y;
00135 }
00136
00137
00138 /* Get relative X distance in local cartesian coordinate system - NOT IMPLEMENTED!
00139  * Was not needed, matching with laser data.
00140  * @return 0
00141  */
00142 float
00143 BoxRelative::get_x(void) const
00144 {
00145   return box_x;
00146 }
00147
00148 void
00149 BoxRelative::set_radius(float r)
00150 {
00151 }
00152
00153
00154 void
00155 BoxRelative::set_center(float x, float y)
00156 {
00157   center.x = x;
00158   center.y = y;
00159 }
00160
00161
00162 void
00163 BoxRelative::set_center(const center_in_roi_t& c)
00164 {
00165   center.x = c.x;
00166   center.y = c.y;
00167 }
00168
00169
00170 void
00171 BoxRelative::set_pan_tilt(float pan, float tilt)
00172 {
00173   this->pan  = pan;
00174   this->tilt = tilt;
00175 }
00176
00177
00178 void
00179 BoxRelative::get_pan_tilt(float *pan, float *tilt) const
00180 {
00181   *pan  = this->pan;
00182   *tilt = this->tilt;
00183 }
00184
00185
00186 const char *
00187 BoxRelative::get_name() const
00188 {
00189   return "BoxRelative";
00190 }
00191
00192 
00193 /** Set the horizontal viewing angle.
00194  * @param angle_deg horizontal viewing angle in degrees
00195  */
00196 void
00197 BoxRelative::set_horizontal_angle(float angle_deg)
00198 {
00199   horizontal_angle = deg2rad( angle_deg );
00200 }
00201
00202 
00203 /** Set the vertical viewing angle.
00204  * @param angle_deg vertical viewing angle in degrees
00205  */
00206 void
00207 BoxRelative::set_vertical_angle(float angle_deg)
00208 {
00209   vertical_angle = deg2rad( angle_deg );
00210 }
00211
00212
00213 void
00214 BoxRelative::reset()
00215 {
00216   last_available = false;
00217   // kalman_filter->reset();
00218 }
00219
00220 void
00221 BoxRelative::calc()
00222 {
00223
00224   /*
00225   char user_input = toupper( getkey() );
00226 
00227   if (user_input == 'P') {
00228     cout << "Enter new kalman process variance values (X Y):" << flush;
00229     cin >> var_proc_x >> var_proc_y;
00230   } else if (user_input == 'M') {
00231     cout << "Enter new kalman measurement variance values (X Y):" << flush;
00232     cin >> var_meas_x >> var_meas_y;
00233   } else if (user_input == 'R') {
00234     cout << "Reset" << endl;
00235     reset();
00236   }
00237   */
00238
00239
00240   calc_unfiltered();
00241   // applyKalmanFilter();
00242
00243 }
00244
00245
00246 bool
00247 BoxRelative::is_pos_valid() const
00248 {
00249   return true;
00250 }
00251
00252
00253 void
00254 BoxRelative::calc_unfiltered()
00255 {
00256   /* Pan to the right is positive. Therefore we add it,
00257      because bearing to the right shall be positive */
00258   bearing = ((center.x - image_width/2) * pan_rad_per_pixel + pan + camera_orientation);
00259
00260   // invert sign, because slope downward shall be negative
00261   slope = -((center.y - image_height / 2) * tilt_rad_per_pixel - tilt);
00262
00263   distance_box_cam = camera_height * tan(M_PI / 2 + slope);
00264   distance_box_motor = distance_box_cam - camera_offset_x;
00265
00266   /*
00267   cout << "pan:" << pan << "  tilt:" << tilt
00268        << "  bearing: " << bearing << "  slope:" << slope
00269        << "  dist->cam:" << distance_box_cam
00270        << "  dist->motor:" << distance_box_motor
00271        << endl;
00272   */
00273
00274   box_x = cos( bearing ) * distance_box_cam + camera_offset_x;
00275   box_y = sin( bearing ) * distance_box_cam + camera_offset_y;
00276 }
00277
00278
00279 /*
00280 void
00281 BoxRelative::applyKalmanFilter()
00282 {
00283 
00284   kalman_filter->setMeasurementCovariance( var_meas_x, var_meas_y );
00285   kalman_filter->setProcessCovariance( var_proc_x, var_proc_y );
00286 
00287   last_x = box_x;
00288   last_y = box_y;
00289 
00290   kalman_filter->setMeasurementX( box_x );
00291   kalman_filter->setMeasurementY( box_y );
00292   kalman_filter->doCalculation();
00293 
00294   box_x = kalman_filter->getStateX();
00295   box_y = kalman_filter->getStateY();
00296 
00297   if ( isnan( box_x ) || isinf( box_x ) ||
00298        isnan( box_y ) || isinf( box_y ) ) {
00299     // Kalman is wedged, use unfiltered result and reset filter
00300     kalman_filter->reset();
00301     box_x = last_x;
00302     box_y = last_y;
00303   }
00304 
00305 }
00306 */