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  */
39 OmniRelative::OmniRelative(MirrorModel *mirror_model)
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
77 OmniRelative::get_bearing(void) const
78 {
79  return bearing;
80 }
81 
82 float
84 {
85  return slope;
86 }
87 
88 float
89 OmniRelative::get_y(void) const
90 {
91  return ball_y;
92 }
93 
94 float
95 OmniRelative::get_x(void) const
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
firevision::OmniRelative::reset
virtual void reset()
Definition: omni_relative.cpp:148
fawkes::polar_coord_2d_t::r
float r
distance
Definition: types.h:96
firevision::OmniRelative::calc
virtual void calc()
Definition: omni_relative.cpp:155
fawkes::polar_coord_2d_t
Polar coordinates.
Definition: types.h:94
firevision::OmniRelative::OmniRelative
OmniRelative(MirrorModel *mirror_model)
Constructor.
Definition: omni_relative.cpp:44
firevision::OmniRelative::get_slope
virtual float get_slope() const
Definition: omni_relative.cpp:88
firevision::OmniRelative::get_pan_tilt
virtual void get_pan_tilt(float *pan, float *tilt) const
Definition: omni_relative.cpp:137
firevision::OmniRelative::set_center
virtual void set_center(float x, float y)
Definition: omni_relative.cpp:106
firevision::OmniRelative::get_y
virtual float get_y() const
Definition: omni_relative.cpp:94
firevision::OmniRelative::set_pan_tilt
virtual void set_pan_tilt(float pan=0.0f, float tilt=0.0f)
Definition: omni_relative.cpp:132
firevision::OmniRelative::get_bearing
virtual float get_bearing() const
Definition: omni_relative.cpp:82
firevision::OmniRelative::is_pos_valid
virtual bool is_pos_valid() const
Definition: omni_relative.cpp:174
firevision::OmniRelative::get_distance
virtual float get_distance() const
Definition: omni_relative.cpp:76
firevision::OmniRelative::get_name
virtual const char * get_name() const
Definition: omni_relative.cpp:142
firevision::MirrorModel::getWorldPointRelative
virtual fawkes::polar_coord_2d_t getWorldPointRelative(unsigned int image_x, unsigned int image_y) const =0
firevision::OmniRelative::get_radius
virtual float get_radius() const
Get radius.
Definition: omni_relative.cpp:126
firevision::center_in_roi_t
Center in ROI.
Definition: types.h:42
firevision::OmniRelative::calc_unfiltered
virtual void calc_unfiltered()
Definition: omni_relative.cpp:180
firevision::MirrorModel::isValidPoint
virtual bool isValidPoint(unsigned int image_x, unsigned int image_y) const =0
fawkes::polar_coord_2d_t::phi
float phi
angle
Definition: types.h:97
firevision::OmniRelative::set_radius
virtual void set_radius(float r)
Definition: omni_relative.cpp:118
firevision::OmniRelative::get_x
virtual float get_x() const
Definition: omni_relative.cpp:100