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
32using namespace std;
33using namespace fawkes;
34
35namespace 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 */
52BoxRelative::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 */
107float
109{
110 return distance_box_motor;
111}
112
113float
115{
116 return bearing;
117}
118
119float
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 */
129float
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 */
139float
141{
142 return box_x;
143}
144
145void
147{
148}
149
150void
151BoxRelative::set_center(float x, float y)
152{
153 center.x = x;
154 center.y = y;
155}
156
157void
159{
160 center.x = c.x;
161 center.y = c.y;
162}
163
164void
165BoxRelative::set_pan_tilt(float pan, float tilt)
166{
167 this->pan = pan;
168 this->tilt = tilt;
169}
170
171void
172BoxRelative::get_pan_tilt(float *pan, float *tilt) const
173{
174 *pan = this->pan;
175 *tilt = this->tilt;
176}
177
178const char *
180{
181 return "BoxRelative";
182}
183
184/** Set the horizontal viewing angle.
185 * @param angle_deg horizontal viewing angle in degrees
186 */
187void
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 */
196void
198{
199 vertical_angle = deg2rad(angle_deg);
200}
201
202void
204{
205 last_available = false;
206 // kalman_filter->reset();
207}
208
209void
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
228 // applyKalmanFilter();
229}
230
231bool
233{
234 return true;
235}
236
237void
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/*
263void
264BoxRelative::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
virtual float get_y() const
Get relative Y coordinate of object.
virtual void set_center(float x, float y)
Set center of a found circle.
virtual void set_horizontal_angle(float angle_deg)
Set the horizontal viewing angle.
virtual const char * get_name() const
Get name of relative position model.
virtual void set_radius(float r)
Set radius of a found circle.
virtual void calc()
Calculate position data.
virtual bool is_pos_valid() const
Check if position is valid.
virtual void reset()
Reset all data.
virtual void set_vertical_angle(float angle_deg)
Set the vertical viewing angle.
BoxRelative(unsigned int image_width, unsigned int image_height, float camera_height, float camera_offset_x, float camera_offset_y, float camera_ori, float horizontal_angle, float vertical_angle)
Constructor.
virtual float get_x() const
Get relative X coordinate of object.
virtual float get_distance() const
Get distance to object.
virtual float get_slope() const
Get slope (vertical angle) to object.
virtual void calc_unfiltered()
Calculate data unfiltered.
virtual void set_pan_tilt(float pan=0.0f, float tilt=0.0f)
Set camera pan and tilt.
virtual float get_bearing() const
Get bearing (horizontal angle) to object.
virtual void get_pan_tilt(float *pan, float *tilt) const
Get camera pan tilt.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
Center in ROI.
Definition: types.h:38
float y
y in pixels
Definition: types.h:40
float x
x in pixels
Definition: types.h:39