00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00040
00041
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
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 ;
00064 m_num_oris = 8 ;
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
00074
00075
00076
00077 MirrorCalibTool::MirrorCalibTool(unsigned int width, unsigned int height
00078 )
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;
00089 m_num_oris = 8;
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
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
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
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
00137
00138
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
00194
00195
00196
00197
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
00224
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
00235
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
00253
00254
00255
00256
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
00271
00272
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
00282
00283
00284
00285 void
00286 MirrorCalibTool::set_dists(float dists[], unsigned int num_dists)
00287 {
00288
00289 }
00290
00291
00292
00293
00294
00295 void
00296 MirrorCalibTool::set_oris(float oris[], unsigned int num_oris)
00297 {
00298
00299 }
00300
00301
00302
00303
00304 void
00305 MirrorCalibTool::setTotalSteps(unsigned int total_steps)
00306 {
00307 }
00308
00309
00310
00311
00312 void
00313 MirrorCalibTool::setProgress(unsigned int progress)
00314 {
00315 }
00316
00317
00318 void
00319 MirrorCalibTool::finished()
00320 {
00321 }
00322