00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include <models/relative_position/back_projection.h>
00023
00024 #include <cmath>
00025
00026 namespace firevision {
00027 #if 0
00028 }
00029 #endif
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042 BackProjectionPositionModel::BackProjectionPositionModel(ProjectiveCam &projective_cam,
00043 float ball_circumference):
00044 __projective_cam(projective_cam),
00045 __ball_circumference(ball_circumference)
00046 {
00047 __ball_radius = __ball_circumference / ( 2.0 * M_PI );
00048
00049 __world_x = __world_y = __bearing = __slope = __distance = 0.f;
00050 __cirt_center.x = __cirt_center.y = 0.0f;
00051 __pos_valid = false;
00052 }
00053
00054
00055 void
00056 BackProjectionPositionModel::set_radius(float r)
00057 {
00058 __ball_radius = r;
00059 }
00060
00061
00062 void
00063 BackProjectionPositionModel::set_center(float x, float y)
00064 {
00065 __cirt_center.x = x;
00066 __cirt_center.y = y;
00067 }
00068
00069
00070 void
00071 BackProjectionPositionModel::set_cam_rotation(float pan, float tilt, float roll)
00072 {
00073 __cam_position.roll = roll;
00074 __cam_position.pitch = -tilt;
00075 __cam_position.yaw = pan;
00076 }
00077
00078 void BackProjectionPositionModel::get_pan_tilt(float *pan, float *tilt) const
00079 {
00080 float roll;
00081 get_cam_rotation(*pan, *tilt, roll);
00082 }
00083
00084 void BackProjectionPositionModel::get_cam_rotation(float &pan, float &tilt, float &roll) const
00085 {
00086 pan = __cam_position.yaw;
00087 tilt = -__cam_position.pitch;
00088 roll = __cam_position.roll;
00089 }
00090
00091 void BackProjectionPositionModel::set_cam_translation(float height, float rel_x, float rel_y)
00092 {
00093 __cam_position.x = rel_x;
00094 __cam_position.y = rel_y;
00095 __cam_position.z = height;
00096 }
00097
00098 void BackProjectionPositionModel::get_cam_translation(float & height, float & rel_x, float & rel_y) const
00099 {
00100 height = __cam_position.z;
00101 rel_x = __cam_position.x;
00102 rel_y = __cam_position.y;
00103 }
00104
00105
00106
00107 void BackProjectionPositionModel::calc()
00108 {
00109 __projective_cam.set_location(
00110 __cam_position.roll,
00111 __cam_position.pitch,
00112 __cam_position.yaw,
00113 __cam_position.z,
00114 __cam_position.x,
00115 __cam_position.y);
00116
00117 try {
00118 fawkes::cart_coord_2d_t wld_pt = __projective_cam.get_GPA_world_coord(__cirt_center);
00119
00120 __world_x = wld_pt.x;
00121 __world_y = wld_pt.y;
00122 __pos_valid = true;
00123 }
00124 catch (AboveHorizonException &e) {
00125 __world_x = 0.f;
00126 __world_y = 0.f;
00127 __pos_valid = false;
00128 }
00129
00130 #ifdef OLD_COORD_SYS
00131
00132 __bearing = -atan2f(_ball_y, __world_x);
00133 #else
00134 __bearing = atan2f(__world_y, __world_x);
00135 #endif
00136
00137 __distance = sqrt(__world_x * __world_x + __world_y * __world_y);
00138 __slope = -atan2f(__cam_position.z, __distance);
00139 }
00140
00141 void BackProjectionPositionModel::reset()
00142 {
00143 __pos_valid = false;
00144 }
00145
00146 }