Fawkes API Fawkes Development Version
omni_relative.cpp
1
2/***************************************************************************
3 * omni_relative.cpp - Implementation of the relative ball model
4 * for the omni cam
5 *
6 * Created: Thu Mar 23 22:00:15 2006
7 * Copyright 2006 Tim Niemueller [www.niemueller.de]
8 *
9 ****************************************************************************/
10
11/* This program is free software; you can redistribute it and/or modify
12 * it under the terms of the GNU General Public License as published by
13 * the Free Software Foundation; either version 2 of the License, or
14 * (at your option) any later version. A runtime exception applies to
15 * this software (see LICENSE.GPL_WRE file mentioned below for details).
16 *
17 * This program is distributed in the hope that it will be useful,
18 * but WITHOUT ANY WARRANTY; without even the implied warranty of
19 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20 * GNU Library General Public License for more details.
21 *
22 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
23 */
24
25#include <fvmodels/relative_position/omni_relative.h>
26
27#include <cmath>
28#include <iostream>
29
30namespace firevision {
31
32/** @class OmniRelative <fvmodels/relative_position/omni_relative.h>
33 * Omni vision relative position model.
34 */
35
36/** Constructor.
37 * @param mirror_model mirror model
38 */
40{
41 this->mirror_model = mirror_model;
42
43 avg_x = avg_y = avg_x_sum = avg_y_sum = 0.f;
44 avg_x_num = avg_y_num = 0;
45
46 ball_x = ball_y = bearing = slope = distance_ball_motor = distance_ball_cam = 0.f;
47
48 // cannot calculate yet
49 slope = 0;
50
51 DEFAULT_X_VARIANCE = 36.f;
52 DEFAULT_Y_VARIANCE = 25.f;
53
54 /*
55 // initial variance for ball pos kf
56 kalman_filter = new kalmanFilter2Dim();
57 CMatrix<float> initialStateVarianceBall(2,2);
58 initialStateVarianceBall[0][0] = DEFAULT_X_VARIANCE;
59 initialStateVarianceBall[1][0] = 0.00;
60 initialStateVarianceBall[0][1] = 0.00;
61 initialStateVarianceBall[1][1] = DEFAULT_Y_VARIANCE;
62 kalman_filter->setInitialStateCovariance( initialStateVarianceBall );
63
64 // process noise for ball pos kf
65 kalman_filter->setProcessCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
66 kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
67 */
68}
69
70float
72{
73 return distance_ball_motor;
74}
75
76float
78{
79 return bearing;
80}
81
82float
84{
85 return slope;
86}
87
88float
90{
91 return ball_y;
92}
93
94float
96{
97 return ball_x;
98}
99
100void
102{
103 image_x = (unsigned int)roundf(x);
104 image_y = (unsigned int)roundf(y);
105}
106
107void
109{
110}
111
112void
114{
115}
116
117/** Get radius.
118 * @return always 0
119 */
120float
122{
123 return 0;
124}
125
126void
127OmniRelative::set_pan_tilt(float pan, float tilt)
128{
129}
130
131void
132OmniRelative::get_pan_tilt(float *pan, float *tilt) const
133{
134}
135
136const char *
138{
139 return "OmniRelative";
140}
141
142void
144{
145 last_available = false;
146 //kalman_filter->reset();
147}
148
149void
151{
152 if (mirror_model->isValidPoint(image_x, image_y)) {
153 fawkes::polar_coord_2d_t rel_pos = mirror_model->getWorldPointRelative(image_x, image_y);
154
155 distance_ball_cam = rel_pos.r;
156 bearing = rel_pos.phi;
157
158 // assumes camera to be centered on robot
159 ball_x = cos(bearing) * distance_ball_cam;
160 ball_y = sin(bearing) * distance_ball_cam;
161
162 // applyKalmanFilter();
163
164 distance_ball_motor = sqrt(ball_x * ball_x + ball_y * ball_y);
165 }
166}
167
168bool
170{
171 return mirror_model->isValidPoint(image_x, image_y);
172}
173
174void
176{
177 fawkes::polar_coord_2d_t rel_pos = mirror_model->getWorldPointRelative(image_x, image_y);
178
179 distance_ball_cam = rel_pos.r;
180 bearing = rel_pos.phi;
181
182 // cannot calculate yet
183 slope = 0;
184
185 // assumes camera to be centered on robot
186 ball_x = cos(bearing) * distance_ball_cam;
187 ball_y = sin(bearing) * distance_ball_cam;
188
189 distance_ball_motor = sqrt(ball_x * ball_x + ball_y * ball_y);
190}
191
192/*
193void
194OmniRelative::applyKalmanFilter()
195{
196
197 if (last_available) {
198 avg_x_sum += ball_x - last_x;
199 avg_y_sum += ball_y - last_y;
200
201 ++avg_x_num;
202 ++avg_y_num;
203
204 avg_x = avg_x_sum / avg_x_num;
205 avg_y = avg_y_sum / avg_y_num;
206
207 rx = (ball_x - avg_x) * distance_ball_cam;
208 ry = (ball_y - avg_y) * distance_ball_cam;
209
210 kalman_filter->setMeasurementCovariance( rx * rx, ry * ry );
211 } else {
212 kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
213 }
214
215 last_x = ball_x;
216 last_y = ball_y;
217
218 kalman_filter->setMeasurementX( ball_x );
219 kalman_filter->setMeasurementY( ball_y );
220 kalman_filter->doCalculation();
221
222 ball_x = kalman_filter->getStateX();
223 ball_y = kalman_filter->getStateY();
224
225 if ( isnan( ball_x ) || isinf( ball_x ) ||
226 isnan( ball_y ) || isinf( ball_y ) ) {
227 // Kalman is wedged, use unfiltered result and reset filter
228 kalman_filter->reset();
229 ball_x = last_x;
230 ball_y = last_y;
231 }
232
233}
234*/
235
236} // end namespace firevision
Mirror model interface.
Definition: mirrormodel.h:32
virtual bool isValidPoint(unsigned int image_x, unsigned int image_y) const =0
Check if the given point is valid.
virtual fawkes::polar_coord_2d_t getWorldPointRelative(unsigned int image_x, unsigned int image_y) const =0
Get relative coordinate based on image coordinates.
virtual bool is_pos_valid() const
Check if position is valid.
virtual void set_radius(float r)
Set radius of a found circle.
virtual float get_slope() const
Get slope (vertical angle) to object.
virtual float get_distance() const
Get distance to object.
virtual void set_pan_tilt(float pan=0.0f, float tilt=0.0f)
Set camera pan and tilt.
virtual void reset()
Reset all data.
virtual float get_bearing() const
Get bearing (horizontal angle) to object.
virtual void calc_unfiltered()
Calculate data unfiltered.
virtual void calc()
Calculate position data.
virtual float get_y() const
Get relative Y coordinate of object.
OmniRelative(MirrorModel *mirror_model)
Constructor.
virtual const char * get_name() const
Get name of relative position model.
virtual float get_radius() const
Get radius.
virtual float get_x() const
Get relative X coordinate of object.
virtual void set_center(float x, float y)
Set center of a found circle.
virtual void get_pan_tilt(float *pan, float *tilt) const
Get camera pan tilt.
Polar coordinates.
Definition: types.h:96
float phi
angle
Definition: types.h:98
float r
distance
Definition: types.h:97
Center in ROI.
Definition: types.h:38