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 /** @class BallTrigoRelativePos <models/relative_position/ball_trigo.h>
00033  * Relative ball position model for pan/tilt camera.
00034  * This uses basic trigonometry to calculate the position of the ball given
00035  * only the center of the ball in the image as variable parameters, and the
00036  * camera parameters as static parameters.
00037  * @author Tim Niemueller
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_base_pan camera base pan in rad
00047  * @param camera_base_tilt camera base tilt in rad
00048  * @param horizontal_angle horizontal viewing angle (in degree)
00049  * @param vertical_angle vertical viewing angle (in degree)
00050  * @param ball_circumference ball circumference
00051  */
00052 BallTrigoRelativePos::BallTrigoRelativePos(unsigned int image_width,
00053                                            unsigned int image_height,
00054                                            float camera_height,
00055                                            float camera_offset_x,
00056                                            float camera_offset_y,
00057                                            float camera_base_pan,
00058                                            float camera_base_tilt,
00059                                            float horizontal_angle,
00060                                            float vertical_angle,
00061                                            float ball_circumference)
00062 {
00063   __image_width        = image_width;
00064   __image_width_2      = __image_width / 2;
00065   __image_height       = image_height;
00066   __image_height_2     = __image_height / 2;
00067   __ball_circumference = ball_circumference;
00068   __camera_height      = camera_height;
00069   __camera_offset_x    = camera_offset_x;
00070   __camera_offset_y    = camera_offset_y;
00071   __camera_base_pan    = camera_base_pan;
00072   __camera_base_tilt   = camera_base_tilt;
00073   __horizontal_angle   = deg2rad( horizontal_angle );
00074   __vertical_angle     = deg2rad( vertical_angle   );
00075
00076   __cirt_center.x      = 0.0f;
00077   __cirt_center.y      = 0.0f;
00078   __pan                = 0.0f;
00079   __tilt               = 0.0f;
00080
00081   __pan_rad_per_pixel  = __horizontal_angle / (float)__image_width;
00082   __tilt_rad_per_pixel = __vertical_angle   / (float)__image_height;
00083   __ball_radius        = __ball_circumference / ( 2.0 * M_PI );
00084
00085   __ball_x = __ball_y = __bearing = __slope = __distance = 0.f;
00086 }
00087
00088
00089 float
00090 BallTrigoRelativePos::get_distance() const
00091 {
00092   return __distance;
00093 }
00094
00095
00096 float
00097 BallTrigoRelativePos::get_bearing() const
00098 {
00099   return __bearing;
00100 }
00101
00102
00103 float
00104 BallTrigoRelativePos::get_slope() const
00105 {
00106   return __slope;
00107 }
00108
00109
00110 float
00111 BallTrigoRelativePos::get_y() const
00112 {
00113   return __ball_y;
00114 }
00115
00116
00117 float
00118 BallTrigoRelativePos::get_x() const
00119 {
00120   return __ball_x;
00121 }
00122
00123
00124 void
00125 BallTrigoRelativePos::set_center(float x, float y)
00126 {
00127   __cirt_center.x = x;
00128   __cirt_center.y = y;
00129 }
00130
00131
00132 void
00133 BallTrigoRelativePos::set_center(const center_in_roi_t& c)
00134 {
00135   __cirt_center.x = c.x;
00136   __cirt_center.y = c.y;
00137 }
00138
00139
00140 void
00141 BallTrigoRelativePos::set_radius(float r)
00142 {
00143 }
00144
00145
00146 void
00147 BallTrigoRelativePos::set_pan_tilt(float pan, float tilt)
00148 {
00149   __pan = pan;
00150   __tilt = tilt;
00151 }
00152
00153
00154 void
00155 BallTrigoRelativePos::get_pan_tilt(float *pan, float *tilt) const
00156 {
00157   *pan  = __pan;
00158   *tilt = __tilt;
00159 }
00160
00161
00162 const char *
00163 BallTrigoRelativePos::get_name() const
00164 {
00165   return "BallTrigoRelativePos";
00166 }
00167
00168
00169 void
00170 BallTrigoRelativePos::reset()
00171 {
00172 }
00173
00174 void
00175 BallTrigoRelativePos::calc()
00176 {
00177 #ifdef OLD_COORD_SYS
00178   /* Bearing shall be clockwise positive. */
00179   __bearing =   (((__cirt_center.x - __image_width_2) * __pan_rad_per_pixel
00180                   + __pan + __camera_base_pan));
00181 #else
00182   /* Bearing shall be counter-clockwise positive. */
00183   __bearing = - (((__cirt_center.x - __image_width_2) * __pan_rad_per_pixel
00184                   + __pan + __camera_base_pan));
00185 #endif
00186 
00187   /* Slope shall be downward negative */
00188   __slope   = ((__image_height_2 - __cirt_center.y) * __tilt_rad_per_pixel
00189                  + __tilt + __camera_base_tilt);
00190
00191   float alpha = M_PI_2 - __slope;
00192
00193   float e = __camera_height - __ball_radius - __ball_radius * cos(alpha);
00194   __distance = - (e * tan(alpha) + __ball_radius * sin(alpha));
00195
00196   __ball_x = cos( __bearing ) * __distance + __camera_offset_x;
00197   __ball_y = sin( __bearing ) * __distance + __camera_offset_y;
00198 }
00199
00200
00201 bool
00202 BallTrigoRelativePos::is_pos_valid() const
00203 {
00204   return __distance > 0; //Distance is < 0 if the ROI is above the horizon
00205 }