Fawkes API Fawkes Development Version

ball_trigo.cpp

00001 
00002 /****************************************************************************
00003  *  ball_trigo.cpp - Ball relpos for pan/tilt camera using basic trigonometry
00004  *
00005  *  Created: Mon Mar 23 10:03:48 2009
00006  *  Copyright  2009  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version. A runtime exception applies to
00014  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00022  */
00023 
00024 #include <models/relative_position/ball_trigo.h>
00025 #include <utils/math/angle.h>
00026 
00027 #include <cmath>
00028 
00029 using namespace std;
00030 using namespace fawkes;
00031 
00032 namespace firevision {
00033 #if 0 /* just to make Emacs auto-indent happy */
00034 }
00035 #endif
00036 
00037 /** @class BallTrigoRelativePos <models/relative_position/ball_trigo.h>
00038  * Relative ball position model for pan/tilt camera.
00039  * This uses basic trigonometry to calculate the position of the ball given
00040  * only the center of the ball in the image as variable parameters, and the
00041  * camera parameters as static parameters.
00042  * @author Tim Niemueller
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_base_pan camera base pan in rad
00052  * @param camera_base_tilt camera base tilt in rad
00053  * @param horizontal_angle horizontal viewing angle (in degree)
00054  * @param vertical_angle vertical viewing angle (in degree)
00055  * @param ball_circumference ball circumference
00056  */
00057 BallTrigoRelativePos::BallTrigoRelativePos(unsigned int image_width,
00058                                            unsigned int image_height,
00059                                            float camera_height,
00060                                            float camera_offset_x,
00061                                            float camera_offset_y,
00062                                            float camera_base_pan,
00063                                            float camera_base_tilt,
00064                                            float horizontal_angle,
00065                                            float vertical_angle,
00066                                            float ball_circumference)
00067 {
00068   __image_width        = image_width;
00069   __image_width_2      = __image_width / 2;
00070   __image_height       = image_height;
00071   __image_height_2     = __image_height / 2;
00072   __ball_circumference = ball_circumference;
00073   __camera_height      = camera_height;
00074   __camera_offset_x    = camera_offset_x;
00075   __camera_offset_y    = camera_offset_y;
00076   __camera_base_pan    = camera_base_pan;
00077   __camera_base_tilt   = camera_base_tilt;
00078   __horizontal_angle   = deg2rad( horizontal_angle );
00079   __vertical_angle     = deg2rad( vertical_angle   );
00080 
00081   __cirt_center.x      = 0.0f;
00082   __cirt_center.y      = 0.0f;
00083   __pan                = 0.0f;
00084   __tilt               = 0.0f;
00085 
00086   __pan_rad_per_pixel  = __horizontal_angle / (float)__image_width;
00087   __tilt_rad_per_pixel = __vertical_angle   / (float)__image_height;
00088   __ball_radius        = __ball_circumference / ( 2.0 * M_PI );
00089 
00090   __ball_x = __ball_y = __bearing = __slope = __distance = 0.f;
00091 }
00092 
00093 
00094 float
00095 BallTrigoRelativePos::get_distance() const
00096 {
00097   return __distance;
00098 }
00099 
00100 
00101 float
00102 BallTrigoRelativePos::get_bearing() const
00103 {
00104   return __bearing;
00105 }
00106 
00107 
00108 float
00109 BallTrigoRelativePos::get_slope() const
00110 {
00111   return __slope;
00112 }
00113 
00114 
00115 float
00116 BallTrigoRelativePos::get_y() const
00117 {
00118   return __ball_y;
00119 }
00120 
00121 
00122 float
00123 BallTrigoRelativePos::get_x() const
00124 {
00125   return __ball_x;
00126 }
00127 
00128 
00129 void
00130 BallTrigoRelativePos::set_center(float x, float y)
00131 {
00132   __cirt_center.x = x;
00133   __cirt_center.y = y;
00134 }
00135 
00136 
00137 void
00138 BallTrigoRelativePos::set_center(const center_in_roi_t& c)
00139 {
00140   __cirt_center.x = c.x;
00141   __cirt_center.y = c.y;
00142 }
00143 
00144 
00145 void
00146 BallTrigoRelativePos::set_radius(float r)
00147 {
00148 }
00149 
00150 
00151 void
00152 BallTrigoRelativePos::set_pan_tilt(float pan, float tilt)
00153 {
00154   __pan = pan;
00155   __tilt = tilt;
00156 }
00157 
00158 
00159 void
00160 BallTrigoRelativePos::get_pan_tilt(float *pan, float *tilt) const
00161 {
00162   *pan  = __pan;
00163   *tilt = __tilt;
00164 }
00165 
00166 
00167 const char *
00168 BallTrigoRelativePos::get_name() const
00169 {
00170   return "BallTrigoRelativePos";
00171 }
00172 
00173 
00174 void
00175 BallTrigoRelativePos::reset()
00176 {
00177 }
00178 
00179 void
00180 BallTrigoRelativePos::calc()
00181 {
00182 #ifdef OLD_COORD_SYS
00183   /* Bearing shall be clockwise positive. */
00184   __bearing =   (((__cirt_center.x - __image_width_2) * __pan_rad_per_pixel
00185                   + __pan + __camera_base_pan));
00186 #else
00187   /* Bearing shall be counter-clockwise positive. */
00188   __bearing = - (((__cirt_center.x - __image_width_2) * __pan_rad_per_pixel
00189                   + __pan + __camera_base_pan));
00190 #endif
00191 
00192   /* Slope shall be downward negative */
00193   __slope   = ((__image_height_2 - __cirt_center.y) * __tilt_rad_per_pixel
00194                  + __tilt + __camera_base_tilt);
00195 
00196   float alpha = M_PI_2 - __slope;
00197 
00198   float e = __camera_height - __ball_radius - __ball_radius * cos(alpha);
00199   __distance = - (e * tan(alpha) + __ball_radius * sin(alpha));
00200 
00201   __ball_x = cos( __bearing ) * __distance + __camera_offset_x;
00202   __ball_y = sin( __bearing ) * __distance + __camera_offset_y;
00203 }
00204 
00205 
00206 bool
00207 BallTrigoRelativePos::is_pos_valid() const
00208 {
00209   return __distance > 0; //Distance is < 0 if the ROI is above the horizon
00210 }
00211 
00212 } // end namespace firevision
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends