OpenTracker

An Open Architecture for Reconfigurable Tracking based on XML | Contact

ARToolKitPlusModule.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  * ======================================================================== */
00043 /* ======================================================================= */
00044 
00045 
00046 // this will remove the warning 4786
00047 #include "../tool/disable4786.h"
00048 #include <cstdlib>
00049 #include <algorithm>
00050 #include <cctype>
00051 
00052 #include "../OpenTracker.h"
00053 
00054 #include <ace/Log_Msg.h>
00055 #include "../tool/OT_ACE_Log.h"
00056 
00057 #include "ARToolKitPlusModule.h"
00058 #include "ARToolKitSource.h"
00059 #include "ARToolKitMultiMarkerSource.h"
00060 
00061 #ifdef USE_ARTOOLKITPLUS
00062 
00063 
00064 // in SAM we use another mechanism to link against ARToolKitPlus
00065 // in order to use the release DLL even in debug mode.
00066 // for that case we can define OT_DO_NOT_LINK_ARTOOLKITPLUS
00067 // to deactivate the automatic linking process
00068 //
00069 #ifdef ARTOOLKITPLUS_DLL
00070 #  include <ARToolKitPlus/TrackerSingleMarker.h>
00071 #  pragma message("compiling against ARToolKitPlus DLL")
00072 #  ifndef OT_DO_NOT_LINK_ARTOOLKITPLUS
00073 #    if (defined(DEBUG) || defined(_DEBUG)) && !defined(ARTOOLKITPLUS_ALWAYS_RELEASE)
00074 #      pragma comment(lib, "ARToolKitPlusDllD.lib")
00075 #      pragma message("ARToolKitPlusModule: linking against ARToolKitPlusDllD.lib")
00076 #    else
00077 #      pragma comment(lib, "ARToolKitPlusDll.lib")
00078 #      pragma message("ARToolKitPlusModule: linking against ARToolKitPlusDll.lib")
00079 #    endif
00080 #  endif
00081 #else
00082 #  pragma message("compiling against ARToolKitPlus Lib")
00083 #  include <ARToolKitPlus/TrackerSingleMarkerImpl.h>
00084 #  ifndef OT_DO_NOT_LINK_ARTOOLKITPLUS
00085 #    if (defined(DEBUG) || defined(_DEBUG)) &&  !defined(ARTOOLKITPLUS_ALWAYS_RELEASE)
00086 #      pragma comment(lib, "ARToolKitPlusD.lib")
00087 #      pragma message("ARToolKitPlusModule: linking against ARToolKitPlusD.lib")
00088 #    else
00089 #      pragma comment(lib, "ARToolKitPlus.lib")
00090 #      pragma message("ARToolKitPlusModule: linking against ARToolKitPlus.lib")
00091 #    endif
00092 #  endif
00093 #endif
00094 
00095 #include <ARToolKitPlus/Logger.h>
00096 
00097 class ARToolKitPlusModuleLogger : public ARToolKitPlus::Logger
00098 {
00099     // implement ARToolKitPlus::Logger
00100     void artLog(const char* nStr)
00101     {
00102         LOG_ACE_INFO("ot:%s\n", nStr);
00103     }
00104 
00105     void artLogEx(const char* nStr, ...)
00106     {
00107         char tmpString[512];
00108         va_list marker;
00109 
00110         va_start(marker, nStr);
00111         vsprintf(tmpString, nStr, marker);
00112 
00113         artLog(tmpString);
00114     }
00115 };
00116 
00117 #include <cstdio>
00118 #include <cstdlib>
00119 #include <iostream>
00120 
00121 
00122 namespace ot {
00123 
00124 
00125     bool convertPixelFormat_OpenTracker_to_ARToolKitPlus(ot::PIXEL_FORMAT nSrcFormat, ARToolKitPlus::PIXEL_FORMAT &nDstFormat)
00126     {
00127     switch(nSrcFormat)
00128     {
00129             case ot::FORMAT_X8B8G8R8:
00130         nDstFormat = ARToolKitPlus::PIXEL_FORMAT_ABGR;
00131         return true;
00132             case ot::FORMAT_B8G8R8X8:
00133         nDstFormat = ARToolKitPlus::PIXEL_FORMAT_BGRA;
00134         return true;
00135             case ot::FORMAT_B8G8R8:
00136         nDstFormat = ARToolKitPlus::PIXEL_FORMAT_BGR;
00137         return true;
00138             case ot::FORMAT_R8G8B8X8:
00139         nDstFormat = ARToolKitPlus::PIXEL_FORMAT_RGBA;
00140         return true;
00141             case ot::FORMAT_R8G8B8:
00142         nDstFormat = ARToolKitPlus::PIXEL_FORMAT_RGB;
00143         return true;
00144             case ot::FORMAT_R5G6B5:
00145         nDstFormat = ARToolKitPlus::PIXEL_FORMAT_RGB565;
00146         return true;
00147             case ot::FORMAT_L8:
00148         nDstFormat = ARToolKitPlus::PIXEL_FORMAT_LUM;
00149         return true;
00150             default:
00151         return false;
00152     }
00153     }
00154 
00155     bool convertPixelFormat_ARToolKitPlus_to_OpenTracker(ARToolKitPlus::PIXEL_FORMAT nSrcFormat, ot::PIXEL_FORMAT &nDstFormat)
00156     {
00157     switch(nSrcFormat)
00158     {
00159             case ARToolKitPlus::PIXEL_FORMAT_ABGR:
00160         nDstFormat = ot::FORMAT_X8B8G8R8;
00161         return true;
00162             case ARToolKitPlus::PIXEL_FORMAT_BGRA:
00163         nDstFormat = ot::FORMAT_B8G8R8X8;
00164         return true;
00165             case ARToolKitPlus::PIXEL_FORMAT_BGR:
00166         nDstFormat = ot::FORMAT_B8G8R8;
00167         return true;
00168             case ARToolKitPlus::PIXEL_FORMAT_RGBA:
00169         nDstFormat = ot::FORMAT_R8G8B8X8;
00170         return true;
00171             case ARToolKitPlus::PIXEL_FORMAT_RGB:
00172         nDstFormat = ot::FORMAT_R8G8B8;
00173         return true;
00174             case ARToolKitPlus::PIXEL_FORMAT_RGB565:
00175         nDstFormat = ot::FORMAT_R5G6B5;
00176         return true;
00177             case ARToolKitPlus::PIXEL_FORMAT_LUM:
00178         nDstFormat = ot::FORMAT_L8;
00179         return true;
00180             default:
00181         return false;
00182     }
00183     }
00184 
00185 
00186     ARToolKitPlusModule::ARToolKitPlusModule() : ThreadModule(), NodeFactory()
00187     {
00188     doBench = false;
00189     sizeX = sizeY = -1;
00190 
00191     trackerNear = 1.0f;
00192     trackerFar = 1000.0f;
00193 
00194     logger = new ARToolKitPlusModuleLogger;
00195 
00196 
00197 #ifdef ARTOOLKITPLUS_DLL
00198 
00199     tracker = ARToolKitPlus::createTrackerSingleMarker(320,240, 6,6,6);
00200 
00201 #else
00202 
00203     // NOTE: if you get an error here complaining about 6 template parameters
00204     //       then please update to the latest version of ARToolKitPlus
00205     tracker = new ARToolKitPlus::TrackerSingleMarkerImpl<6,6,6, 32,32>(320,240);
00206 
00207 #endif //ARTOOLKITPLUS_DLL
00208 
00209 
00210     tracker->init(NULL, trackerNear, trackerFar, logger);
00211     tracker->setThreshold(100);
00212 
00213     bestCFs = NULL;
00214     maxMarkerId = MAX_MARKERID;
00215     useMarkerDetectLite = false;
00216 
00217     stop = 0;
00218     }
00219 
00220     ARToolKitPlusModule::~ARToolKitPlusModule()
00221     {
00222         sources.clear();
00223     sourcesMap.clear();
00224 
00225     delete tracker;
00226     delete logger;
00227     }
00228 
00229     // This method is called to construct a new Node.
00230 
00231     Node* ARToolKitPlusModule::createNode( const std::string& name, StringTable& attributes)
00232     {
00233         if( name.compare("ARToolKitPlusSource") == 0 )
00234         {
00235             double center[2], size;
00236             int num;
00237 
00238             if( (num = attributes.get("center", center, 2)) != 2 )
00239             {
00240                 //cout << "ARToolKit Wrong number of center coordinates : " << num << endl;
00241                 ACE_DEBUG((LM_DEBUG, ACE_TEXT("ot:ARToolKit Wrong number of center coordinates : %d\n"), num));
00242                 return NULL;
00243             }
00244             if( attributes.get("size", &size) != 1 )
00245             {
00246                 //cout << "ARToolKit Not a size" << endl;
00247                 ACE_DEBUG((LM_DEBUG, ACE_TEXT("ot:ARToolKit Not a size\n")));
00248                 return NULL;
00249             }
00250 
00251             int id;
00252             std::string filename = attributes.get("tag-file");
00253             std::string markerid = attributes.get("tag-id");
00254             std::string fullname;
00255 
00256             // see if we have a marker-id field
00257             if(markerid.length())
00258             {
00259                 id = atoi(markerid.c_str());
00260             }
00261             else
00262         // otherwise look for a marker-filename
00263         if(filename.length())
00264         {
00265                     if( patternDirectory.compare("") != 0)
00266                         context->addDirectoryFirst( patternDirectory );
00267 
00268                     if( context->findFile(filename, fullname))
00269                     {
00270                         filename = fullname;
00271                     }
00272                     else
00273                     {
00274                         LOG_ACE_ERROR("ot:ARToolkit could not find tag file %s\n", filename.c_str());
00275                         return NULL;
00276                     }
00277 
00278                     if( patternDirectory.compare("") != 0)
00279                         context->removeDirectory( patternDirectory );
00280         
00281                     if((id = tracker->addPattern((char *)filename.c_str() )) < 0 )
00282                     {
00283                         LOG_ACE_ERROR("ot:ARToolKit Error reading tag-file %s or %s\n", attributes.get("tag-file").c_str(), filename.c_str());
00284                         return NULL;
00285                     }
00286         }
00287         else
00288         {
00289                     // not good, this means we have a problem...
00290         }
00291 
00292             // we need to store the maximum marker id for later...
00293             if(id>maxMarkerId)
00294             {
00295                 maxMarkerId = id;
00296                 if(bestCFs)
00297                 {
00298                     delete bestCFs;
00299                     bestCFs = NULL;
00300                 }
00301             }
00302 
00303             ARToolKitSource * source = new ARToolKitSource( id, center, size );
00304             source->type = "ARToolKitSource";
00305             sources.push_back( source );
00306             sourcesMap.insert(std::make_pair(id, source));
00307 
00308             //cout << "Build ARToolKitSource " << filename << " id " << id << endl;
00309             LOG_ACE_INFO("ot:Build ARToolKitSource %s id %d\n", filename.c_str(), id);
00310             return source;
00311         }
00312 
00313         if( name.compare("ARToolKitPlusMultiMarkerSource") == 0 )
00314         {
00315             std::string filename = attributes.get("cfg-file");
00316             std::string fullname;
00317 
00318             if(patternDirectory.compare("") != 0)
00319                 context->addDirectoryFirst(patternDirectory);
00320 
00321             if(context->findFile(filename, fullname))
00322                 filename = fullname;
00323             else
00324             {
00325                 LOG_ACE_ERROR("ot:ARToolkit could not find multi-cfg file %s\n", filename.c_str());
00326                 return NULL;
00327             }
00328 
00329             ARToolKitPlus::ARMultiMarkerInfoT* mmConfig = tracker->arMultiReadConfigFile(filename.c_str());
00330 
00331             if(mmConfig)
00332             {
00333                 ARToolKitMultiMarkerSource * source = new ARToolKitMultiMarkerSource( filename, mmConfig );
00334                 source->type = "ARToolKitMultiMarkerSource";
00335                 sources.push_back( source );
00336 
00337                 // we store the ids of all markers in this config...
00338                 for(int i=0; i<mmConfig->marker_num; i++)
00339                 {
00340                     int mId = mmConfig->marker[i].patt_id;
00341 
00342                     sourcesMap.insert(std::make_pair(mId, source));
00343 
00344                     if(mId>maxMarkerId)
00345                         maxMarkerId = mId;
00346                 }
00347 
00348                 if(bestCFs)
00349                 {
00350                     delete bestCFs;
00351                     bestCFs = NULL;
00352                 }
00353 
00354                 return source;
00355             }
00356             else
00357             {
00358                 LOG_ACE_ERROR("ot:ARToolKit Error reading multi-cfg %s\n");
00359                 return NULL;
00360             }
00361     }
00362 
00363         return NULL;
00364     }
00365 
00366 
00367     // initializes the ARToolKit module
00368 
00369 #define MAKE_STRING_LOWER(STR)  std::transform(STR.begin(), STR.end(), STR.begin(), std::tolower);
00370 
00371     void ARToolKitPlusModule::init(StringTable& attributes, ConfigNode * localTree)
00372     {
00373     cameradata = attributes.get("camera-parameter");
00374         patternDirectory = attributes.get("pattern-dir");
00375 
00376     std::string undistmode = attributes.get("undist-mode");
00377     std::string detectmode = attributes.get("detect-mode");
00378     std::string posemode = attributes.get("pose-estimator");
00379     std::string threshold = attributes.get("treshold");
00380     std::string markermode = attributes.get("marker-mode");
00381 
00382     MAKE_STRING_LOWER(undistmode);
00383     MAKE_STRING_LOWER(detectmode);
00384     MAKE_STRING_LOWER(posemode);
00385     MAKE_STRING_LOWER(threshold);
00386     MAKE_STRING_LOWER(markermode);
00387 
00388 
00389     // marker detection mode: lite vs. full
00390     //
00391     if(useMarkerDetectLite = (detectmode=="lite"))
00392     {
00393             LOG_ACE_INFO("ot:ARToolkitModule using marker detect mode 'lite'\n");
00394     }
00395     else
00396     {
00397             LOG_ACE_INFO("ot:ARToolkitModule using marker detect mode 'normal'\n");
00398     }
00399 
00400 
00401     if(undistmode=="none")
00402     {
00403             tracker->setUndistortionMode(ARToolKitPlus::UNDIST_NONE);
00404             LOG_ACE_INFO("ot:ARToolkitModule lens undistortion disabled\n");
00405     }
00406     else if(undistmode=="lut")
00407     {
00408             tracker->setUndistortionMode(ARToolKitPlus::UNDIST_LUT);
00409             LOG_ACE_INFO("ot:ARToolkitModule lens undistortion set to lookup-table\n");
00410     }
00411     else
00412     {
00413             LOG_ACE_INFO("ot:ARToolkitModule using default lens undistortion (this will be slow!)\n");
00414     }
00415 
00416 
00417     // pose estimator settings
00418     //
00419     if(posemode=="cont")
00420     {
00421             tracker->setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_ORIGINAL_CONT);
00422             LOG_ACE_INFO("ot:ARToolkitModule using CONT pose estimator\n");
00423     }
00424     else if(posemode=="rpp")
00425     {
00426             tracker->setPoseEstimator(ARToolKitPlus::POSE_ESTIMATOR_RPP);
00427             LOG_ACE_INFO("ot:ARToolkitModule using robust pose estimator\n");
00428     }
00429     else
00430     {
00431             LOG_ACE_INFO("ot:ARToolkitModule using default pose estimator\n");
00432     }
00433 
00434 
00435     // setting of threshold value
00436     //
00437     if(threshold == "auto")
00438     {
00439             tracker->activateAutoThreshold(true);
00440             LOG_ACE_INFO("ot:ARToolkitModule auto-thresholding activated\n");
00441     }
00442     else
00443     {
00444             int tmpThreshold = atoi(threshold.c_str());
00445 
00446             if( tmpThreshold < 0 )
00447                 tmpThreshold = 0;
00448             else if( tmpThreshold > 255 )
00449                 tmpThreshold = 255;
00450 
00451             tracker->setThreshold(tmpThreshold);
00452             LOG_ACE_INFO("ot:ARToolkitModule manual thresholding with '%s'\n", tmpThreshold);
00453     }
00454 
00455 
00456 
00457     // setting for id-based vs. template-based markers
00458     //
00459     if(markermode == "idbased")
00460     {
00461             tracker->setMarkerMode(ARToolKitPlus::MARKER_ID_SIMPLE);
00462             LOG_ACE_INFO("ot:ARToolkitModule using id-based markers\n");
00463     }
00464     else
00465             if(markermode == "bch")
00466             {
00467         tracker->setMarkerMode(ARToolKitPlus::MARKER_ID_BCH);
00468         LOG_ACE_INFO("ot:ARToolkitModule using BCH markers\n");
00469             }
00470             else
00471             {
00472         LOG_ACE_INFO("ot:ARToolkitModule using template markers\n");
00473             }
00474 
00475 
00476     // setting for the border width - default value is 0.250
00477     //
00478     if(attributes.get("border-width").length()>0)
00479     {
00480             float w = (float)atof(attributes.get("border-width").c_str());
00481             LOG_ACE_INFO("ot:ARToolkitModule using border-width of %.3f\n", w);
00482             tracker->setBorderWidth(w);
00483     }
00484     else
00485     {
00486             LOG_ACE_INFO("ot:ARToolkitModule using default border-width of 0.250\n");
00487     }
00488 
00489 
00490         // parsing camera config hints
00491     //
00492     if(cameraDeviceHint.length()>0)
00493             for(unsigned int i = 0; i <localTree->countChildren(); i++)
00494             {
00495                 ConfigNode * child = (ConfigNode *)localTree->getChild(i);
00496                 StringTable & childAttr = child->getAttributes();
00497 
00498                 std::string devName = childAttr.get("device-name"),
00499                     devFile = childAttr.get("camera-parameter");
00500 
00501                 if(devName.length()>0 && devFile.length()>0 && devName==cameraDeviceHint)
00502                 {
00503                     LOG_ACE_INFO("ot:ARToolkitModule uses camera-hint for '%s'\n", devName.c_str());
00504                     cameradata = devFile;
00505                     break;
00506                 }
00507             }
00508 
00509     LOG_ACE_INFO("ot:ARToolkitModule loads camera file %s\n", cameradata.c_str());
00510 
00511 
00512     // setting for template pattern folder
00513     //
00514         if( patternDirectory.compare("") != 0)
00515             context->addDirectoryFirst( patternDirectory );
00516 
00517         std::string fullname;
00518         if( context->findFile(cameradata, fullname))
00519         {
00520             cameradata = fullname;
00521         }
00522         else
00523         {
00524             //cout << "ARToolkitModule could not find camera parameter file " << cameradata << endl;
00525             LOG_ACE_ERROR("ot:ARToolkitModule could not find camera parameter file %s\n", cameradata.c_str());
00526             initialized = 0;
00527             return;
00528         }
00529 
00530         if( patternDirectory.compare("") != 0)
00531             context->removeDirectory( patternDirectory );
00532 
00533 
00534     // finally load the camera file
00535     //
00536     if(!tracker->loadCameraFile(cameradata.c_str(), trackerNear, trackerFar))
00537     {
00538             LOG_ACE_ERROR("ot:ARToolkitModule error loading camera parameters from %s\n", cameradata.c_str());
00539             initialized = 0;
00540             return;
00541     }
00542 
00543 
00544     initialized = 1;
00545     LOG_ACE_INFO("ot:ARToolkitModule initialization finished\n");
00546     }
00547 
00548 
00549     void
00550     ARToolKitPlusModule::newVideoFrame(const unsigned char* frameData, int newSizeX, int newSizeY, PIXEL_FORMAT imgFormat)
00551     {
00552     if(!initialized || maxMarkerId<0)
00553             return;
00554 
00555         ARToolKitPlus::ARMarkerInfo * markerInfo;
00556         int markerNum;
00557         int j;
00558         ARFloat matrix[3][4];
00559     PIXEL_FORMAT artkpImgFormat;
00560 
00561     if(!convertPixelFormat_ARToolKitPlus_to_OpenTracker(tracker->getPixelFormat(), artkpImgFormat))
00562             return;
00563 
00564     // did we get another pixel format than ARToolKitPlus currently expects?
00565     if(imgFormat!=artkpImgFormat)
00566     {
00567             // ARToolKitPlus 2.1 and later can change pixel format at runtime!
00568             //
00569             ARToolKitPlus::PIXEL_FORMAT artkpFormat;
00570             if(!convertPixelFormat_OpenTracker_to_ARToolKitPlus(imgFormat, artkpFormat))
00571                 return;
00572             tracker->setPixelFormat(artkpFormat);
00573     }
00574 
00575 
00576     // if the image size changed we have to reinitialize some ARToolKitPlus internal stuff...
00577     // 
00578     if(newSizeX!=sizeX || newSizeY!=sizeY)
00579     {
00580             sizeX = newSizeX;
00581             sizeY = newSizeY;
00582             tracker->changeCameraSize(newSizeX, newSizeY);
00583     }
00584 
00585 
00586     // reset all markers from last grab, then clear list
00587     //
00588         for(NodeVector::iterator it=visibleMarkers.begin(); it!=visibleMarkers.end(); it++)
00589     {
00590         ARToolKitSource *source = (ARToolKitSource *)*it;
00591 
00592             Event & event = source->buffer;
00593             if (event.getConfidence() > 0.00000001f) 
00594             {
00595                 event.getConfidence() = 0.0f;
00596                 event.timeStamp();
00597                 source->modified = 1;
00598             }
00599     }
00600     visibleMarkers.clear();
00601 
00602     // try to find markers in the camera image
00603     //
00604     if(useMarkerDetectLite)
00605     {
00606             if(tracker->arDetectMarkerLite((ARToolKitPlus::ARUint8*)frameData, tracker->getThreshold(), &markerInfo, &markerNum ) < 0 )
00607             {
00608                 return;
00609             }
00610     }
00611     else
00612     {
00613             if(tracker->arDetectMarker((ARToolKitPlus::ARUint8*)frameData, tracker->getThreshold(), &markerInfo, &markerNum ) < 0 )
00614             {
00615                 return;
00616             }
00617     }
00618 
00619         if( markerNum < 1 )
00620     {
00621             return;
00622     }
00623 
00624     // we use an array of best confidences to quickly find
00625     // the best markers for each id. a simple check against all
00626     // other visible markers would result in O(nē), now this method 
00627     // results roughly in 4*O(n) which is better than O(nē) for large n.
00628     //
00629     if(!bestCFs)
00630     {
00631             bestCFs = new float[maxMarkerId+1];
00632             for(j=0; j<maxMarkerId; j++)
00633                 bestCFs[j] = 0.0f;
00634     }
00635 
00636 
00637     // store best confidences
00638     //
00639     for(j=0; j<markerNum; j++)
00640     {
00641             int id = markerInfo[j].id;
00642             if(id!=-1 && bestCFs[id]<markerInfo[j].cf)
00643                 bestCFs[id] = markerInfo[j].cf;
00644     }
00645 
00646 
00647     // stores all multi.marker sources that have already been processed
00648     // in order to prevent multiple calculation of the same source due to multiple markers
00649     NodeVector processedSources;
00650 
00651 
00652     // walk through all markers in the image...
00653     //
00654 
00655     for(j=0; j<markerNum; j++)
00656     {
00657             int id = markerInfo[j].id;
00658             Node* source = NULL;
00659 
00660             if(id==-1)
00661                 continue;
00662 
00663             // only use a marker if it has the best confidence for its id
00664             //
00665             if(markerInfo[j].cf<bestCFs[id])
00666                 continue;
00667 
00668             MarkerIdMap::iterator it = sourcesMap.find(id);
00669 
00670             if(it==sourcesMap.end())
00671                 continue;
00672 
00673             source = it->second;
00674             if(!source)
00675                 continue;
00676 
00677             // we found a "best" marker and its source.
00678             // everything is fine to process it...
00679 
00680             // store that source for later usage
00681             //
00682             visibleMarkers.push_back(source);
00683 
00684             if(source->getType()=="ARToolKitPlusSource")
00685             {
00686             ARToolKitSource *sourceA = (ARToolKitSource*)source;
00687                 ARFloat source_center[2], source_size;
00688             
00689                 source_center[0] = (ARFloat)sourceA->center[0];
00690                 source_center[1] = (ARFloat)sourceA->center[1];
00691                 source_size = (ARFloat)sourceA->size;
00692 
00693                 //if(tracker->arGetTransMat(&markerInfo[j], source_center, source_size, matrix)>=0)
00694                 if(tracker->executeSingleMarkerPoseEstimator(&markerInfo[j], source_center, source_size, matrix)>=0)
00695                                                                                                                         updateSingleMarkerSource(sourceA, markerInfo[j].cf, matrix);
00696             }
00697             else
00698         if(source->getType()=="ARToolKitPlusMultiMarkerSource")
00699         {
00700                     bool alreadyProcessed = false;
00701 
00702                     for(NodeVector::iterator it=processedSources.begin(); it!=processedSources.end(); it++)
00703                         if(*it==source)
00704                         {
00705                             alreadyProcessed = true;
00706                             break;
00707                         }
00708 
00709                     if(!alreadyProcessed)
00710                     {
00711                         ARToolKitMultiMarkerSource *sourceM = (ARToolKitMultiMarkerSource*)source;
00712                         ARToolKitPlus::ARMultiMarkerInfoT* mmConfig = (ARToolKitPlus::ARMultiMarkerInfoT*)sourceM->mmConfig;
00713 
00714                         //if((tracker->arMultiGetTransMat(markerInfo, markerNum, mmConfig))>=0)
00715                         if((tracker->executeMultiMarkerPoseEstimator(markerInfo, markerNum, mmConfig))>=0)
00716                                                                                                               updateMultiMarkerSource(sourceM, 1.0f, mmConfig->trans);
00717 
00718                         processedSources.push_back(source);
00719                     }
00720         }
00721     }
00722 
00723 
00724     // reset array of best confidences
00725     //
00726     for(j=0; j<markerNum; j++)
00727             bestCFs[markerInfo[j].id] = 0.0f;
00728 
00729     return;
00730     }
00731 
00732     // pushes events into the tracker tree.
00733 
00734     void ARToolKitPlusModule::pushEvent()
00735     {
00736     for( NodeVector::iterator it = sources.begin(); it != sources.end(); it ++ )
00737     {
00738             ARToolKitSource * source = (ARToolKitSource *)*it;
00739             if( source->modified == 1 )
00740             {
00741                 source->event = source->buffer;
00742                 source->modified = 0;
00743                 source->updateObservers( source->event );
00744             }
00745     }
00746     }
00747 
00748     void ARToolKitPlusModule::updateEvent(Event &event, float matrix[3][4])
00749     {
00750 #ifdef ARTOOLKIT_UNFLIP_V
00751 #undef ARTOOLKIT_UNFLIP_V
00752 #endif
00753 
00754 #ifdef ARTOOLKIT_UNFLIP_V
00755 
00756     //  --- correct ARToolkit's vertical image mirroring ---
00757 
00758     MathUtils::Matrix4x4 matrix_4x4;
00759     for(int r = 0; r < 3; r ++ )
00760             for(int c = 0; c < 4; c ++ )
00761                 matrix_4x4[r][c] = (float)matrix[r][c];
00762     
00763     matrix_4x4[3][0] = 0; matrix_4x4[3][1] = 0;
00764     matrix_4x4[3][2] = 0; matrix_4x4[3][3] = 1;
00765     
00766     MathUtils::Matrix4x4 matrix_4x4_corrected;
00767  
00768     // fix translation
00769     MathUtils::matrixMultiply(MathUtils::matrix4x4_flipY,matrix_4x4,matrix_4x4_corrected);
00770     
00771     MathUtils::Vector3 euler_angles;
00772     MathUtils::MatrixToEuler(euler_angles,matrix_4x4);
00773 
00774     MathUtils::eulerToQuaternion(-euler_angles[Q_Z],euler_angles[Q_Y],-euler_angles[Q_X], event.getOrientation);
00775 
00776     event.getPosition()[0] = (float)matrix_4x4_corrected[0][3];
00777     event.getPosition()[1] = (float)matrix_4x4_corrected[1][3];
00778     event.getPosition()[2] = (float)matrix_4x4_corrected[2][3];
00779     //  -----------------------------------------------------------
00780 #else
00781     //  --- DO NOT correct ARToolkit's vertical image mirroring ---
00782 
00783     float m[3][3];
00784     for( int r = 0; r < 3; r ++ )
00785     {
00786             for( int s = 0; s < 3; s ++ )
00787             {
00788                 m[r][s] = (float)matrix[r][s];
00789             }
00790     }
00791     MathUtils::matrixToQuaternion( m, event.getOrientation() );
00792     event.getPosition()[0] = (float)matrix[0][3];
00793     event.getPosition()[1] = (float)matrix[1][3];
00794     event.getPosition()[2] = (float)matrix[2][3];
00795     //  -----------------------------------------------------------
00796 #endif
00797     event.timeStamp();
00798     }
00799 
00800     void ARToolKitPlusModule::updateSingleMarkerSource(Node *node, float cf, ARFloat matrix[3][4])
00801     {
00802     ARToolKitSource* source = (ARToolKitSource*)node;
00803 
00804     Event & event = source->buffer;
00805     event.setConfidence(cf);
00806     updateEvent(event,matrix);
00807     source->modified = 1;
00808     }
00809 
00810     void ARToolKitPlusModule::updateMultiMarkerSource(Node *node, float cf, ARFloat matrix[3][4])
00811     {
00812     ARToolKitMultiMarkerSource* source = (ARToolKitMultiMarkerSource*)node;
00813 
00814     Event & event = source->buffer;
00815     event.setConfidence(cf);
00816     updateEvent(event,matrix);
00817     source->modified = 1;
00818     }
00819 
00820 
00821     const char*
00822     ARToolKitPlusModule::getARToolKitPlusDescription() const
00823     {
00824     const char* descr = tracker->getDescription();
00825     return descr;
00826     }
00827 
00828 
00829     bool newVideoFrame(const unsigned char* image, int width, int height, PIXEL_FORMAT format);
00830 
00831 
00832 } //namespace ot
00833 
00834 
00835 #else
00836 #pragma message(">>> no ARToolKitPlus support")
00837 #endif //USE_ARTOOLKITPLUS
00838 
00839 /* 
00840  * ------------------------------------------------------------
00841  *   End of ARToolKitPlusModule.cxx
00842  * ------------------------------------------------------------
00843  *   Automatic Emacs configuration follows.
00844  *   Local Variables:
00845  *   mode:c++
00846  *   c-basic-offset: 4
00847  *   eval: (c-set-offset 'substatement-open 0)
00848  *   eval: (c-set-offset 'case-label '+)
00849  *   eval: (c-set-offset 'statement 'c-lineup-runin-statements)
00850  *   eval: (setq indent-tabs-mode nil)
00851  *   End:
00852  * ------------------------------------------------------------ 
00853  */

copyright (c) 2006 Graz University of Technology