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 <models/mirror/bulb/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 
00036 /** @class MirrorCalibTool tools/firestation/mirror_calib.h
00037  * This class encapsulates the routines necessary for interactive mirror
00038  * calibration.
00039  */
00040
00041 float MirrorCalibTool::m_sample_dist[] = {0.5, 1.0, 1.5, 2.0, 2.5,
00042                                           3.0, 3.5, 4.0, 4.5, 5.0};
00043 float MirrorCalibTool::m_sample_ori[]  = {0.0, deg2rad(45.0),
00044                                           deg2rad(90.0), deg2rad(135.0),
00045                                           deg2rad(180.0),deg2rad(225.0),
00046                                           deg2rad(270.0), deg2rad(315.0)};
00047 
00048 /** Constructor. */
00049 MirrorCalibTool::MirrorCalibTool()
00050 {
00051 #ifdef HAVE_BULB_CREATOR
00052   m_bulb = 0;
00053   m_sampler = 0;
00054   m_generator = 0;
00055 #endif
00056 
00057   m_img_width = 0;
00058   m_img_height = 0;
00059
00060   m_num_dists = 10 /*num_dists*/;
00061   m_num_oris = 8 /*num_oris*/;
00062
00063   m_sample_step = 0;
00064   m_sample_dist_step = 0;
00065   m_sample_ori_step = 0;
00066
00067   m_calib_done = false;
00068 }
00069 
00070 /** Constructor.
00071  * @param width image width
00072  * @param height image height
00073  */
00074 MirrorCalibTool::MirrorCalibTool(unsigned int width, unsigned int height
00075                                  /*, unsigned int num_dists, unsigned int num_oris*/ )
00076 {
00077 #ifdef HAVE_BULB_CREATOR
00078   m_bulb = 0;
00079   m_sampler = 0;
00080   m_generator = 0;
00081
00082   m_img_width = width;
00083   m_img_height = height;
00084
00085   m_num_dists = 5/*num_dists*/;
00086   m_num_oris = 8/*num_oris*/;
00087
00088   m_sample_step = 0;
00089   m_sample_dist_step = 0;
00090   m_sample_ori_step = 0;
00091
00092   m_calib_done = false;
00093 #endif
00094 }
00095 
00096 /** Destructor. */
00097 MirrorCalibTool::~MirrorCalibTool()
00098 {
00099 #ifdef HAVE_BULB_CREATOR
00100   delete m_bulb;
00101   delete m_sampler;
00102   delete m_generator;
00103 #endif
00104 }
00105 
00106 /** Inititates the calibration process. */
00107 void
00108 MirrorCalibTool::start()
00109 {
00110 #ifdef HAVE_BULB_CREATOR
00111   m_sample_step = 0;
00112   m_sample_dist_step = 0;
00113   m_sample_ori_step = 0;
00114
00115   m_step_two = false;
00116
00117   m_sampler = new BulbSampler(m_img_width, m_img_height);
00118   m_next_sample_point = HomPoint(0.0, 0.0, 0.0);
00119
00120   cout << "Define center" << endl;
00121 #endif
00122 }
00123 
00124 /** Aborts the calibration process. */
00125 void
00126 MirrorCalibTool::abort()
00127 {
00128   m_sample_step = 0;
00129   m_sample_dist_step = 0;
00130   m_sample_ori_step = 0;
00131 }
00132 
00133 /** Do one step in the calibration process.
00134  * @param x the x-coordinate of the of the current calibration point 
00135  * @param y the y-coordinate of the of the current calibration point 
00136  */
00137 void
00138 MirrorCalibTool::step(unsigned int x, unsigned int y)
00139 {
00140 #ifdef HAVE_BULB_CREATOR
00141   if (m_sample_step == 0)
00142   {
00143     m_sampler->setCenter(x, y);
00144     m_center_x = x;
00145     m_center_y = y;
00146     cout << "Center set to (" << x << ", " << y << ")" << endl;
00147     m_sample_step++;
00148     m_sampler->calculateOmniOrientation(x, y-100);
00149   }
00150   else if (m_sample_step == 1)
00151   {
00152     if (m_sample_dist_step < m_num_dists)
00153     {
00154       float dist = m_sample_dist[m_sample_dist_step];
00155       m_next_sample_point = HomPoint(0.0, dist, 0.0);
00156       float phi = atan2f( float(x) - float(m_center_x),
00157                           float(m_center_y) - float(y) );
00158       cout << "phi: " << phi << endl;
00159       m_next_sample_point.rotate_z(phi);
00160       cout << "Next sample dist  : " << dist << endl;
00161       cout << "Next sample ori   : " << rad2deg(phi) << endl;
00162       cout << "Next sample point : " << m_next_sample_point.x()
00163            << ", " << m_next_sample_point.y() << endl;
00164
00165       m_sampler->setBallPosition(m_next_sample_point.x(), m_next_sample_point.y());
00166       try
00167       {
00168         m_sampler->consider(x, y, 0.0, 0.0, 0.0);
00169       }
00170       catch (Exception &e)
00171       {
00172         e.print_trace();
00173       }
00174
00175       ++m_sample_dist_step;
00176     }
00177     else
00178     {
00179       cout << "Generating bulb" << endl;
00180       m_generator = new BulbGenerator(m_sampler, this);
00181       m_generator->generate();
00182       m_calib_done = true;
00183       cout << "Calibration done" << endl;
00184       ++m_sample_step;
00185     }
00186   }
00187 #endif
00188 }
00189 
00190 /** Returns the world coordinates of the next calibration point in polar
00191  * coordinates.
00192  * @param dist distance to the next calibration point
00193  * @param ori relative orientation towards the next calibration point
00194  * @return true if the next calibration point can be determined
00195  */
00196 bool
00197 MirrorCalibTool::get_next(float* dist, float* ori)
00198 {
00199 #ifdef HAVE_BULB_CREATOR
00200   if (m_step_two)
00201   {
00202     *dist = m_sample_dist[m_sample_dist_step];
00203     *ori = rad2deg(m_sample_ori[m_sample_ori_step]);
00204
00205     return true;
00206   }
00207   else
00208   {
00209     *dist = 0;
00210     *ori = 0;
00211     return false;
00212   }
00213 #else
00214   *dist = 0;
00215   *ori = 0;
00216   return false;
00217 #endif
00218 }
00219 
00220 /** Loads a calibration file.
00221  * @param filename name of the file containing the calibration data
00222  */
00223 void
00224 MirrorCalibTool::load(const char* filename)
00225 {
00226 #ifdef HAVE_BULB_CREATOR
00227   m_bulb = new Bulb(filename);
00228 #endif
00229 }
00230 
00231 /** Saves calibration data to a file.
00232  * @param filename the nem of the file
00233  */
00234 void
00235 MirrorCalibTool::save(const char* filename)
00236 {
00237 #ifdef HAVE_BULB_CREATOR
00238   if (m_calib_done)
00239   {
00240     m_generator->getResult()->save(filename);
00241   }
00242   else
00243   {
00244     cout << "Can't save in the middle of the calibration" << endl;
00245   }
00246 #endif
00247 }
00248 
00249 /** Determines the world coordinates for the given image point.
00250  * @param x x-coordinate of the image point
00251  * @param y y-coordinate of the image point
00252  * @param dist_ret pointer to the relative distance of the world point
00253  * @param ori_ret pointer to the relative orientation of the world point
00254  */
00255 void
00256 MirrorCalibTool::eval(unsigned int x, unsigned int y, float* dist_ret, float* ori_ret)
00257 {
00258 #ifdef HAVE_BULB_CREATOR
00259   polar_coord_2d_t coord;
00260   coord = m_bulb->getWorldPointRelative(x, y);
00261
00262   *dist_ret = coord.r;
00263   *ori_ret = coord.phi;
00264 #endif
00265 }
00266 
00267 /** Setter routine for the image dimensions.
00268  * @param width image width
00269  * @param height image height
00270  */
00271 void
00272 MirrorCalibTool::set_img_dimensions(unsigned int width, unsigned int height)
00273 {
00274   m_img_width = width;
00275   m_img_height = height;
00276 }
00277 
00278 /** Setter routine for the distances used for calibration.
00279  * @param dists array of distances
00280  * @param num_dists number of distances
00281  */
00282 void
00283 MirrorCalibTool::set_dists(float dists[], unsigned int num_dists)
00284 {
00285   // TODO
00286 }
00287 
00288 /** Setter routine for the orientations used for calibration.
00289  * @param oris array of orientations
00290  * @param num_oris number of orientations
00291  */
00292 void
00293 MirrorCalibTool::set_oris(float oris[], unsigned int num_oris)
00294 {
00295   // TODO
00296 }
00297 
00298 /** Set total steps.
00299  * @param total_steps total number of steps
00300  */
00301 void
00302 MirrorCalibTool::setTotalSteps(unsigned int total_steps)
00303 {
00304 }
00305 
00306 /** Set progress.
00307  * @param progress current progress
00308  */
00309 void
00310 MirrorCalibTool::setProgress(unsigned int progress)
00311 {
00312 }
00313 
00314 /** Generation finished. */
00315 void
00316 MirrorCalibTool::finished()
00317 {
00318 }
00319