OpenTracker

An Open Architecture for Reconfigurable Tracking based on XML | Contact

OTQtMEMCalibProc.cxx

Go to the documentation of this file.
00001 /* ========================================================================
00002  * Copyright (c) 2006,
00003  * Institute for Computer Graphics and Vision
00004  * Graz University of Technology
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are
00009  * met:
00010  *
00011  * Redistributions of source code must retain the above copyright notice,
00012  * this list of conditions and the following disclaimer.
00013  *
00014  * Redistributions in binary form must reproduce the above copyright
00015  * notice, this list of conditions and the following disclaimer in the
00016  * documentation and/or other materials provided with the distribution.
00017  *
00018  * Neither the name of the Graz University of Technology nor the names of
00019  * its contributors may be used to endorse or promote products derived from
00020  * this software without specific prior written permission.
00021  *
00022  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
00023  * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
00024  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
00025  * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
00026  * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00027  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
00028  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
00029  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00030  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00031  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00032  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00033  * ========================================================================
00034  * PROJECT: OpenTracker
00035  * ======================================================================== */
00045 #include <dllinclude.h>
00046 #if USE_OTQT
00047 
00048 #include "OTQtMEMCalibProc.h"
00049 #include "OTQtMath.h"
00050 #include "OTQtLog.h"
00051 #include <stdexcept>
00052 #include <stdio.h>
00053 #include <stdexcept>
00054 #include <math.h>
00055 #include <qdatetime.h>
00056 
00057 namespace ot {
00058 
00059 //--------------------------------------------------------------------------------
00060 OTQtMEMCalibProc::OTQtMEMCalibProc(std::string input_cfg_filename, std::string output_cfg_filename )
00061   : OTQt(),
00062     input_cfg_filename_(input_cfg_filename),
00063     output_cfg_filename_(output_cfg_filename),
00064     editor_(OTQtConfigFileEditor(input_cfg_filename))
00065 {
00066 }
00067 
00068 //--------------------------------------------------------------------------------
00069 void OTQtMEMCalibProc::exec() {
00070 
00071   QtAppScreen::CalibInputData calib_input_data;
00072 
00073   printf("\n");
00074   printf("Welcome to the OTQt Calibration Tool!\n");
00075   printf("Author: Christian Pirchheim (chrispi@sbox.tugraz.at)\n");
00076   printf("\n");
00077   printf("-- Input Data:\n");
00078   printf("OT configuration input file: %s\n", input_cfg_filename_.c_str());
00079   printf("OT configuration output file: %s\n",
00080          (output_cfg_filename_.empty()) ? "STDOUT" : output_cfg_filename_.c_str());
00081   printf("OT configuration DTD file: opentracker.dtd\n");
00082   printf("\n");
00083   printf("-- Usage:\n");
00084   printf("Quit anytime by pressing \"ctrl-c\"\n");
00085   printf("\n");
00086   waitForEnterKey();
00087   printf("\n");
00088 
00090 
00091   printf("** Checking OT input configuration file syntax\n");
00092   editor_.init();
00093   if (!isInputConfigFileValid()) {
00094     throw std::invalid_argument("OT Config file is invalid.");
00095   }
00096   printf(".. OK\n");
00097 
00098   printf("** Starting OpenTracker\n");
00100   init(input_cfg_filename_);
00101   printf(".. OK\n");
00102 
00103 #define NEWMAT_EXCEPTION_TEST   0
00104 #if defined(WIN32) && NEWMAT_EXCEPTION_TEST
00105   RowVector v(4); RowVector w(3);
00106   OTQtMath::crossProductR3(v,w);
00107 #endif
00108 
00110   printf("** Tracking ASP (Qt Application Screen Position)\n");
00111   Event app_screen;
00112   if (!trackASPos(app_screen)) {
00113     throw std::runtime_error("Could not track Application Screen Position.");
00114   }
00115   printf("Tracked at: %f %f %f\n",
00116          app_screen.getPosition()[0], app_screen.getPosition()[1], app_screen.getPosition()[2]);
00117   printf(".. OK\n");
00118   calib_input_data.as_cs_root = app_screen;
00119 
00121   printf("** Tracking MPD (Mouse Position Device)\n");
00122   Event mouse_pos_obj;
00123   if (!trackMPD(mouse_pos_obj)) {
00124     throw std::runtime_error("Could not track MPD (Mouse Position Device).");
00125   }
00126   printf("Tracked at: %f %f %f\n",
00127          mouse_pos_obj.getPosition()[0], mouse_pos_obj.getPosition()[1], mouse_pos_obj.getPosition()[2]);
00128   printf(".. OK\n");
00129 
00131   printf("** Tracking MBD (Mouse Button Device) Button One (aka \"Left Mouse Button\")\n");
00132   printf("(TASK): Press and release MBD Button One once ..\n");
00133   Event mbo_button_one;
00134   if (!trackMBDButtonOne(mbo_button_one)) {
00135     throw std::runtime_error("Could not track MBD (Mouse Button Device) Button One.");
00136   }
00137   printf(".. OK\n");
00138 
00139   printf("\n\n");
00140 
00141   // track SC top/left
00142   trackASCornerMain("top/left", calib_input_data.top_left);
00143 
00144   // track SC top/right
00145   trackASCornerMain("top/right", calib_input_data.top_right);
00146 
00147   // track SC top/right
00148   trackASCornerMain("bottom/right", calib_input_data.bottom_right);
00149 
00150   // track SC top/right
00151   trackASCornerMain("bottom/left", calib_input_data.bottom_left);
00152 
00154 
00155   // calib input -> calib output
00156   QtAppScreen::CalibOutputData calib_output_data;
00157   QtAppScreen::convert(calib_input_data, calib_output_data);
00158 
00159   // calib output -> string table
00160 
00161   StringTable attribs;
00162   QtAppScreen::convert(calib_output_data, attribs);
00163 
00165 
00166   OTQtConfigFileEditor::LineList ins_lines;
00167 
00168   // kill lines including
00169   //
00170   // <QtMouseEventCalibConfig
00171   // [..]
00172   // />
00173   int kill_line_no_start = editor_.firstLineNoOfExpr("<QtMouseEventCalibConfig");
00174   int kill_line_no_end = editor_.firstLineNoOfExpr("/>", kill_line_no_start);
00175   for (int count = kill_line_no_start; count <= kill_line_no_end; count++) {
00176     if (!editor_.killLine(kill_line_no_start))
00177       throw logic_error("Killing XML element 'QtMouseEventCalibConfig' failed.");
00178   }
00179 
00180   // insert the following lines
00181   //
00182   // <QtMouseEventConfig>
00183   //   [ QtMouseEventCalibConfig attributes ]
00184   // </QtMouseEventConfig>
00185   int ins_mem_line_no = kill_line_no_start;
00186   int ins_as_line_no = -1;
00187   StringTable mecm_attribs = getMECM().getXMLAttribTable();
00188   int mecm_attrib_count = mecm_attribs.size();
00189   if (mecm_attrib_count != 0) {
00190     ins_lines.push_back("<QtMouseEventConfig");
00191     KeyIterator it_mecm(mecm_attribs);
00192     std::string spaces("  ");
00193     while (it_mecm.hasMoreKeys()) {
00194       std::string key = it_mecm.nextElement();
00195       std::string value = mecm_attribs.get(key);
00196       std::string attribute = spaces + key + "=\"" + value + "\"";
00197       ins_lines.push_back(attribute);
00198     }
00199     ins_lines.push_back(">");
00200     ins_as_line_no = ins_mem_line_no + 1 + mecm_attrib_count + 1;
00201   } else {
00202     ins_lines.push_back("<QtMouseEventConfig>");
00203     ins_as_line_no = ins_mem_line_no + 1;
00204   }
00205   ins_lines.push_back("</QtMouseEventConfig>");
00206   int const indent = 4;
00207   if (!editor_.insertLines(ins_mem_line_no, ins_lines, indent))
00208     throw logic_error("Inserting XML element 'QtMouseEventConfig' failed.");
00209   ins_lines.clear();
00210 
00211   // insert
00212   //
00213   // <QtAppScreen
00214   //   [ attributes ]
00215   // />
00216   ins_lines.push_back("<QtAppScreen");
00217   ins_lines.push_back("/>");
00218   if (!editor_.insertLines(ins_as_line_no, ins_lines, indent + 2))
00219     throw logic_error("Inserting XML element 'QtAppScreen' failed.");
00220   ins_lines.clear();
00221   // insert attributes
00222   KeyIterator it(attribs);
00223   OTQT_DEBUG("--- CalibOutputData ---\n");
00224   while (it.hasMoreKeys()) {
00225     std::string key = it.nextElement();
00226     std::string value = attribs.get(key);
00227     std::string attrib = key + "=\"" + value + "\"";
00228     OTQT_DEBUG("attrib = %s\n", attrib.c_str());
00229     ins_lines.push_back(attrib);
00230   }
00231   if (!editor_.insertLines(++ins_as_line_no, ins_lines, indent + 4))
00232     throw logic_error("Inserting XML attributes of element 'QtAppScreen' failed.");
00233   ins_lines.clear();
00234 
00235 
00237 
00238   printf("\n");
00239   printf("OTQt calibration sucessful!\n");
00240   printf("OT configuraton file is written NOW.\n");
00241   printf("\n");
00242   waitForEnterKey();
00243 
00244   // write config file to stdout / to file
00245   if (!output_cfg_filename_.empty()) {
00246     editor_.writeToFile(output_cfg_filename_);
00247   } else {
00248     editor_.writeToStdOut();
00249   }
00250 }
00251 
00252 //--------------------------------------------------------------------------------
00253 bool
00254 OTQtMEMCalibProc::isInputConfigFileValid() const {
00255   bool ret = true;
00256   ret &= editor_.containsExpr("<QtMouseEventCalibConfig");
00257   ret &= editor_.containsExpr("<QtAppScreenPosSink");
00258   ret &= editor_.containsExpr("<QtMousePosSink");
00259   ret &= editor_.containsExpr("<QtMouseButtonSink");
00260   return ret;
00261 }
00262 
00263 //--------------------------------------------------------------------------------
00264 bool
00265 OTQtMEMCalibProc::trackMPD(Event & event) {
00266   QTime const MAX_TIME = QTime::currentTime().addMSecs(DEV_TRACKING_MAX_TIMEOUT_MSEC);
00267   while (QTime::currentTime() <= MAX_TIME) {
00268 //    getMECM().resetPendingEventBitAllSinks();
00269     driveOT();
00270     if (getMECM().getMPS().isEventPending()) {
00271       event = getMECM().getMPS().getCurrentEvent();
00272       return true;
00273     }
00274   }
00275   return false;
00276 }
00277 
00278 //--------------------------------------------------------------------------------
00279 bool
00280 OTQtMEMCalibProc::trackASPos(Event & event) {
00281   QTime const MAX_TIME = QTime::currentTime().addMSecs(DEV_TRACKING_MAX_TIMEOUT_MSEC);
00282   while (QTime::currentTime() <= MAX_TIME) {
00283 //    getMECM().resetPendingEventBitAllSinks();
00284     driveOT();
00285     if (getMECM().getASPS().isEventPending()) {
00286       event = getMECM().getASPS().getCurrentEvent();
00287       return true;
00288     }
00289   }
00290   return false;
00291 }
00292 
00293 //--------------------------------------------------------------------------------
00294 bool
00295 OTQtMEMCalibProc::trackMBDButtonOne(Event & event) {
00296   unsigned char test_mask = 0x00;
00297   while (true) {
00298 //    getMECM().resetPendingEventBitAllSinks();
00299     driveOT();
00300     bool is_event_pending = getMECM().getMBS().isEventPending();
00301     OTQT_DEBUG("OTQtMEMCalibProc::trackMBDButtonOne(): +++ MBS.isEventPending() = %d\n", is_event_pending);
00302     if (is_event_pending) {
00303       if (getMECM().getMBS().buttonPressed(QtMouseButtonSink::LEFT_BUTTON)) {
00304         printf("MBD Button One pressed.\n");
00305         test_mask |= 0xF0;
00306       } else if (getMECM().getMBS().buttonReleased(QtMouseButtonSink::LEFT_BUTTON)) {
00307         printf("MBD Button One released.\n");
00308         test_mask |= 0x0F;
00309       }
00310       if (test_mask == 0xFF) {
00311         event = getMECM().getMBS().getCurrentEvent();
00312         return true;
00313       }
00314     } // if
00315     if (OTQT_DEBUG_ON) is_event_pending = getMECM().getMBS().isEventPending();
00316     OTQT_DEBUG("OTQtMEMCalibProc::trackMBDButtonOne(): --- MBS.isEventPending() = %d\n", is_event_pending);
00317   } // while
00318   return false;
00319 }
00320 
00321 //--------------------------------------------------------------------------------
00322 void OTQtMEMCalibProc::trackASCornerMain(char const * desc, QtAppScreen::ASCorner & as_corner) {
00323   printf("** Tracking Screen Corner: %s\n", desc);
00324   printf("(TASK): Guide MPD to %s screen corner and keep MBD Button One pressed when ready ..\n", desc);
00325   if (!trackASCorner(as_corner)) {
00326     std::string err_msg = "Failed tracking screen corner ";
00327     err_msg += std::string(desc) + std::string(".");
00328     throw std::runtime_error(err_msg.c_str());
00329   }
00330   printf("Tracked at: %f %f %f (ASP: %f %f %f)\n",
00331          as_corner.corner.getPosition()[0], as_corner.corner.getPosition()[1], as_corner.corner.getPosition()[2],
00332          as_corner.local_cs_root.getPosition()[0], as_corner.local_cs_root.getPosition()[1], as_corner.local_cs_root.getPosition()[2]);
00333   printf(".. OK\n");
00334   printf("\n");
00335   if (getMECM().getMBS().buttonOn(QtMouseButtonSink::LEFT_BUTTON)) {
00336     printf("Release MBD Button One to continue ..");
00337     fflush(stdout);
00338     while (!getMECM().getMBS().buttonReleased(QtMouseButtonSink::LEFT_BUTTON)) {
00339 //      getMECM().resetPendingEventBitAllSinks();
00340       driveOT();
00341     }
00342     printf("\n\n");
00343   }
00344 }
00345 
00346 //--------------------------------------------------------------------------------
00347 bool OTQtMEMCalibProc::trackASCorner(QtAppScreen::ASCorner & as_corner) {
00348 
00349   OTQT_DEBUG("OTQtMEMCalibProc::trackASCorner(): *** START.\n");
00350 
00351   // init timer
00352   QTime timer;
00353   // number of cycles while timer is running
00354   unsigned int cycle_counter = 0;
00355   // corner tracking trigger flag
00356   bool is_button_one_pressed = false;
00357 
00358   // one meter in tracking scale
00359   float tracking_one_meter;
00360   getMECM().getXMLAttribTable().get("TrackingSystemScaleOneMeter", &tracking_one_meter);
00361   // converted to millimeter
00362   float THRESH_SPHERE_RADIUS = tracking_one_meter * 0.001;
00363 
00364   while (true) {
00365 
00366 #define SINGLE_CORNER_TEST    0
00367 #define CONVERT_DATA_TEST     0
00368 #if SINGLE_CORNER_TEST
00369     sleep(1);
00370 #endif
00371 //    getMECM().resetPendingEventBitAllSinks();
00372     driveOT();
00373     OTQT_DEBUG("OTQtMEMCalibProc::trackASCorner(): +++ CYCLE STARTED.\n");
00374 
00376 
00377     if (getMECM().getMBS().isEventPending()) {
00378       OTQT_DEBUG("OTQtMEMCalibProc::trackASCorner(): MBS is pending\n");
00379       // start timer / set corner position
00380       if (getMECM().getMBS().buttonPressed(QtMouseButtonSink::LEFT_BUTTON)) {
00381         printf("Button One pressed: tracked corner position, started timer.\n");
00382         // start timer
00383         timer.start();
00384         cycle_counter = 1;
00385         // store corner data
00386         as_corner.local_cs_root = getMECM().getASPS().getCurrentEvent();
00387         as_corner.corner = getMECM().getMPS().getCurrentEvent();
00388         is_button_one_pressed = true;
00389         continue;
00390       }
00391       // stop timer / release corner
00392       if (getMECM().getMBS().buttonReleased(QtMouseButtonSink::LEFT_BUTTON)) {
00393         printf("Button One released: stopped timer.\n");
00394         is_button_one_pressed = false;
00395         // reset timer
00396         timer = QTime();
00397         cycle_counter = 0;
00398         continue;
00399       }
00400     }
00401 
00402 #if CONVERT_DATA_TEST
00403 #else
00404     // return if trigger not enabled
00405     if (!is_button_one_pressed) {
00406       continue;
00407     }
00408 #endif
00409 
00410     // do not test distance by default
00411     bool do_distance_check = false;
00412 
00414 
00415     // NOTE: as_corner = [ prev_local_cs_root, prev_corner ]
00416 
00417     Event curr_corner;
00418     if (getMECM().getMPS().isEventPending()) {
00419       OTQT_DEBUG("OTQtMEMCalibProc::trackASCorner(): MPS is pending\n");
00420       curr_corner = getMECM().getMPS().getCurrentEvent();
00421       do_distance_check = true;
00422     } else {
00423       curr_corner = as_corner.corner;
00424     }
00425     OTQT_DEBUG("OTQtMEMCalibProc::trackASCorner(): curr_corner = %f %f %f\n",
00426                curr_corner.getPosition()[0], curr_corner.getPosition()[1], curr_corner.getPosition()[2]);
00427 
00429 
00430     // convert previous corner so that it is compareable with current corner
00431     Event prev_cmpable_curr_corner;
00432     if (getMECM().getASPS().isEventPending()) {
00433       OTQT_DEBUG("OTQtMEMCalibProc::trackASCorner(): ASP is pending\n");
00434       Event curr_local_cs_root = getMECM().getASPS().getCurrentEvent();
00435       OTQtMath::transformVectorFromCSToCS(as_corner.local_cs_root, curr_local_cs_root,
00436                                           as_corner.corner, prev_cmpable_curr_corner);
00437       do_distance_check = true;
00438     } else {
00439       prev_cmpable_curr_corner = curr_corner;
00440     }
00441     OTQT_DEBUG("OTQtMEMCalibProc::trackASCorner(): prev_cmpable_curr_corner = %f %f %f\n",
00442                prev_cmpable_curr_corner.getPosition()[0], prev_cmpable_curr_corner.getPosition()[1], prev_cmpable_curr_corner.getPosition()[2]);
00443 
00444     // check if current event is within threshold sphere around previous event
00445     if (do_distance_check) {
00446       // compare distance between current and previous corner with threshold radius
00447       float const curr_prev_distance = OTQtMath::distance(prev_cmpable_curr_corner.getPosition(),
00448                                                           curr_corner.getPosition());
00449       OTQT_DEBUG("OTQtMEMCalibProc::trackASCorner(): distance check, curr_prev_distance = %f\n", curr_prev_distance);
00450       if (curr_prev_distance >= THRESH_SPHERE_RADIUS) {
00451         OTQT_DEBUG("OTQtMEMCalibProc::trackASCorner(): curr outside of THRESH_SPHERE_RADIUS\n");
00452         // reset timer
00453         printf("Corner position out of bounds: tracked new corner position, restarted timer.\n");
00454         timer.restart();
00455         cycle_counter = 1;
00456       }
00457       // set corner data
00458       as_corner.local_cs_root = getMECM().getASPS().getCurrentEvent();
00459       as_corner.corner = getMECM().getMPS().getCurrentEvent();
00460     }
00461 
00463 
00464 #if CONVERT_DATA_TEST
00465     return true;
00466 #else
00467     if (timer.elapsed() >= CORNER_ACCEPTANCE_TIMEOUT_MSEC) {
00468       OTQT_DEBUG("OTQtMEMCalibProc::trackASCorner(): *** FINISHED with ret = true.\n");
00469 
00470       // TODO: compute mean sum of corner position
00471 
00472       return true;
00473     }
00474 #endif
00475   } // while END
00476 
00477   OTQT_DEBUG("OTQtMEMCalibProc::trackASCorner(): *** FINISHED with ret = false.\n");
00478   return false;
00479 }
00480 
00481 //--------------------------------------------------------------------------------
00482 void OTQtMEMCalibProc::waitForEnterKey() const {
00483   char ch = '\0';
00484   printf("Press ENTER to continue ..");
00485   fflush(stdout);
00486   while (!(ch == '\n' || ch == '\r'))
00487     ch = (char)getchar();
00488   return;
00489 }
00490 
00491 } // namespace ot
00492 
00493 #endif // USE_OTQT
00494 
00495 
00496 /*
00497  * ------------------------------------------------------------
00498  *   End of OTQtMEMCalibProc.cxx
00499  * ------------------------------------------------------------
00500  *   Automatic Emacs configuration follows.
00501  *   Local Variables:
00502  *   mode:c++
00503  *   c-basic-offset: 4
00504  *   eval: (c-set-offset 'substatement-open 0)
00505  *   eval: (c-set-offset 'case-label '+)
00506  *   eval: (c-set-offset 'statement 'c-lineup-runin-statements)
00507  *   eval: (setq indent-tabs-mode nil)
00508  *   End:
00509  * ------------------------------------------------------------
00510  */

copyright (c) 2006 Graz University of Technology