box_relative.cpp

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

Generated on 1 Mar 2011 for Fawkes API by  doxygen 1.6.1