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 <cmath>
26 #include <iostream>
27 #include <fvmodels/relative_position/omni_relative.h>
28 
29 namespace firevision {
30 #if 0 /* just to make Emacs auto-indent happy */
31 }
32 #endif
33 
34 /** @class OmniRelative <fvmodels/relative_position/omni_relative.h>
35  * Omni vision relative position model.
36  */
37 
38 /** Constructor.
39  * @param mirror_model mirror model
40  */
42 {
43  this->mirror_model = mirror_model;
44 
45  avg_x = avg_y = avg_x_sum = avg_y_sum = 0.f;
46  avg_x_num = avg_y_num = 0;
47 
48  ball_x = ball_y = bearing = slope = distance_ball_motor = distance_ball_cam = 0.f;
49 
50  // cannot calculate yet
51  slope = 0;
52 
53  DEFAULT_X_VARIANCE = 36.f;
54  DEFAULT_Y_VARIANCE = 25.f;
55 
56  /*
57  // initial variance for ball pos kf
58  kalman_filter = new kalmanFilter2Dim();
59  CMatrix<float> initialStateVarianceBall(2,2);
60  initialStateVarianceBall[0][0] = DEFAULT_X_VARIANCE;
61  initialStateVarianceBall[1][0] = 0.00;
62  initialStateVarianceBall[0][1] = 0.00;
63  initialStateVarianceBall[1][1] = DEFAULT_Y_VARIANCE;
64  kalman_filter->setInitialStateCovariance( initialStateVarianceBall );
65 
66  // process noise for ball pos kf
67  kalman_filter->setProcessCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
68  kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
69  */
70 
71 }
72 
73 
74 float
76 {
77  return distance_ball_motor;
78 }
79 
80 
81 float
83 {
84  return bearing;
85 }
86 
87 
88 float
90 {
91  return slope;
92 }
93 
94 
95 float
97 {
98  return ball_y;
99 }
100 
101 
102 float
104 {
105  return ball_x;
106 }
107 
108 
109 void
110 OmniRelative::set_center(float x, float y)
111 {
112  image_x = (unsigned int)roundf(x);
113  image_y = (unsigned int)roundf(y);
114 }
115 
116 
117 void
119 {
120 }
121 
122 
123 void
125 {
126 }
127 
128 
129 /** Get radius.
130  * @return always 0
131  */
132 float
134 {
135  return 0;
136 }
137 
138 
139 void
140 OmniRelative::set_pan_tilt(float pan, float tilt)
141 {
142 }
143 
144 
145 void
146 OmniRelative::get_pan_tilt(float *pan, float *tilt) const
147 {
148 }
149 
150 
151 const char *
153 {
154  return "OmniRelative";
155 }
156 
157 
158 void
160 {
161  last_available = false;
162  //kalman_filter->reset();
163 }
164 
165 
166 void
168 {
169 
170  if ( mirror_model->isValidPoint( image_x, image_y ) ) {
171  fawkes::polar_coord_2d_t rel_pos = mirror_model->getWorldPointRelative( image_x, image_y );
172 
173  distance_ball_cam = rel_pos.r;
174  bearing = rel_pos.phi;
175 
176  // assumes camera to be centered on robot
177  ball_x = cos( bearing ) * distance_ball_cam;
178  ball_y = sin( bearing ) * distance_ball_cam;
179 
180  // applyKalmanFilter();
181 
182  distance_ball_motor = sqrt( ball_x * ball_x + ball_y * ball_y );
183  }
184 }
185 
186 
187 bool
189 {
190  return mirror_model->isValidPoint( image_x, image_y );
191 }
192 
193 
194 void
196 {
197 
198  fawkes::polar_coord_2d_t rel_pos = mirror_model->getWorldPointRelative( image_x, image_y );
199 
200  distance_ball_cam = rel_pos.r;
201  bearing = rel_pos.phi;
202 
203  // cannot calculate yet
204  slope = 0;
205 
206  // assumes camera to be centered on robot
207  ball_x = cos( bearing ) * distance_ball_cam;
208  ball_y = sin( bearing ) * distance_ball_cam;
209 
210  distance_ball_motor = sqrt( ball_x * ball_x + ball_y * ball_y );
211 }
212 
213 
214 /*
215 void
216 OmniRelative::applyKalmanFilter()
217 {
218 
219  if (last_available) {
220  avg_x_sum += ball_x - last_x;
221  avg_y_sum += ball_y - last_y;
222 
223  ++avg_x_num;
224  ++avg_y_num;
225 
226  avg_x = avg_x_sum / avg_x_num;
227  avg_y = avg_y_sum / avg_y_num;
228 
229  rx = (ball_x - avg_x) * distance_ball_cam;
230  ry = (ball_y - avg_y) * distance_ball_cam;
231 
232  kalman_filter->setMeasurementCovariance( rx * rx, ry * ry );
233  } else {
234  kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
235  }
236 
237  last_x = ball_x;
238  last_y = ball_y;
239 
240  kalman_filter->setMeasurementX( ball_x );
241  kalman_filter->setMeasurementY( ball_y );
242  kalman_filter->doCalculation();
243 
244  ball_x = kalman_filter->getStateX();
245  ball_y = kalman_filter->getStateY();
246 
247  if ( isnan( ball_x ) || isinf( ball_x ) ||
248  isnan( ball_y ) || isinf( ball_y ) ) {
249  // Kalman is wedged, use unfiltered result and reset filter
250  kalman_filter->reset();
251  ball_x = last_x;
252  ball_y = last_y;
253  }
254 
255 }
256 */
257 
258 } // end namespace firevision
virtual void reset()
Reset all data.
OmniRelative(MirrorModel *mirror_model)
Constructor.
virtual float get_x() const
Get relative X coordinate of object.
virtual float get_bearing() const
Get bearing (horizontal angle) to object.
virtual void calc_unfiltered()
Calculate data unfiltered.
virtual bool is_pos_valid() const
Check if position is valid.
virtual void set_radius(float r)
Set radius of a found circle.
Polar coordinates.
Definition: types.h:85
virtual bool isValidPoint(unsigned int image_x, unsigned int image_y) const =0
Check if the given point is valid.
virtual float get_distance() const
Get distance to object.
virtual float get_radius() const
Get radius.
Mirror model interface.
Definition: mirrormodel.h:34
virtual void calc()
Calculate position data.
virtual fawkes::polar_coord_2d_t getWorldPointRelative(unsigned int image_x, unsigned int image_y) const =0
Get relative coordinate based on image coordinates.
float r
distance
Definition: types.h:86
virtual float get_y() const
Get relative Y coordinate of object.
virtual const char * get_name() const
Get name of relative position model.
virtual float get_slope() const
Get slope (vertical angle) to object.
float phi
angle
Definition: types.h:87
Center in ROI.
Definition: types.h:39
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.
virtual void set_pan_tilt(float pan=0.0f, float tilt=0.0f)
Set camera pan and tilt.