Fawkes API  Fawkes Development Version
box_relative.cpp
1 
2 /***************************************************************************
3  * boxrelative.cpp - Implementation of the relative box position model
4  *
5  * Created: Fri Jun 03 22:56:22 2005
6  * Copyright 2005 Hu Yuxiao <Yuxiao.Hu@rwth-aachen.de>
7  * 2005-2006 Tim Niemueller [www.niemueller.de]
8  * 2005 Martin Heracles <Martin.Heracles@rwth-aachen.de>
9  *
10  ****************************************************************************/
11 
12 /* This program is free software; you can redistribute it and/or modify
13  * it under the terms of the GNU General Public License as published by
14  * the Free Software Foundation; either version 2 of the License, or
15  * (at your option) any later version. A runtime exception applies to
16  * this software (see LICENSE.GPL_WRE file mentioned below for details).
17  *
18  * This program is distributed in the hope that it will be useful,
19  * but WITHOUT ANY WARRANTY; without even the implied warranty of
20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21  * GNU Library General Public License for more details.
22  *
23  * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
24  */
25 
26 #include <cmath>
27 #include <fvmodels/relative_position/box_relative.h>
28 #include <utils/math/angle.h>
29 
30 #include <iostream>
31 
32 using namespace std;
33 using namespace fawkes;
34 
35 namespace firevision {
36 #if 0 /* just to make Emacs auto-indent happy */
37 }
38 #endif
39 
40 /** @class BoxRelative <fvmodels/relative_position/box_relative.h>
41  * Relative (beer) box position model.
42  * Model used in Bremen to get world champions :-)
43  */
44 
45 /** Constructor.
46  * @param image_width width of image in pixels
47  * @param image_height height of image in pixels
48  * @param camera_height height of camera in meters
49  * @param camera_offset_x camera offset of the motor axis in x direction
50  * @param camera_offset_y camera offset of the motor axis in y direction
51  * @param camera_ori camera orientation compared to the robot
52  * @param horizontal_angle horizontal viewing angle (in degree)
53  * @param vertical_angle vertical viewing angle (in degree)
54  */
55 BoxRelative::BoxRelative(unsigned int image_width,
56  unsigned int image_height,
57  float camera_height,
58  float camera_offset_x, float camera_offset_y,
59  float camera_ori,
60  float horizontal_angle, float vertical_angle
61  )
62 {
63 
64  this->image_width = image_width;
65  this->image_height = image_height;
66  this->horizontal_angle = deg2rad( horizontal_angle );
67  this->vertical_angle = deg2rad( vertical_angle );
68  this->camera_orientation = deg2rad( camera_ori );
69  this->camera_height = camera_height;
70  this->camera_offset_x = camera_offset_x;
71  this->camera_offset_y = camera_offset_y;
72 
73  center.x = center.y = 0.f;
74  pan = 0.0f;
75  tilt = 0.0f;
76 
77  pan_rad_per_pixel = this->horizontal_angle / this->image_width;
78  tilt_rad_per_pixel = this->vertical_angle / this->image_height;
79 
80  box_x = box_y = bearing = slope = distance_box_motor = distance_box_cam = 0.f;
81 
82  DEFAULT_X_VARIANCE = 1500.f;
83  DEFAULT_Y_VARIANCE = 1000.f;
84 
85  /*
86  var_proc_x = 1500;
87  var_proc_y = 1000;
88  var_meas_x = 1500;
89  var_meas_y = 1000;
90 
91  // initial variance for box pos kf
92  kalman_filter = new kalmanFilter2Dim();
93  CMatrix<float> initialStateVarianceBox(2,2);
94  initialStateVarianceBox[0][0] = DEFAULT_X_VARIANCE;
95  initialStateVarianceBox[1][0] = 0.00;
96  initialStateVarianceBox[0][1] = 0.00;
97  initialStateVarianceBox[1][1] = DEFAULT_Y_VARIANCE;
98  kalman_filter->setInitialStateCovariance( initialStateVarianceBox );
99 
100  // process noise for box pos kf
101  kalman_filter->setProcessCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
102  kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
103  */
104 }
105 
106 
107 /* Get the distance to the box - NOT IMPLEMENTED!
108  * Was not needed, matching with laser data.
109  * @return 0
110  */
111 float
112 BoxRelative::get_distance() const
113 {
114  return distance_box_motor;
115 }
116 
117 
118 float
119 BoxRelative::get_bearing(void) const
120 {
121  return bearing;
122 }
123 
124 
125 float
126 BoxRelative::get_slope() const
127 {
128  return slope;
129 }
130 
131 
132 /* Get relative Y distance in local cartesian coordinate system - NOT IMPLEMENTED!
133  * Was not needed, matching with laser data.
134  * @return 0
135  */
136 float
137 BoxRelative::get_y(void) const
138 {
139  return box_y;
140 }
141 
142 
143 /* Get relative X distance in local cartesian coordinate system - NOT IMPLEMENTED!
144  * Was not needed, matching with laser data.
145  * @return 0
146  */
147 float
148 BoxRelative::get_x(void) const
149 {
150  return box_x;
151 }
152 
153 void
154 BoxRelative::set_radius(float r)
155 {
156 }
157 
158 
159 void
160 BoxRelative::set_center(float x, float y)
161 {
162  center.x = x;
163  center.y = y;
164 }
165 
166 
167 void
168 BoxRelative::set_center(const center_in_roi_t& c)
169 {
170  center.x = c.x;
171  center.y = c.y;
172 }
173 
174 
175 void
176 BoxRelative::set_pan_tilt(float pan, float tilt)
177 {
178  this->pan = pan;
179  this->tilt = tilt;
180 }
181 
182 
183 void
184 BoxRelative::get_pan_tilt(float *pan, float *tilt) const
185 {
186  *pan = this->pan;
187  *tilt = this->tilt;
188 }
189 
190 
191 const char *
192 BoxRelative::get_name() const
193 {
194  return "BoxRelative";
195 }
196 
197 
198 /** Set the horizontal viewing angle.
199  * @param angle_deg horizontal viewing angle in degrees
200  */
201 void
202 BoxRelative::set_horizontal_angle(float angle_deg)
203 {
204  horizontal_angle = deg2rad( angle_deg );
205 }
206 
207 
208 /** Set the vertical viewing angle.
209  * @param angle_deg vertical viewing angle in degrees
210  */
211 void
212 BoxRelative::set_vertical_angle(float angle_deg)
213 {
214  vertical_angle = deg2rad( angle_deg );
215 }
216 
217 
218 void
219 BoxRelative::reset()
220 {
221  last_available = false;
222  // kalman_filter->reset();
223 }
224 
225 void
226 BoxRelative::calc()
227 {
228 
229  /*
230  char user_input = toupper( getkey() );
231 
232  if (user_input == 'P') {
233  cout << "Enter new kalman process variance values (X Y):" << flush;
234  cin >> var_proc_x >> var_proc_y;
235  } else if (user_input == 'M') {
236  cout << "Enter new kalman measurement variance values (X Y):" << flush;
237  cin >> var_meas_x >> var_meas_y;
238  } else if (user_input == 'R') {
239  cout << "Reset" << endl;
240  reset();
241  }
242  */
243 
244 
245  calc_unfiltered();
246  // applyKalmanFilter();
247 
248 }
249 
250 
251 bool
252 BoxRelative::is_pos_valid() const
253 {
254  return true;
255 }
256 
257 
258 void
259 BoxRelative::calc_unfiltered()
260 {
261  /* Pan to the right is positive. Therefore we add it,
262  because bearing to the right shall be positive */
263  bearing = ((center.x - image_width/2) * pan_rad_per_pixel + pan + camera_orientation);
264 
265  // invert sign, because slope downward shall be negative
266  slope = -((center.y - image_height / 2) * tilt_rad_per_pixel - tilt);
267 
268  distance_box_cam = camera_height * tan(M_PI / 2 + slope);
269  distance_box_motor = distance_box_cam - camera_offset_x;
270 
271  /*
272  cout << "pan:" << pan << " tilt:" << tilt
273  << " bearing: " << bearing << " slope:" << slope
274  << " dist->cam:" << distance_box_cam
275  << " dist->motor:" << distance_box_motor
276  << endl;
277  */
278 
279  box_x = cos( bearing ) * distance_box_cam + camera_offset_x;
280  box_y = sin( bearing ) * distance_box_cam + camera_offset_y;
281 }
282 
283 
284 /*
285 void
286 BoxRelative::applyKalmanFilter()
287 {
288 
289  kalman_filter->setMeasurementCovariance( var_meas_x, var_meas_y );
290  kalman_filter->setProcessCovariance( var_proc_x, var_proc_y );
291 
292  last_x = box_x;
293  last_y = box_y;
294 
295  kalman_filter->setMeasurementX( box_x );
296  kalman_filter->setMeasurementY( box_y );
297  kalman_filter->doCalculation();
298 
299  box_x = kalman_filter->getStateX();
300  box_y = kalman_filter->getStateY();
301 
302  if ( isnan( box_x ) || isinf( box_x ) ||
303  isnan( box_y ) || isinf( box_y ) ) {
304  // Kalman is wedged, use unfiltered result and reset filter
305  kalman_filter->reset();
306  box_x = last_x;
307  box_y = last_y;
308  }
309 
310 }
311 */
312 
313 } // end namespace firevision
Fawkes library namespace.
STL namespace.
float x
x in pixels
Definition: types.h:40
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
Center in ROI.
Definition: types.h:39
float y
y in pixels
Definition: types.h:41