Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * mirror_calib.cpp - Mirror calibration tool 00004 * 00005 * Created: Fri Dec 07 18:35:40 2007 00006 * Copyright 2007 Daniel Beck 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include <tools/firestation/mirror_calib.h> 00024 #ifdef HAVE_BULB_CREATOR 00025 #include <models/mirror/bulb.h> 00026 #include <bulb_calib/bulb_sampler.h> 00027 #endif 00028 #include <core/exception.h> 00029 #include <utils/math/angle.h> 00030 00031 #include <iostream> 00032 00033 using namespace std; 00034 using namespace fawkes; 00035 #ifdef HAVE_BULB_CREATOR 00036 using namespace firevision; 00037 #endif 00038 00039 /** @class MirrorCalibTool "mirror_calib.h" 00040 * This class encapsulates the routines necessary for interactive mirror 00041 * calibration. 00042 */ 00043 00044 float MirrorCalibTool::m_sample_dist[] = {0.5, 1.0, 1.5, 2.0, 2.5, 00045 3.0, 3.5, 4.0}; 00046 float MirrorCalibTool::m_sample_ori[] = {0.0, deg2rad(45.0), 00047 deg2rad(90.0), deg2rad(135.0), 00048 deg2rad(180.0),deg2rad(225.0), 00049 deg2rad(270.0), deg2rad(315.0)}; 00050 00051 /** Constructor. */ 00052 MirrorCalibTool::MirrorCalibTool() 00053 { 00054 #ifdef HAVE_BULB_CREATOR 00055 m_bulb = 0; 00056 m_sampler = 0; 00057 m_generator = 0; 00058 #endif 00059 00060 m_img_width = 0; 00061 m_img_height = 0; 00062 00063 m_num_dists = 8 /*num_dists*/; 00064 m_num_oris = 8 /*num_oris*/; 00065 00066 m_sample_step = 0; 00067 m_sample_dist_step = 0; 00068 m_sample_ori_step = 0; 00069 00070 m_calib_done = false; 00071 } 00072 00073 /** Constructor. 00074 * @param width image width 00075 * @param height image height 00076 */ 00077 MirrorCalibTool::MirrorCalibTool(unsigned int width, unsigned int height 00078 /*, unsigned int num_dists, unsigned int num_oris*/ ) 00079 { 00080 #ifdef HAVE_BULB_CREATOR 00081 m_bulb = 0; 00082 m_sampler = 0; 00083 m_generator = 0; 00084 00085 m_img_width = width; 00086 m_img_height = height; 00087 00088 m_num_dists = 5/*num_dists*/; 00089 m_num_oris = 8/*num_oris*/; 00090 00091 m_sample_step = 0; 00092 m_sample_dist_step = 0; 00093 m_sample_ori_step = 0; 00094 00095 m_calib_done = false; 00096 #endif 00097 } 00098 00099 /** Destructor. */ 00100 MirrorCalibTool::~MirrorCalibTool() 00101 { 00102 #ifdef HAVE_BULB_CREATOR 00103 delete m_bulb; 00104 delete m_sampler; 00105 delete m_generator; 00106 #endif 00107 } 00108 00109 /** Inititates the calibration process. */ 00110 void 00111 MirrorCalibTool::start() 00112 { 00113 #ifdef HAVE_BULB_CREATOR 00114 m_sample_step = 0; 00115 m_sample_dist_step = 0; 00116 m_sample_ori_step = 0; 00117 00118 m_step_two = false; 00119 00120 m_sampler = new BulbSampler(m_img_width, m_img_height); 00121 m_next_sample_point = HomPoint(0.0, 0.0, 0.0); 00122 00123 cout << "Define center" << endl; 00124 #endif 00125 } 00126 00127 /** Aborts the calibration process. */ 00128 void 00129 MirrorCalibTool::abort() 00130 { 00131 m_sample_step = 0; 00132 m_sample_dist_step = 0; 00133 m_sample_ori_step = 0; 00134 } 00135 00136 /** Do one step in the calibration process. 00137 * @param x the x-coordinate of the of the current calibration point 00138 * @param y the y-coordinate of the of the current calibration point 00139 */ 00140 void 00141 MirrorCalibTool::step(unsigned int x, unsigned int y) 00142 { 00143 #ifdef HAVE_BULB_CREATOR 00144 if (m_sample_step == 0) 00145 { 00146 m_sampler->setCenter(x, y); 00147 m_center_x = x; 00148 m_center_y = y; 00149 cout << "Center set to (" << x << ", " << y << ")" << endl; 00150 m_sample_step++; 00151 m_sampler->calculateOmniOrientation(x, y-100); 00152 } 00153 else if (m_sample_step == 1) 00154 { 00155 if (m_sample_dist_step < m_num_dists) 00156 { 00157 float dist = m_sample_dist[m_sample_dist_step]; 00158 m_next_sample_point = HomPoint(0.0, dist, 0.0); 00159 float phi = atan2f( float(x) - float(m_center_x), 00160 float(m_center_y) - float(y) ); 00161 cout << "phi: " << phi << endl; 00162 m_next_sample_point.rotate_z(phi); 00163 cout << "Next sample dist : " << dist << endl; 00164 cout << "Next sample ori : " << rad2deg(phi) << endl; 00165 cout << "Next sample point : " << m_next_sample_point.x() 00166 << ", " << m_next_sample_point.y() << endl; 00167 00168 m_sampler->setBallPosition(m_next_sample_point.x(), m_next_sample_point.y()); 00169 try 00170 { 00171 m_sampler->consider(x, y, 0.0, 0.0, 0.0); 00172 } 00173 catch (Exception &e) 00174 { 00175 e.print_trace(); 00176 } 00177 00178 ++m_sample_dist_step; 00179 } 00180 else 00181 { 00182 cout << "Generating bulb" << endl; 00183 m_generator = new BulbGenerator(m_sampler, this); 00184 m_generator->generate(); 00185 m_calib_done = true; 00186 cout << "Calibration done" << endl; 00187 ++m_sample_step; 00188 } 00189 } 00190 #endif 00191 } 00192 00193 /** Returns the world coordinates of the next calibration point in polar 00194 * coordinates. 00195 * @param dist distance to the next calibration point 00196 * @param ori relative orientation towards the next calibration point 00197 * @return true if the next calibration point can be determined 00198 */ 00199 bool 00200 MirrorCalibTool::get_next(float* dist, float* ori) 00201 { 00202 #ifdef HAVE_BULB_CREATOR 00203 if (m_step_two) 00204 { 00205 *dist = m_sample_dist[m_sample_dist_step]; 00206 *ori = rad2deg(m_sample_ori[m_sample_ori_step]); 00207 00208 return true; 00209 } 00210 else 00211 { 00212 *dist = 0; 00213 *ori = 0; 00214 return false; 00215 } 00216 #else 00217 *dist = 0; 00218 *ori = 0; 00219 return false; 00220 #endif 00221 } 00222 00223 /** Loads a calibration file. 00224 * @param filename name of the file containing the calibration data 00225 */ 00226 void 00227 MirrorCalibTool::load(const char* filename) 00228 { 00229 #ifdef HAVE_BULB_CREATOR 00230 m_bulb = new Bulb(filename); 00231 #endif 00232 } 00233 00234 /** Saves calibration data to a file. 00235 * @param filename the nem of the file 00236 */ 00237 void 00238 MirrorCalibTool::save(const char* filename) 00239 { 00240 #ifdef HAVE_BULB_CREATOR 00241 if (m_calib_done) 00242 { 00243 m_generator->getResult()->save(filename); 00244 } 00245 else 00246 { 00247 cout << "Can't save in the middle of the calibration" << endl; 00248 } 00249 #endif 00250 } 00251 00252 /** Determines the world coordinates for the given image point. 00253 * @param x x-coordinate of the image point 00254 * @param y y-coordinate of the image point 00255 * @param dist_ret pointer to the relative distance of the world point 00256 * @param ori_ret pointer to the relative orientation of the world point 00257 */ 00258 void 00259 MirrorCalibTool::eval(unsigned int x, unsigned int y, float* dist_ret, float* ori_ret) 00260 { 00261 #ifdef HAVE_BULB_CREATOR 00262 polar_coord_2d_t coord; 00263 coord = m_bulb->getWorldPointRelative(x, y); 00264 00265 *dist_ret = coord.r; 00266 *ori_ret = coord.phi; 00267 #endif 00268 } 00269 00270 /** Setter routine for the image dimensions. 00271 * @param width image width 00272 * @param height image height 00273 */ 00274 void 00275 MirrorCalibTool::set_img_dimensions(unsigned int width, unsigned int height) 00276 { 00277 m_img_width = width; 00278 m_img_height = height; 00279 } 00280 00281 /** Setter routine for the distances used for calibration. 00282 * @param dists array of distances 00283 * @param num_dists number of distances 00284 */ 00285 void 00286 MirrorCalibTool::set_dists(float dists[], unsigned int num_dists) 00287 { 00288 // TODO 00289 } 00290 00291 /** Setter routine for the orientations used for calibration. 00292 * @param oris array of orientations 00293 * @param num_oris number of orientations 00294 */ 00295 void 00296 MirrorCalibTool::set_oris(float oris[], unsigned int num_oris) 00297 { 00298 // TODO 00299 } 00300 00301 /** Set total steps. 00302 * @param total_steps total number of steps 00303 */ 00304 void 00305 MirrorCalibTool::setTotalSteps(unsigned int total_steps) 00306 { 00307 } 00308 00309 /** Set progress. 00310 * @param progress current progress 00311 */ 00312 void 00313 MirrorCalibTool::setProgress(unsigned int progress) 00314 { 00315 } 00316 00317 /** Generation finished. */ 00318 void 00319 MirrorCalibTool::finished() 00320 { 00321 } 00322