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 <fvmodels/relative_position/box_relative.h>
27 #include <utils/math/angle.h>
28 
29 #include <cmath>
30 #include <iostream>
31 
32 using namespace std;
33 using namespace fawkes;
34 
35 namespace firevision {
36 
37 /** @class BoxRelative <fvmodels/relative_position/box_relative.h>
38  * Relative (beer) box position model.
39  * Model used in Bremen to get world champions :-)
40  */
41 
42 /** Constructor.
43  * @param image_width width of image in pixels
44  * @param image_height height of image in pixels
45  * @param camera_height height of camera in meters
46  * @param camera_offset_x camera offset of the motor axis in x direction
47  * @param camera_offset_y camera offset of the motor axis in y direction
48  * @param camera_ori camera orientation compared to the robot
49  * @param horizontal_angle horizontal viewing angle (in degree)
50  * @param vertical_angle vertical viewing angle (in degree)
51  */
52 BoxRelative::BoxRelative(unsigned int image_width,
53  unsigned int image_height,
54  float camera_height,
55  float camera_offset_x,
56  float camera_offset_y,
57  float camera_ori,
58  float horizontal_angle,
59  float vertical_angle)
60 {
61  this->image_width = image_width;
62  this->image_height = image_height;
63  this->horizontal_angle = deg2rad(horizontal_angle);
64  this->vertical_angle = deg2rad(vertical_angle);
65  this->camera_orientation = deg2rad(camera_ori);
66  this->camera_height = camera_height;
67  this->camera_offset_x = camera_offset_x;
68  this->camera_offset_y = camera_offset_y;
69 
70  center.x = center.y = 0.f;
71  pan = 0.0f;
72  tilt = 0.0f;
73 
74  pan_rad_per_pixel = this->horizontal_angle / this->image_width;
75  tilt_rad_per_pixel = this->vertical_angle / this->image_height;
76 
77  box_x = box_y = bearing = slope = distance_box_motor = distance_box_cam = 0.f;
78 
79  DEFAULT_X_VARIANCE = 1500.f;
80  DEFAULT_Y_VARIANCE = 1000.f;
81 
82  /*
83  var_proc_x = 1500;
84  var_proc_y = 1000;
85  var_meas_x = 1500;
86  var_meas_y = 1000;
87 
88  // initial variance for box pos kf
89  kalman_filter = new kalmanFilter2Dim();
90  CMatrix<float> initialStateVarianceBox(2,2);
91  initialStateVarianceBox[0][0] = DEFAULT_X_VARIANCE;
92  initialStateVarianceBox[1][0] = 0.00;
93  initialStateVarianceBox[0][1] = 0.00;
94  initialStateVarianceBox[1][1] = DEFAULT_Y_VARIANCE;
95  kalman_filter->setInitialStateCovariance( initialStateVarianceBox );
96 
97  // process noise for box pos kf
98  kalman_filter->setProcessCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
99  kalman_filter->setMeasurementCovariance( DEFAULT_X_VARIANCE, DEFAULT_Y_VARIANCE );
100  */
101 }
102 
103 /* Get the distance to the box - NOT IMPLEMENTED!
104  * Was not needed, matching with laser data.
105  * @return 0
106  */
107 float
108 BoxRelative::get_distance() const
109 {
110  return distance_box_motor;
111 }
112 
113 float
114 BoxRelative::get_bearing(void) const
115 {
116  return bearing;
117 }
118 
119 float
120 BoxRelative::get_slope() const
121 {
122  return slope;
123 }
124 
125 /* Get relative Y distance in local cartesian coordinate system - NOT IMPLEMENTED!
126  * Was not needed, matching with laser data.
127  * @return 0
128  */
129 float
130 BoxRelative::get_y(void) const
131 {
132  return box_y;
133 }
134 
135 /* Get relative X distance in local cartesian coordinate system - NOT IMPLEMENTED!
136  * Was not needed, matching with laser data.
137  * @return 0
138  */
139 float
140 BoxRelative::get_x(void) const
141 {
142  return box_x;
143 }
144 
145 void
146 BoxRelative::set_radius(float r)
147 {
148 }
149 
150 void
151 BoxRelative::set_center(float x, float y)
152 {
153  center.x = x;
154  center.y = y;
155 }
156 
157 void
158 BoxRelative::set_center(const center_in_roi_t &c)
159 {
160  center.x = c.x;
161  center.y = c.y;
162 }
163 
164 void
165 BoxRelative::set_pan_tilt(float pan, float tilt)
166 {
167  this->pan = pan;
168  this->tilt = tilt;
169 }
170 
171 void
172 BoxRelative::get_pan_tilt(float *pan, float *tilt) const
173 {
174  *pan = this->pan;
175  *tilt = this->tilt;
176 }
177 
178 const char *
179 BoxRelative::get_name() const
180 {
181  return "BoxRelative";
182 }
183 
184 /** Set the horizontal viewing angle.
185  * @param angle_deg horizontal viewing angle in degrees
186  */
187 void
188 BoxRelative::set_horizontal_angle(float angle_deg)
189 {
190  horizontal_angle = deg2rad(angle_deg);
191 }
192 
193 /** Set the vertical viewing angle.
194  * @param angle_deg vertical viewing angle in degrees
195  */
196 void
197 BoxRelative::set_vertical_angle(float angle_deg)
198 {
199  vertical_angle = deg2rad(angle_deg);
200 }
201 
202 void
203 BoxRelative::reset()
204 {
205  last_available = false;
206  // kalman_filter->reset();
207 }
208 
209 void
210 BoxRelative::calc()
211 {
212  /*
213  char user_input = toupper( getkey() );
214 
215  if (user_input == 'P') {
216  cout << "Enter new kalman process variance values (X Y):" << flush;
217  cin >> var_proc_x >> var_proc_y;
218  } else if (user_input == 'M') {
219  cout << "Enter new kalman measurement variance values (X Y):" << flush;
220  cin >> var_meas_x >> var_meas_y;
221  } else if (user_input == 'R') {
222  cout << "Reset" << endl;
223  reset();
224  }
225  */
226 
227  calc_unfiltered();
228  // applyKalmanFilter();
229 }
230 
231 bool
232 BoxRelative::is_pos_valid() const
233 {
234  return true;
235 }
236 
237 void
238 BoxRelative::calc_unfiltered()
239 {
240  /* Pan to the right is positive. Therefore we add it,
241  because bearing to the right shall be positive */
242  bearing = ((center.x - image_width / 2) * pan_rad_per_pixel + pan + camera_orientation);
243 
244  // invert sign, because slope downward shall be negative
245  slope = -((center.y - image_height / 2) * tilt_rad_per_pixel - tilt);
246 
247  distance_box_cam = camera_height * tan(M_PI / 2 + slope);
248  distance_box_motor = distance_box_cam - camera_offset_x;
249 
250  /*
251  cout << "pan:" << pan << " tilt:" << tilt
252  << " bearing: " << bearing << " slope:" << slope
253  << " dist->cam:" << distance_box_cam
254  << " dist->motor:" << distance_box_motor
255  << endl;
256  */
257 
258  box_x = cos(bearing) * distance_box_cam + camera_offset_x;
259  box_y = sin(bearing) * distance_box_cam + camera_offset_y;
260 }
261 
262 /*
263 void
264 BoxRelative::applyKalmanFilter()
265 {
266 
267  kalman_filter->setMeasurementCovariance( var_meas_x, var_meas_y );
268  kalman_filter->setProcessCovariance( var_proc_x, var_proc_y );
269 
270  last_x = box_x;
271  last_y = box_y;
272 
273  kalman_filter->setMeasurementX( box_x );
274  kalman_filter->setMeasurementY( box_y );
275  kalman_filter->doCalculation();
276 
277  box_x = kalman_filter->getStateX();
278  box_y = kalman_filter->getStateY();
279 
280  if ( isnan( box_x ) || isinf( box_x ) ||
281  isnan( box_y ) || isinf( box_y ) ) {
282  // Kalman is wedged, use unfiltered result and reset filter
283  kalman_filter->reset();
284  box_x = last_x;
285  box_y = last_y;
286  }
287 
288 }
289 */
290 
291 } // end namespace firevision
fawkes
fawkes::deg2rad
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:40
firevision::center_in_roi_t::x
float x
x in pixels
Definition: types.h:50
firevision::center_in_roi_t::y
float y
y in pixels
Definition: types.h:51
firevision::center_in_roi_t
Center in ROI.
Definition: types.h:42