mirror_calib.cpp

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 

Generated on Tue Feb 22 13:31:18 2011 for Fawkes API by  doxygen 1.4.7