Fawkes API
Fawkes Development Version
|
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 <fvmodels/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 <fvmodels/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