Fawkes API  Fawkes Development Version
ball_trigo.cpp
1 
2 /****************************************************************************
3  * ball_trigo.cpp - Ball relpos for pan/tilt camera using basic trigonometry
4  *
5  * Created: Mon Mar 23 10:03:48 2009
6  * Copyright 2009 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22  */
23 
24 #include <fvmodels/relative_position/ball_trigo.h>
25 #include <utils/math/angle.h>
26 
27 #include <cmath>
28 
29 using namespace std;
30 using namespace fawkes;
31 
32 namespace firevision {
33 #if 0 /* just to make Emacs auto-indent happy */
34 }
35 #endif
36 
37 /** @class BallTrigoRelativePos <fvmodels/relative_position/ball_trigo.h>
38  * Relative ball position model for pan/tilt camera.
39  * This uses basic trigonometry to calculate the position of the ball given
40  * only the center of the ball in the image as variable parameters, and the
41  * camera parameters as static parameters.
42  * @author Tim Niemueller
43  */
44 
45 /** Constructor.
46  * @param image_width width of image in pixels
47  * @param image_height height of image in pixels
48  * @param camera_height height of camera in meters
49  * @param camera_offset_x camera offset of the motor axis in x direction
50  * @param camera_offset_y camera offset of the motor axis in y direction
51  * @param camera_base_pan camera base pan in rad
52  * @param camera_base_tilt camera base tilt in rad
53  * @param horizontal_angle horizontal viewing angle (in degree)
54  * @param vertical_angle vertical viewing angle (in degree)
55  * @param ball_circumference ball circumference
56  */
57 BallTrigoRelativePos::BallTrigoRelativePos(unsigned int image_width,
58  unsigned int image_height,
59  float camera_height,
60  float camera_offset_x,
61  float camera_offset_y,
62  float camera_base_pan,
63  float camera_base_tilt,
64  float horizontal_angle,
65  float vertical_angle,
66  float ball_circumference)
67 {
68  __image_width = image_width;
69  __image_width_2 = __image_width / 2;
70  __image_height = image_height;
71  __image_height_2 = __image_height / 2;
72  __ball_circumference = ball_circumference;
73  __camera_height = camera_height;
74  __camera_offset_x = camera_offset_x;
75  __camera_offset_y = camera_offset_y;
76  __camera_base_pan = camera_base_pan;
77  __camera_base_tilt = camera_base_tilt;
78  __horizontal_angle = deg2rad( horizontal_angle );
79  __vertical_angle = deg2rad( vertical_angle );
80 
81  __cirt_center.x = 0.0f;
82  __cirt_center.y = 0.0f;
83  __pan = 0.0f;
84  __tilt = 0.0f;
85 
86  __pan_rad_per_pixel = __horizontal_angle / (float)__image_width;
87  __tilt_rad_per_pixel = __vertical_angle / (float)__image_height;
88  __ball_radius = __ball_circumference / ( 2.0 * M_PI );
89 
90  __ball_x = __ball_y = __bearing = __slope = __distance = 0.f;
91 }
92 
93 
94 float
95 BallTrigoRelativePos::get_distance() const
96 {
97  return __distance;
98 }
99 
100 
101 float
102 BallTrigoRelativePos::get_bearing() const
103 {
104  return __bearing;
105 }
106 
107 
108 float
109 BallTrigoRelativePos::get_slope() const
110 {
111  return __slope;
112 }
113 
114 
115 float
116 BallTrigoRelativePos::get_y() const
117 {
118  return __ball_y;
119 }
120 
121 
122 float
123 BallTrigoRelativePos::get_x() const
124 {
125  return __ball_x;
126 }
127 
128 
129 void
130 BallTrigoRelativePos::set_center(float x, float y)
131 {
132  __cirt_center.x = x;
133  __cirt_center.y = y;
134 }
135 
136 
137 void
138 BallTrigoRelativePos::set_center(const center_in_roi_t& c)
139 {
140  __cirt_center.x = c.x;
141  __cirt_center.y = c.y;
142 }
143 
144 
145 void
146 BallTrigoRelativePos::set_radius(float r)
147 {
148 }
149 
150 
151 void
152 BallTrigoRelativePos::set_pan_tilt(float pan, float tilt)
153 {
154  __pan = pan;
155  __tilt = tilt;
156 }
157 
158 
159 void
160 BallTrigoRelativePos::get_pan_tilt(float *pan, float *tilt) const
161 {
162  *pan = __pan;
163  *tilt = __tilt;
164 }
165 
166 
167 const char *
168 BallTrigoRelativePos::get_name() const
169 {
170  return "BallTrigoRelativePos";
171 }
172 
173 
174 void
175 BallTrigoRelativePos::reset()
176 {
177 }
178 
179 void
180 BallTrigoRelativePos::calc()
181 {
182 #ifdef OLD_COORD_SYS
183  /* Bearing shall be clockwise positive. */
184  __bearing = (((__cirt_center.x - __image_width_2) * __pan_rad_per_pixel
185  + __pan + __camera_base_pan));
186 #else
187  /* Bearing shall be counter-clockwise positive. */
188  __bearing = - (((__cirt_center.x - __image_width_2) * __pan_rad_per_pixel
189  + __pan + __camera_base_pan));
190 #endif
191 
192  /* Slope shall be downward negative */
193  __slope = ((__image_height_2 - __cirt_center.y) * __tilt_rad_per_pixel
194  + __tilt + __camera_base_tilt);
195 
196  float alpha = M_PI_2 - __slope;
197 
198  float e = __camera_height - __ball_radius - __ball_radius * cos(alpha);
199  __distance = - (e * tan(alpha) + __ball_radius * sin(alpha));
200 
201  __ball_x = cos( __bearing ) * __distance + __camera_offset_x;
202  __ball_y = sin( __bearing ) * __distance + __camera_offset_y;
203 }
204 
205 
206 bool
207 BallTrigoRelativePos::is_pos_valid() const
208 {
209  return __distance > 0; //Distance is < 0 if the ROI is above the horizon
210 }
211 
212 } // end namespace firevision
Fawkes library namespace.
STL namespace.
float x
x in pixels
Definition: types.h:40
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
Center in ROI.
Definition: types.h:39
float y
y in pixels
Definition: types.h:41