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 
30 namespace 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 
70 float
72 {
73  return distance_ball_motor;
74 }
75 
76 float
78 {
79  return bearing;
80 }
81 
82 float
84 {
85  return slope;
86 }
87 
88 float
90 {
91  return ball_y;
92 }
93 
94 float
96 {
97  return ball_x;
98 }
99 
100 void
101 OmniRelative::set_center(float x, float y)
102 {
103  image_x = (unsigned int)roundf(x);
104  image_y = (unsigned int)roundf(y);
105 }
106 
107 void
109 {
110 }
111 
112 void
114 {
115 }
116 
117 /** Get radius.
118  * @return always 0
119  */
120 float
122 {
123  return 0;
124 }
125 
126 void
127 OmniRelative::set_pan_tilt(float pan, float tilt)
128 {
129 }
130 
131 void
132 OmniRelative::get_pan_tilt(float *pan, float *tilt) const
133 {
134 }
135 
136 const char *
138 {
139  return "OmniRelative";
140 }
141 
142 void
144 {
145  last_available = false;
146  //kalman_filter->reset();
147 }
148 
149 void
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 
168 bool
170 {
171  return mirror_model->isValidPoint(image_x, image_y);
172 }
173 
174 void
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 /*
193 void
194 OmniRelative::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