box_relative.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include <cmath>
00027 #include <models/relative_position/box_relative.h>
00028 #include <utils/math/angle.h>
00029
00030 #include <iostream>
00031
00032 using namespace std;
00033 using namespace fawkes;
00034
00035 namespace firevision {
00036 #if 0
00037 }
00038 #endif
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055 BoxRelative::BoxRelative(unsigned int image_width,
00056 unsigned int image_height,
00057 float camera_height,
00058 float camera_offset_x, float camera_offset_y,
00059 float camera_ori,
00060 float horizontal_angle, float vertical_angle
00061 )
00062 {
00063
00064 this->image_width = image_width;
00065 this->image_height = image_height;
00066 this->horizontal_angle = deg2rad( horizontal_angle );
00067 this->vertical_angle = deg2rad( vertical_angle );
00068 this->camera_orientation = deg2rad( camera_ori );
00069 this->camera_height = camera_height;
00070 this->camera_offset_x = camera_offset_x;
00071 this->camera_offset_y = camera_offset_y;
00072
00073 center.x = center.y = 0.f;
00074 pan = 0.0f;
00075 tilt = 0.0f;
00076
00077 pan_rad_per_pixel = this->horizontal_angle / this->image_width;
00078 tilt_rad_per_pixel = this->vertical_angle / this->image_height;
00079
00080 box_x = box_y = bearing = slope = distance_box_motor = distance_box_cam = 0.f;
00081
00082 DEFAULT_X_VARIANCE = 1500.f;
00083 DEFAULT_Y_VARIANCE = 1000.f;
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104 }
00105
00106
00107
00108
00109
00110
00111 float
00112 BoxRelative::get_distance() const
00113 {
00114 return distance_box_motor;
00115 }
00116
00117
00118 float
00119 BoxRelative::get_bearing(void) const
00120 {
00121 return bearing;
00122 }
00123
00124
00125 float
00126 BoxRelative::get_slope() const
00127 {
00128 return slope;
00129 }
00130
00131
00132
00133
00134
00135
00136 float
00137 BoxRelative::get_y(void) const
00138 {
00139 return box_y;
00140 }
00141
00142
00143
00144
00145
00146
00147 float
00148 BoxRelative::get_x(void) const
00149 {
00150 return box_x;
00151 }
00152
00153 void
00154 BoxRelative::set_radius(float r)
00155 {
00156 }
00157
00158
00159 void
00160 BoxRelative::set_center(float x, float y)
00161 {
00162 center.x = x;
00163 center.y = y;
00164 }
00165
00166
00167 void
00168 BoxRelative::set_center(const center_in_roi_t& c)
00169 {
00170 center.x = c.x;
00171 center.y = c.y;
00172 }
00173
00174
00175 void
00176 BoxRelative::set_pan_tilt(float pan, float tilt)
00177 {
00178 this->pan = pan;
00179 this->tilt = tilt;
00180 }
00181
00182
00183 void
00184 BoxRelative::get_pan_tilt(float *pan, float *tilt) const
00185 {
00186 *pan = this->pan;
00187 *tilt = this->tilt;
00188 }
00189
00190
00191 const char *
00192 BoxRelative::get_name() const
00193 {
00194 return "BoxRelative";
00195 }
00196
00197
00198
00199
00200
00201 void
00202 BoxRelative::set_horizontal_angle(float angle_deg)
00203 {
00204 horizontal_angle = deg2rad( angle_deg );
00205 }
00206
00207
00208
00209
00210
00211 void
00212 BoxRelative::set_vertical_angle(float angle_deg)
00213 {
00214 vertical_angle = deg2rad( angle_deg );
00215 }
00216
00217
00218 void
00219 BoxRelative::reset()
00220 {
00221 last_available = false;
00222
00223 }
00224
00225 void
00226 BoxRelative::calc()
00227 {
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245 calc_unfiltered();
00246
00247
00248 }
00249
00250
00251 bool
00252 BoxRelative::is_pos_valid() const
00253 {
00254 return true;
00255 }
00256
00257
00258 void
00259 BoxRelative::calc_unfiltered()
00260 {
00261
00262
00263 bearing = ((center.x - image_width/2) * pan_rad_per_pixel + pan + camera_orientation);
00264
00265
00266 slope = -((center.y - image_height / 2) * tilt_rad_per_pixel - tilt);
00267
00268 distance_box_cam = camera_height * tan(M_PI / 2 + slope);
00269 distance_box_motor = distance_box_cam - camera_offset_x;
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279 box_x = cos( bearing ) * distance_box_cam + camera_offset_x;
00280 box_y = sin( bearing ) * distance_box_cam + camera_offset_y;
00281 }
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313 }