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 
34 /** @class BallTrigoRelativePos <fvmodels/relative_position/ball_trigo.h>
35  * Relative ball position model for pan/tilt camera.
36  * This uses basic trigonometry to calculate the position of the ball given
37  * only the center of the ball in the image as variable parameters, and the
38  * camera parameters as static parameters.
39  * @author Tim Niemueller
40  */
41 
42 /** Constructor.
43  * @param image_width width of image in pixels
44  * @param image_height height of image in pixels
45  * @param camera_height height of camera in meters
46  * @param camera_offset_x camera offset of the motor axis in x direction
47  * @param camera_offset_y camera offset of the motor axis in y direction
48  * @param camera_base_pan camera base pan in rad
49  * @param camera_base_tilt camera base tilt in rad
50  * @param horizontal_angle horizontal viewing angle (in degree)
51  * @param vertical_angle vertical viewing angle (in degree)
52  * @param ball_circumference ball circumference
53  */
54 BallTrigoRelativePos::BallTrigoRelativePos(unsigned int image_width,
55  unsigned int image_height,
56  float camera_height,
57  float camera_offset_x,
58  float camera_offset_y,
59  float camera_base_pan,
60  float camera_base_tilt,
61  float horizontal_angle,
62  float vertical_angle,
63  float ball_circumference)
64 {
65  image_width_ = image_width;
66  image_width_2_ = image_width_ / 2;
67  image_height_ = image_height;
68  image_height_2_ = image_height_ / 2;
69  ball_circumference_ = ball_circumference;
70  camera_height_ = camera_height;
71  camera_offset_x_ = camera_offset_x;
72  camera_offset_y_ = camera_offset_y;
73  camera_base_pan_ = camera_base_pan;
74  camera_base_tilt_ = camera_base_tilt;
75  horizontal_angle_ = deg2rad(horizontal_angle);
76  vertical_angle_ = deg2rad(vertical_angle);
77 
78  cirt_center_.x = 0.0f;
79  cirt_center_.y = 0.0f;
80  pan_ = 0.0f;
81  tilt_ = 0.0f;
82 
83  pan_rad_per_pixel_ = horizontal_angle_ / (float)image_width_;
84  tilt_rad_per_pixel_ = vertical_angle_ / (float)image_height_;
85  ball_radius_ = ball_circumference_ / (2.0 * M_PI);
86 
87  ball_x_ = ball_y_ = bearing_ = slope_ = distance_ = 0.f;
88 }
89 
90 float
91 BallTrigoRelativePos::get_distance() const
92 {
93  return distance_;
94 }
95 
96 float
97 BallTrigoRelativePos::get_bearing() const
98 {
99  return bearing_;
100 }
101 
102 float
103 BallTrigoRelativePos::get_slope() const
104 {
105  return slope_;
106 }
107 
108 float
109 BallTrigoRelativePos::get_y() const
110 {
111  return ball_y_;
112 }
113 
114 float
115 BallTrigoRelativePos::get_x() const
116 {
117  return ball_x_;
118 }
119 
120 void
121 BallTrigoRelativePos::set_center(float x, float y)
122 {
123  cirt_center_.x = x;
124  cirt_center_.y = y;
125 }
126 
127 void
128 BallTrigoRelativePos::set_center(const center_in_roi_t &c)
129 {
130  cirt_center_.x = c.x;
131  cirt_center_.y = c.y;
132 }
133 
134 void
135 BallTrigoRelativePos::set_radius(float r)
136 {
137 }
138 
139 void
140 BallTrigoRelativePos::set_pan_tilt(float pan, float tilt)
141 {
142  pan_ = pan;
143  tilt_ = tilt;
144 }
145 
146 void
147 BallTrigoRelativePos::get_pan_tilt(float *pan, float *tilt) const
148 {
149  *pan = pan_;
150  *tilt = tilt_;
151 }
152 
153 const char *
154 BallTrigoRelativePos::get_name() const
155 {
156  return "BallTrigoRelativePos";
157 }
158 
159 void
160 BallTrigoRelativePos::reset()
161 {
162 }
163 
164 void
165 BallTrigoRelativePos::calc()
166 {
167 #ifdef OLD_COORD_SYS
168  /* Bearing shall be clockwise positive. */
169  bearing_ = (((cirt_center_.x - image_width_2_) * pan_rad_per_pixel_ + pan_ + camera_base_pan_));
170 #else
171  /* Bearing shall be counter-clockwise positive. */
172  bearing_ = -(((cirt_center_.x - image_width_2_) * pan_rad_per_pixel_ + pan_ + camera_base_pan_));
173 #endif
174 
175  /* Slope shall be downward negative */
176  slope_ = ((image_height_2_ - cirt_center_.y) * tilt_rad_per_pixel_ + tilt_ + camera_base_tilt_);
177 
178  float alpha = M_PI_2 - slope_;
179 
180  float e = camera_height_ - ball_radius_ - ball_radius_ * cos(alpha);
181  distance_ = -(e * tan(alpha) + ball_radius_ * sin(alpha));
182 
183  ball_x_ = cos(bearing_) * distance_ + camera_offset_x_;
184  ball_y_ = sin(bearing_) * distance_ + camera_offset_y_;
185 }
186 
187 bool
188 BallTrigoRelativePos::is_pos_valid() const
189 {
190  return distance_ > 0; //Distance is < 0 if the ROI is above the horizon
191 }
192 
193 } // end namespace firevision
fawkes
Fawkes library namespace.
fawkes::deg2rad
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
firevision::center_in_roi_t::x
float x
x in pixels
Definition: types.h:39
firevision::center_in_roi_t::y
float y
y in pixels
Definition: types.h:40
firevision::center_in_roi_t
Center in ROI.
Definition: types.h:38