Fawkes API
Fawkes Development Version
|
00001 /*************************************************************************** 00002 * back_projection.cpp - Projective camera back projection model 00003 * 00004 * Created: Mon Apr 20 21:59:00 2009 00005 * Copyright 2009 Christof Rath <christof.rath@gmail.com> 00006 * 00007 ****************************************************************************/ 00008 00009 /* This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL file in the doc directory. 00020 */ 00021 00022 #include <fvmodels/relative_position/back_projection.h> 00023 00024 #include <cmath> 00025 00026 namespace firevision { 00027 #if 0 /* just to make Emacs auto-indent happy */ 00028 } 00029 #endif 00030 00031 /** @class BackProjectionPositionModel <fvmodels/relative_position/back_projection.h> 00032 * This model uses a ProjectiveCam to back project points in the image to 00033 * the world by the ground plane assumption. 00034 * 00035 * @author Christof Rath 00036 */ 00037 00038 /** Constructor 00039 * @param projective_cam the projective camera object 00040 * @param ball_circumference the circumference in case it's a ball 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; //tilt down-wards negative, cam pitch down-wards positive 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; //tilt down-wards negative, cam pitch down-wards positive 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 /* Bearing shall be clockwise positive. */ 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); //slope is down-wards negative 00139 } 00140 00141 void BackProjectionPositionModel::reset() 00142 { 00143 __pos_valid = false; 00144 } 00145 00146 } // end namespace firevision