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
29using namespace std;
30using namespace fawkes;
31
32namespace 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 */
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
90float
92{
93 return distance_;
94}
95
96float
98{
99 return bearing_;
100}
101
102float
104{
105 return slope_;
106}
107
108float
110{
111 return ball_y_;
112}
113
114float
116{
117 return ball_x_;
118}
119
120void
122{
123 cirt_center_.x = x;
124 cirt_center_.y = y;
125}
126
127void
129{
130 cirt_center_.x = c.x;
131 cirt_center_.y = c.y;
132}
133
134void
136{
137}
138
139void
141{
142 pan_ = pan;
143 tilt_ = tilt;
144}
145
146void
147BallTrigoRelativePos::get_pan_tilt(float *pan, float *tilt) const
148{
149 *pan = pan_;
150 *tilt = tilt_;
151}
152
153const char *
155{
156 return "BallTrigoRelativePos";
157}
158
159void
161{
162}
163
164void
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
187bool
189{
190 return distance_ > 0; //Distance is < 0 if the ROI is above the horizon
191}
192
193} // end namespace firevision
BallTrigoRelativePos(unsigned int image_width, unsigned int image_height, float camera_height, float camera_offset_x, float camera_offset_y, float camera_base_pan, float camera_base_tilt, float horizontal_angle, float vertical_angle, float ball_circumference)
Constructor.
Definition: ball_trigo.cpp:54
virtual void reset()
Reset all data.
Definition: ball_trigo.cpp:160
virtual void set_center(float x, float y)
Set center of a found circle.
Definition: ball_trigo.cpp:121
virtual const char * get_name() const
Get name of relative position model.
Definition: ball_trigo.cpp:154
virtual float get_x() const
Get relative X coordinate of object.
Definition: ball_trigo.cpp:115
virtual void set_pan_tilt(float pan=0.0f, float tilt=0.0f)
Set camera pan and tilt.
Definition: ball_trigo.cpp:140
virtual float get_distance() const
Get distance to object.
Definition: ball_trigo.cpp:91
virtual void set_radius(float r)
Set radius of a found circle.
Definition: ball_trigo.cpp:135
virtual float get_slope() const
Get slope (vertical angle) to object.
Definition: ball_trigo.cpp:103
virtual float get_bearing() const
Get bearing (horizontal angle) to object.
Definition: ball_trigo.cpp:97
virtual float get_y() const
Get relative Y coordinate of object.
Definition: ball_trigo.cpp:109
virtual void get_pan_tilt(float *pan, float *tilt) const
Get camera pan tilt.
Definition: ball_trigo.cpp:147
virtual void calc()
Calculate position data.
Definition: ball_trigo.cpp:165
virtual bool is_pos_valid() const
Check if position is valid.
Definition: ball_trigo.cpp:188
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
Center in ROI.
Definition: types.h:38
float y
y in pixels
Definition: types.h:40
float x
x in pixels
Definition: types.h:39