Beobot2_GistSalLocalizerMaster.C

Go to the documentation of this file.
00001 /*!@file                                                                */
00002 // //////////////////////////////////////////////////////////////////// //
00003 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00004 // University of Southern California (USC) and the iLab at USC.         //
00005 // See http://iLab.usc.edu for information about this project.          //
00006 // //////////////////////////////////////////////////////////////////// //
00007 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00008 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00009 // in Visual Environments, and Applications'' by Christof Koch and      //
00010 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00011 // pending; application number 09/912,225 filed July 23, 2001; see      //
00012 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00013 // //////////////////////////////////////////////////////////////////// //
00014 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00015 //                                                                      //
00016 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00017 // redistribute it and/or modify it under the terms of the GNU General  //
00018 // Public License as published by the Free Software Foundation; either  //
00019 // version 2 of the License, or (at your option) any later version.     //
00020 //                                                                      //
00021 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00022 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00023 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00024 // PURPOSE.  See the GNU General Public License for more details.       //
00025 //                                                                      //
00026 // You should have received a copy of the GNU General Public License    //
00027 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00028 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00029 // Boston, MA 02111-1307 USA.                                           //
00030 // //////////////////////////////////////////////////////////////////// //
00031 //
00032 // Primary maintainer for this file: Christian Siagian <siagian@usc.edu>
00033 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Robots/Beobot2/Localization/Beobot2_GistSalLocalizerMaster.C $
00034 // $Id: Beobot2_GistSalLocalizerMaster.C 13085 2010-03-30 03:21:24Z siagian $
00035 //
00036 
00037 #include "Robots/Beobot2/Localization/Beobot2_GistSalLocalizerMaster.H"
00038 
00039 #include "Image/MathOps.H"      // for inPlaceNormalize()
00040 #include "Image/CutPaste.H"     // for inplacePaste()
00041 #include "Image/DrawOps.H"      // for drawing
00042 #include "Image/ShapeOps.H"     // for decXY()
00043 #include "Image/MatrixOps.H"    // for matrixMult(), matrixInv(), etc
00044 
00045 #include "Beobot/beobot-GSnav-def.H"
00046 #include "Beobot/GSnavResult.H"
00047 
00048 
00049 #ifndef BEOBOT2_GISTSALLOCALIZERMASTERI_C
00050 #define BEOBOT2_GISTSALLOCALIZERMASTERI_C
00051 
00052 // number of particles used
00053 #define NUM_PARTICLES          100
00054 
00055 // maximum allowable localization error (in unit map)
00056 #define MAX_LOC_ERROR          5.0
00057 
00058 // standard deviation for odometry error (in feet)
00059 #define STD_ODO_ERROR          0.02
00060 
00061 // standard deviation for length traveled error (in ltrav [0.0 ... 1.0])
00062 #define STD_LTRAV_ERROR        0.02
00063 
00064 #define GIST_PRIORITY_WEIGHT   0.5
00065 #define SAL_PRIORITY_WEIGHT    0.2
00066 #define LOCN_PRIORITY_WEIGHT   0.3
00067 
00068 //! feed in the gist file and current ground truth
00069 Image<double>
00070 hack(uint fNum, uint &snumGT, float &ltravGT, float &dx, float &dy);
00071 
00072 
00073 
00074 // ######################################################################
00075 Beobot2_GistSalLocalizerMasterI::Beobot2_GistSalLocalizerMasterI
00076 (OptionManager& mgr,const std::string& descrName, const std::string& tagName) :
00077   RobotBrainComponent(mgr, descrName, tagName),
00078   itsSegmentBeliefHistogram(new Histogram()),
00079   itsSearchTimer(1000000),
00080   itsAbort(false),
00081   itsInputFnum(-1),
00082   itsSearchInputFnum(-1)
00083 {
00084   // default start and ground truth location:
00085   //  the two formats are not the same
00086   itsSegmentLocation       = 0;
00087   itsSegmentLengthTraveled = 0.0F;
00088   itsLocation              = Point2D<int>(-1,-1);
00089   itsSnumGT                = 0;
00090   itsLtravGT               = 0.0;
00091 
00092   itsTimes.clear();
00093 
00094   itsNumWorkers     = 1;
00095   itsNumBusyWorkers = 0;
00096 
00097   itsSavePrefix = "";
00098 
00099   itsStopSent = false;
00100 }
00101 
00102 // ######################################################################
00103 void Beobot2_GistSalLocalizerMasterI::start1()
00104 {
00105   uint w = 160; uint h = 120;
00106   itsResultWin.reset
00107     (new XWinManaged(Dims(5*w, 3*h), 0, h+30, "Result Window" ));
00108   //itsInputWin.reset
00109   //  (new XWinManaged(Dims(w, h), 5*w+30, h+30, "Input Window" ));
00110 }
00111 
00112 // ######################################################################
00113 Beobot2_GistSalLocalizerMasterI::~Beobot2_GistSalLocalizerMasterI()
00114 { }
00115 
00116 // ######################################################################
00117 void Beobot2_GistSalLocalizerMasterI::registerTopics()
00118 {
00119   this->registerSubscription("GistSalMessageTopic");
00120   this->registerSubscription("LandmarkMatchResultMessageTopic");
00121   this->registerSubscription("LandmarkSearchStatMessageTopic");
00122   this->registerSubscription("SearchDoneMessageTopic");
00123   this->registerSubscription("AbortMessageTopic");
00124 
00125   this->registerPublisher("LandmarkSearchQueueMessageTopic");
00126   this->registerPublisher("CancelSearchMessageTopic");
00127   this->registerPublisher("NextFrameMessageTopic");
00128   this->registerPublisher("LandmarkTrackMessageTopic");
00129   this->registerPublisher("CurrentLocationMessageTopic");
00130   this->registerPublisher("LandmarkDBSearchResultMessageTopic");
00131 }
00132 
00133 // ######################################################################
00134 void Beobot2_GistSalLocalizerMasterI::evolve()
00135 { }
00136 
00137 // ######################################################################
00138 void Beobot2_GistSalLocalizerMasterI::updateMessage
00139 (const RobotSimEvents::EventMessagePtr& eMsg, const Ice::Current&)
00140 {
00141   Timer timer(1000000);
00142 
00143   // Get a gist-sal message
00144   if(eMsg->ice_isA("::BeobotEvents::GistSalMessage"))
00145   {
00146     its_fnum_mutex.lock();
00147     int inputFnum       = itsInputFnum;
00148     its_fnum_mutex.unlock();
00149 
00150     // process previous results
00151     if(inputFnum != -1)
00152       {
00153         // update the belief using individual object
00154         // NOTE: this is for individual object updating
00155         LDEBUG("Saving belief[%d]", inputFnum);
00156         its_particles_mutex.lock();
00157         its_results_mutex.lock();
00158 
00159         objectUpdateBelief();
00160         updateBelief();
00161 
00162         // send current location message right away
00163         BeobotEvents::CurrentLocationMessagePtr clmsg =
00164           new BeobotEvents::CurrentLocationMessage;
00165         clmsg->RequestID = inputFnum;
00166         clmsg->segNum    = itsSegmentLocation;
00167         clmsg->lenTrav   = itsSegmentLengthTraveled;
00168         LINFO("Publishing CurrentLocationMessage with ID: %d [%d,%f]",
00169               inputFnum, clmsg->segNum, clmsg->lenTrav);
00170         publish("CurrentLocationMessageTopic", clmsg);
00171 
00172         // TIME: takes 10 - 20 ms
00173         saveLocalizerResults();
00174 
00175         its_results_mutex.unlock();
00176         its_particles_mutex.unlock();
00177       }
00178 
00179     BeobotEvents::GistSalMessagePtr gistSalMsg =
00180       BeobotEvents::GistSalMessagePtr::dynamicCast(eMsg);
00181 
00182     // Get the current request ID
00183     int currRequestID = gistSalMsg->RequestID;
00184 
00185     // TIME: takes 4ms
00186     //Image<PixRGB<byte> > currImg = 
00187     //  Ice2Image<PixRGB<byte> >(gistSalMsg->currIma);  
00188     //itsInputWin->drawImage(currImg,0,0);
00189 
00190     its_fnum_mutex.lock();
00191     itsInputFnum = currRequestID;
00192     its_fnum_mutex.unlock();
00193     LINFO("Got a GSMessage with Request ID = %d", currRequestID);
00194 
00195     // get the gist
00196     its_gist_mutex.lock();
00197     uint gsize = gistSalMsg->gistFeatures.size();
00198     itsInputGist.resize(1, gsize, NO_INIT);
00199     Image<double>::iterator aptr = itsInputGist.beginw();
00200     for(uint i = 0; i < gsize; i++)
00201       *aptr++ = gistSalMsg->gistFeatures[i];
00202 
00203     // TIME: takes 0.4ms
00204     // calculate the segment prediction
00205     itsSegmentHistogram = itsEnvironment->classifySegNum(itsInputGist);
00206     its_gist_mutex.unlock();
00207 
00208     // FIX: HACK for ground truth and robot movement
00209     //itsInputGist;
00210     hack(currRequestID, itsSnumGT, itsLtravGT,
00211          itsRobotDx, itsRobotDy);
00212 
00213     // apply action model and segment observation model
00214     its_particles_mutex.lock();
00215     //its_gist_mutex.unlock();
00216     actionUpdateBelief();
00217     segmentUpdateBelief();
00218 
00219     //its_gist_mutex.unlock();
00220     its_particles_mutex.unlock();
00221 
00222     its_fnum_mutex.lock();
00223     inputFnum           = itsInputFnum;
00224     int searchInputFnum = itsSearchInputFnum;
00225     its_fnum_mutex.unlock();
00226 
00227     // if timer goes past the time limit and robot is moving
00228     // FIX: maybe we still discard search even if robot is stationary
00229     if((inputFnum - searchInputFnum) > SEARCH_TIME_LIMIT &&
00230        !itsStopSent)
00231       {
00232         LINFO("%d STOP SEARCH", inputFnum);
00233         BeobotEvents::CancelSearchMessagePtr msg =
00234           new BeobotEvents::CancelSearchMessage;
00235 
00236         LINFO("Publishing CancelSearchMessage");
00237         publish("CancelSearchMessageTopic", msg);
00238 
00239         // sleep for 10 ms
00240         usleep(10000);
00241 
00242         itsStopSent = true;
00243       }
00244 
00245     // check if all workers are not running
00246     its_num_busy_workers_mutex.lock();
00247     bool noBusyWorkers = (itsNumBusyWorkers == 0);
00248     its_num_busy_workers_mutex.unlock();
00249 
00250     // send tracker info right away
00251     BeobotEvents::LandmarkTrackMessagePtr tmsg =
00252       new BeobotEvents::LandmarkTrackMessage;
00253     tmsg->currIma         = gistSalMsg->currIma;
00254     tmsg->conspicuityMaps = gistSalMsg->conspicuityMaps;
00255     tmsg->salientRegions  = gistSalMsg->salientRegions;
00256     tmsg->RequestID       = currRequestID;
00257     tmsg->resetTracker    = noBusyWorkers;
00258     LINFO("Publishing ltrackMessage with ID: %d at %f ms",
00259           inputFnum, timer.get()/1000.0);
00260     publish("LandmarkTrackMessageTopic", tmsg);
00261 
00262     if(noBusyWorkers)
00263       {
00264         itsStopSent = false;
00265 
00266         itsSearchTimer.reset();
00267         its_num_busy_workers_mutex.lock();
00268         itsNumBusyWorkers    = itsNumWorkers;
00269         its_num_busy_workers_mutex.unlock();
00270 
00271         its_fnum_mutex.lock();
00272         itsSearchInputFnum = currRequestID;
00273         its_fnum_mutex.unlock();
00274         LINFO("[%6d] NEW salregs", currRequestID);
00275 
00276         BeobotEvents::LandmarkSearchQueueMessagePtr smsg =
00277           new BeobotEvents::LandmarkSearchQueueMessage;
00278 
00279         // get the input image
00280         its_input_info_mutex.lock();
00281         itsInputImage = Ice2Image<PixRGB<byte> >(gistSalMsg->currIma);
00282 
00283         if((itsResultWin->getDims().w()/5) != itsInputImage.getWidth() ||
00284            (itsResultWin->getDims().h()/3) != itsInputImage.getHeight()  )
00285           itsResultWin->setDims
00286             (Dims(5*itsInputImage.getWidth(), 3*itsInputImage.getHeight()));
00287 
00288         // pass the image as well
00289         smsg->currIma = gistSalMsg->currIma;
00290 
00291         // get the salient region information
00292         itsInputVO.clear();
00293         itsInputObjOffset.clear();
00294         uint inputSize = gistSalMsg->salientRegions.size();
00295         for(uint i = 0; i < inputSize; i++)
00296           {
00297             BeobotEvents::SalientRegion salReg = gistSalMsg->salientRegions[i];
00298             LDEBUG("M[%4d] sp[%4d,%4d] rect[%4d,%4d,%4d,%4d]",
00299                    i, salReg.salpt.i, salReg.salpt.j,
00300                    salReg.objRect.tl.i, salReg.objRect.tl.j,
00301                    salReg.objRect.br.i, salReg.objRect.br.j);
00302 
00303             // print the pre-attentive feature vector
00304             std::vector<float> features;
00305             uint fsize = salReg.salFeatures.size();
00306             for(uint j = 0; j < fsize; j++)
00307               features.push_back(salReg.salFeatures[j]);
00308 
00309             Point2D<int> salpt(salReg.salpt.i, salReg.salpt.j);
00310             Point2D<int> offset( salReg.objRect.tl.i, salReg.objRect.tl.j);
00311             Rectangle rect = Rectangle::tlbrO
00312               (salReg.objRect.tl.j, salReg.objRect.tl.i,
00313                salReg.objRect.br.j, salReg.objRect.br.i);
00314             itsInputObjOffset.push_back(offset);
00315 
00316             // create a visual object for the salient region
00317             Image<PixRGB<byte> > objImg = crop(itsInputImage, rect);
00318 
00319             std::string testRunFPrefix("testRunFPrefix");
00320             std::string iname("iname");
00321             std::string saveFilePath("saveFilePath");
00322 
00323             std::string
00324               iName(sformat("%s_SAL_%07d_%02d",
00325                             testRunFPrefix.c_str(), currRequestID, i));
00326             std::string ifName = iName + std::string(".png");
00327             ifName = saveFilePath + ifName;
00328             rutz::shared_ptr<VisualObject>
00329               vo(new VisualObject
00330                  (iName, ifName, objImg, salpt - offset, features,
00331                   std::vector< rutz::shared_ptr<Keypoint> >(), false, false));
00332             itsInputVO.push_back(vo);
00333 
00334             LDEBUG("[%d] image[%d]: %s sal:[%d,%d] offset:[%d,%d]",
00335                    currRequestID, i, iName.c_str(),
00336                    (salpt - offset).i, (salpt - offset).j,
00337                    offset.i, offset.j);
00338 
00339             // Push the salient region onto the job queue
00340             smsg->salientRegions.push_back(salReg);
00341           }
00342         its_input_info_mutex.unlock();
00343 
00344         // call the search priority function
00345         its_job_queue_mutex.lock();
00346         //its_input_info_mutex.lock();
00347         itsJobQueue.clear();
00348         setSearchPriority();
00349         //its_input_info_mutex.unlock();
00350 
00351         // we will prioritize using saliency in the search loop
00352         // FIX: funky project forward stuff
00353 
00354         // store the jobs to the message
00355         std::list<GSlocJobData>::iterator itr = itsJobQueue.begin();
00356         uint count = 0;
00357         while (itr != itsJobQueue.end())
00358           {
00359             // Create a job queue
00360             BeobotEvents::LandmarkSearchJob tempJob;
00361             tempJob.inputSalRegID  = (*itr).objNum;
00362             tempJob.dbSegNum       = (*itr).segNum;
00363             tempJob.dbLmkNum       = (*itr).lmkNum;
00364             tempJob.dbVOStart      = (*itr).voStartNum;
00365             tempJob.dbVOEnd        = (*itr).voEndNum;
00366 
00367             //LINFO("[%5d] match obj[%d] lDB[%3d][%3d]:[%3d,%3d]", count,
00368             //       (*itr).objNum, (*itr).segNum, (*itr).lmkNum,
00369             //       (*itr).voStartNum,(*itr).voEndNum);
00370 
00371             smsg->jobs.push_back(tempJob);
00372             itr++; count++;
00373           }
00374         its_job_queue_mutex.unlock();
00375 
00376         // resize the result storage and information for when to quit
00377         its_results_mutex.lock();
00378         itsMatchFound.clear();
00379         itsVOmatchImage.clear();    itsVOmatchImage.resize(inputSize);
00380         itsLmkMatch.clear();        itsLmkMatch.resize(inputSize);
00381         itsSegNumMatch.clear();     itsSegNumMatch.resize(inputSize);
00382         itsLmkNumMatch.clear();     itsLmkNumMatch.resize(inputSize);
00383         itsVobNumMatch.clear();     itsVobNumMatch.resize(inputSize);
00384         itsLenTravMatch.clear();    itsLenTravMatch.resize(inputSize);
00385         itsNumObjectSearch.clear(); itsNumObjectSearch.resize(inputSize);
00386         for(uint i = 0; i < inputSize; i++) itsMatchFound.push_back(false);
00387         for(uint i = 0; i < inputSize; i++) itsNumObjectSearch[i] = 0;
00388 
00389         itsNumJobs           = count;
00390         itsNumJobsProcessed  = 0;
00391         itsLastSuccessfulJob = 0;
00392         itsNumObjectFound    = 0;
00393         its_results_mutex.unlock();
00394         smsg->RequestID = currRequestID;
00395 
00396         LINFO("Publishing lsqMessage[%d] with ID: %d", count, currRequestID);
00397         publish("LandmarkSearchQueueMessageTopic", smsg);
00398       }
00399 
00400       float time = timer.get()/1000.0F;
00401       LINFO("Time[%6d]: %f\n", currRequestID, time);
00402   }
00403 
00404   // Get a landmark match results
00405   else if(eMsg->ice_isA("::BeobotEvents::LandmarkMatchResultMessage"))
00406   {
00407     BeobotEvents::LandmarkMatchResultMessagePtr lmrMsg =
00408       BeobotEvents::LandmarkMatchResultMessagePtr::dynamicCast(eMsg);
00409 
00410     //Get the current request ID
00411     int currRequestID = lmrMsg->RequestID;
00412 
00413     BeobotEvents::LandmarkSearchJob tempJob = lmrMsg->matchInfo;
00414     LINFO("Got an lmrMessage");
00415 
00416     // matched landmark identity
00417     LINFO("-> found match[%d]: with itsLandmarkDB[%d][%d]",
00418           tempJob.inputSalRegID, tempJob.dbSegNum, tempJob.dbLmkNum);
00419 
00420     // location of the salient region found
00421     // used to estimate robot location
00422     its_results_mutex.lock();
00423     if(!itsMatchFound[tempJob.inputSalRegID])
00424       itsNumObjectFound++;
00425     itsMatchFound[tempJob.inputSalRegID]   = true;
00426     itsSegNumMatch[tempJob.inputSalRegID]  = tempJob.dbSegNum;
00427     itsLmkNumMatch[tempJob.inputSalRegID]  = tempJob.dbLmkNum;
00428     itsVobNumMatch[tempJob.inputSalRegID]  = tempJob.dbVOStart;
00429     itsLenTravMatch[tempJob.inputSalRegID] = lmrMsg->lenTravMatch;
00430     itsVOmatchImage[tempJob.inputSalRegID] =
00431       Ice2Image<PixRGB<byte> >(lmrMsg->voMatchImage);
00432     its_results_mutex.unlock();
00433 
00434     float time = timer.get()/1000.0F;
00435     LINFO("::BeobotEvents::LandmarkMatchResultMessage Time[%6d]: %f\n", 
00436           currRequestID, time);
00437   }
00438 
00439   // Got a landmark search stat
00440   else if(eMsg->ice_isA("::BeobotEvents::LandmarkSearchStatMessage"))
00441   {
00442     BeobotEvents::LandmarkSearchStatMessagePtr lssMsg =
00443       BeobotEvents::LandmarkSearchStatMessagePtr::dynamicCast(eMsg);
00444 
00445     // Get the current request ID
00446     //int currRequestID = lssMsg->RequestID;
00447 
00448     LDEBUG("Got an lssMessage with Request ID = %d [%d,%d]",
00449            lssMsg->RequestID, lssMsg->inputSalRegID, lssMsg->numObjSearch);
00450 
00451     // update
00452     its_results_mutex.lock();
00453     itsNumObjectSearch[lssMsg->inputSalRegID] += lssMsg->numObjSearch;
00454     if(lssMsg->found) itsLastSuccessfulJob = itsNumJobsProcessed;
00455     itsNumJobsProcessed++;
00456 
00457     // update last successful job count
00458     int dlast = itsNumJobsProcessed - itsLastSuccessfulJob;
00459 
00460       // stop after matching 3 of 5
00461       // check if the number of matches since last successful one
00462       //   is bigger than a percentage of the queue size
00463     bool earlyExit =
00464       itsNumObjectFound > 2 ||
00465         (itsNumObjectFound == 2 && dlast > (int)(0.05 * itsNumJobs)) ||
00466         (itsNumObjectFound == 1 && dlast > (int)(0.10 * itsNumJobs)) ||
00467         (itsNumObjectFound == 0 && dlast > (int)(0.30 * itsNumJobs));
00468     its_results_mutex.unlock();
00469 
00470     its_job_queue_mutex.lock();
00471     uint njobs = itsJobQueue.size();
00472     if(njobs > 0 && earlyExit)
00473       {
00474         its_results_mutex.lock();
00475         LINFO("EE: %d [found: %d, dlast: %d] clear: %d,"
00476               " jobs processed: %d/%d = %f",
00477               earlyExit, itsNumObjectFound, dlast, njobs,
00478               itsNumJobsProcessed, itsNumJobs,
00479               (float)itsNumJobsProcessed/itsNumJobs);
00480         its_results_mutex.unlock();
00481 
00482         // send clear jobs message
00483         itsJobQueue.clear();
00484         BeobotEvents::CancelSearchMessagePtr msg =
00485           new BeobotEvents::CancelSearchMessage;
00486 
00487         LINFO("Publishing CancelSearchMessage");
00488         publish("CancelSearchMessageTopic", msg);
00489       }
00490     its_job_queue_mutex.unlock();
00491   }
00492 
00493   // Got a search-is-done message
00494   else if(eMsg->ice_isA("::BeobotEvents::SearchDoneMessage"))
00495   {
00496     // check how many workers are done
00497     its_num_busy_workers_mutex.lock();
00498     if(itsNumBusyWorkers > 0) itsNumBusyWorkers--;
00499     bool noBusyWorkers = (itsNumBusyWorkers == 0);
00500     its_num_busy_workers_mutex.unlock();
00501     if(noBusyWorkers)
00502       {
00503         // store the search time
00504         float loopTime = itsSearchTimer.get()/1000.0F;
00505         itsTimes.push_back(loopTime);
00506 
00507         its_fnum_mutex.lock();
00508         int inputFnum       = itsInputFnum;
00509         int searchInputFnum = itsSearchInputFnum;
00510         its_fnum_mutex.unlock();
00511 
00512         LINFO("[%5d] Time: %f", inputFnum, loopTime);
00513 
00514         BeobotEvents::NextFrameMessagePtr msg =
00515           new BeobotEvents::NextFrameMessage;
00516 
00517         LINFO("Publishing NextFrameMessage");
00518         publish("NextFrameMessageTopic", msg);
00519 
00520         // inform navigation which landmarks are identified
00521         // to be used for navigation
00522         BeobotEvents::LandmarkDBSearchResultMessagePtr lmsg =
00523           new BeobotEvents::LandmarkDBSearchResultMessage;
00524         lmsg->RequestID = searchInputFnum;
00525 
00526         // send the found matches to tracker for movement
00527         its_results_mutex.lock();
00528         for(uint i = 0; i < itsMatchFound.size(); i++)
00529           {
00530             if(itsMatchFound[i])
00531               {
00532                 BeobotEvents::LandmarkSearchJob match;
00533                 match.inputSalRegID = i;
00534                 match.dbSegNum      = itsSegNumMatch[i];
00535                 match.dbLmkNum      = itsLmkNumMatch[i];
00536                 match.dbVOStart     = itsVobNumMatch[i];
00537                 match.dbVOEnd       = itsVobNumMatch[i];
00538                 lmsg->matches.push_back(match);
00539 
00540                 LDEBUG("sending [%d - %d %d - %d %d]",
00541                        match.inputSalRegID,
00542                        match.dbSegNum, match.dbLmkNum,
00543                        match.dbVOStart, match.dbVOEnd);
00544               }
00545           }
00546         its_results_mutex.unlock();
00547 
00548         LINFO("[%3d] Publishing LandmarkDBSearchResultMessage",
00549               lmsg->RequestID );
00550         publish("LandmarkDBSearchResultMessageTopic", lmsg);
00551 
00552         // display the search loop statistics
00553         if(itsAbort)
00554           {
00555             double meanTime, minTime, maxTime, stdevTime;
00556             Image<double> t1(1, itsTimes.size(),NO_INIT);
00557             for(uint i = 0; i < itsTimes.size(); i++)
00558               t1.setVal(0, i, double(itsTimes[i]));
00559 
00560             meanTime = mean(t1);
00561             getMinMax(t1, minTime, maxTime);
00562             stdevTime = stdev(t1);
00563 
00564             LINFO("Time: %f (%f - %f) std: %f",
00565                   meanTime, minTime, maxTime, stdevTime);
00566           }
00567       }
00568   }
00569 
00570   // Got an abort message
00571   else if(eMsg->ice_isA("::BeobotEvents::AbortMessage"))
00572   {
00573     itsAbort = true;
00574 
00575     // get and store robot location & error, and loop time
00576     its_particles_mutex.lock();
00577     its_results_mutex.lock();
00578     objectUpdateBelief();
00579 
00580     // update the belief using all the found salient regions
00581     updateBelief();
00582 
00583     saveLocalizerResults();
00584     its_results_mutex.unlock();
00585     its_particles_mutex.unlock();
00586 
00587     // send clear jobs message
00588     its_job_queue_mutex.lock();
00589     itsJobQueue.clear();
00590     its_job_queue_mutex.unlock();
00591     BeobotEvents::CancelSearchMessagePtr msg =
00592       new BeobotEvents::CancelSearchMessage;
00593 
00594     LINFO("Publishing CancelSearchMessage");
00595     publish("CancelSearchMessageTopic", msg);
00596 
00597     // print the results summary
00598     GSnavResult r1;
00599     r1.read(itsSavePrefix, itsTopologicalMap->getSegmentNum());
00600     r1.createSummaryResult();
00601 
00602     sleep(5);
00603     exit(0);
00604   }
00605 
00606   // this is for checking if we need to reset search for the workers
00607   // for failed tracking or time is up
00608 }
00609 
00610 // ######################################################################
00611 void Beobot2_GistSalLocalizerMasterI::setEnvironment
00612 (rutz::shared_ptr<Environment> env)
00613 {
00614   itsEnvironment = env;
00615 
00616   //! from its environment: topological map
00617   itsTopologicalMap = env->getTopologicalMap();
00618 
00619   //! from its environment: visual landmark database
00620   itsLandmarkDB = env->getLandmarkDB();
00621 }
00622 
00623 // ######################################################################
00624 void Beobot2_GistSalLocalizerMasterI::setNumWorkers(uint numWorkers)
00625 {
00626   itsNumWorkers = numWorkers;
00627 }
00628 
00629 // ######################################################################
00630 void Beobot2_GistSalLocalizerMasterI::initParticles(std::string belFName)
00631 {
00632   uint nsegment = itsTopologicalMap->getSegmentNum();
00633   itsSegmentBeliefHistogram->resize(nsegment);
00634   LINFO("number of segment : %d", nsegment);
00635 
00636   itsBeliefParticles.clear();
00637   itsBeliefLocations.clear();
00638 
00639   // check if the file does not exist or it's a blank entry
00640   FILE *fp; if((fp = fopen(belFName.c_str(),"rb")) == NULL)
00641     {
00642       LINFO("Belief file %s not found", belFName.c_str());
00643       LINFO("create random particles");
00644 
00645       // create initial random particles
00646       for(uint i = 0; i < NUM_PARTICLES; i++)
00647         {
00648           float t  = rand()/(RAND_MAX + 1.0);
00649           float t2 = rand()/(RAND_MAX + 1.0);
00650 
00651           uint  snum  = uint ((0)    + ((nsegment) * t ));
00652           float ltrav = float((0.0F) + ((1.0F    ) * t2));
00653           itsBeliefParticles.push_back(GSparticle(snum, ltrav));
00654         }
00655     }
00656   else
00657     {
00658       LINFO("Belief file %s found", belFName.c_str());
00659 
00660       // get the particles
00661       for(uint i = 0; i < NUM_PARTICLES; i++)
00662         {
00663           char inLine[200]; fgets(inLine, 200, fp);
00664           uint snum; float ltrav;
00665           sscanf(inLine, "%d %f", &snum, &ltrav);
00666           itsBeliefParticles.push_back(GSparticle(snum, ltrav));
00667         }
00668       Raster::waitForKey();
00669     }
00670 
00671   // fill in the locations as well
00672   for(uint i = 0; i < NUM_PARTICLES; i++)
00673     {
00674       uint  snum  = itsBeliefParticles[i].segnum;
00675       float ltrav = itsBeliefParticles[i].lentrav;
00676 
00677       // convert to Point2D<int>
00678       Point2D<int> loc = itsTopologicalMap->getLocation(snum, ltrav);
00679       itsBeliefLocations.push_back(loc);
00680 
00681       LDEBUG("particle[%4u]: (%3u, %10.6f) = (%4d %4d)",
00682              i, snum, ltrav, loc.i, loc.j);
00683     }
00684 }
00685 
00686 // // ######################################################################
00687 // std::vector<GSparticle> Beobot2_GistSalLocalizerMasterI::getBeliefParticles()
00688 // {
00689 //   std::vector<GSparticle> beliefParticles(itsBeliefParticles.size());
00690 
00691 //   pthread_mutex_lock(&particleLock);
00692 //   for(uint i = 0; i < itsBeliefParticles.size(); i++)
00693 //     {
00694 //       beliefParticles[i] =
00695 //         GSparticle(itsBeliefParticles[i].segnum,
00696 //                    itsBeliefParticles[i].lentrav);
00697 //     }
00698 //   pthread_mutex_unlock(&particleLock);
00699 
00700 //   return beliefParticles;
00701 // }
00702 
00703 // ######################################################################
00704 //! set the search priority for landmark DB
00705 void Beobot2_GistSalLocalizerMasterI::setSearchPriority()
00706 {
00707   // search priority is:
00708   // GIST_PRIORITY_WEIGHT * segment priority +
00709   // LOCN_PRIORITY_WEIGHT * current location priority +
00710   // SAL_PRIORITY_WEIGHT * saliency priority
00711   //   (sal is done in the search loop)
00712 
00713   Timer timer(1000000);
00714   // create jobs for each landmark - object combination
00715   for(uint i = 0; i < itsEnvironment->getNumSegment(); i++)
00716     {
00717       uint nlmk = itsLandmarkDB->getNumSegLandmark(i);
00718       LDEBUG("itsLandmarkDB[%d]: %d", i, nlmk);
00719       for(uint j = 0; j < nlmk; j++)
00720         {
00721           // check each object
00722           for(uint l = 0; l < itsInputVO.size(); l++)
00723             {
00724               uint nObj = itsLandmarkDB->getLandmark(i,j)->numObjects();
00725               uint k = 0;
00726               while(k < nObj)
00727                 {
00728                   uint k2 = k + N_OBJECT_BLOCK - 1;
00729                   if(k2 > nObj-1) k2 = nObj - 1;
00730                   itsJobQueue.push_back(GSlocJobData(l, i, j, k, k2));
00731                   LDEBUG("match obj[%d] lDB[%3d][%3d]:[%3d,%3d]",
00732                          l, i,j,k,k2);
00733                   k = k2 + 1;
00734                 }
00735             }
00736         }
00737     }
00738   LINFO("setting jobs %11.5f", timer.get()/1000.0F);
00739 
00740   // FIX: GOOD LANDMARKS ARE FOUND IN MULTIPLE RUNS !
00741 
00742   // set the order of search to random values
00743   // not actually used, just for baseline to put in ICRA08
00744   //addRandomPriority();
00745 
00746   // FIX: always load the last 10 matched landmarks first
00747   // we do this by adding a value of 3.0
00748 
00749   // add the segment priority
00750   timer.reset();
00751   //its_gist_mutex.unlock();
00752   addSegmentPriority();
00753   //its_gist_mutex.unlock();
00754   LINFO("segment      %11.5f", timer.get()/1000.0F);
00755 
00756   // add the current location priority
00757   timer.reset();
00758   addLocationPriority();
00759   LINFO("location     %11.5f", timer.get()/1000.0F);
00760 
00761   // BIG NOTE:
00762   // doing saliency prioritization is slow
00763   // has to be done outside the input loop
00764 
00765   // add the saliency priority
00766   timer.reset();
00767   addSaliencyPriority();
00768   LINFO("sal prior    %11.5f", timer.get()/1000.0F);
00769 
00770   // push the jobs based on the biasing values
00771   timer.reset();
00772   itsJobQueue.sort();
00773   LINFO("sort         %11.5f", timer.get()/1000.0F);
00774 
00775   // print the priority values
00776   std::list<GSlocJobData>::iterator itr = itsJobQueue.begin();
00777   uint count = 0;
00778   while (itr != itsJobQueue.end())
00779     {
00780       LDEBUG("[%5d] pval[%3d][%3d][%3d]: %f + %f + %f = %f", count,
00781              (*itr).segNum, (*itr).lmkNum, (*itr).objNum,
00782              (*itr).segVal, (*itr).salVal, (*itr).locVal,
00783              (*itr).pVal);
00784       itr++; count++;
00785     }
00786 }
00787 
00788 // ######################################################################
00789 void Beobot2_GistSalLocalizerMasterI::addRandomPriority()
00790 {
00791   // add a random value to the priority value
00792   std::list<GSlocJobData>::iterator itr = itsJobQueue.begin();
00793   while (itr != itsJobQueue.end())
00794     {
00795       // flip the value
00796       float val = float(rand()/(RAND_MAX + 1.0));
00797       (*itr).pVal += val;
00798       itr++;
00799     }
00800 }
00801 
00802 // ######################################################################
00803 void Beobot2_GistSalLocalizerMasterI::addSegmentPriority()
00804 {
00805   // add the segment value to the priority value
00806   std::list<GSlocJobData>::iterator itr = itsJobQueue.begin();
00807   while (itr != itsJobQueue.end())
00808     {
00809       // flip the value
00810       float val = GIST_PRIORITY_WEIGHT *
00811         (1.0 - itsSegmentHistogram->getValue((*itr).segNum));
00812 
00813       (*itr).pVal += val;
00814       (*itr).segVal = val;
00815       itr++;
00816     }
00817 }
00818 
00819 // ######################################################################
00820 void Beobot2_GistSalLocalizerMasterI::addSaliencyPriority()
00821 {
00822   uint nObj = itsInputVO.size();
00823 
00824   // go through each landmark - object combination
00825   Timer timer(1000000);
00826   uint nSeg = itsEnvironment->getNumSegment();
00827   std::vector<std::vector<std::vector<float> > > salVal(nSeg);
00828   for(uint i = 0; i < itsEnvironment->getNumSegment(); i++)
00829     {
00830       uint nlmk = itsLandmarkDB->getNumSegLandmark(i);
00831       salVal[i].resize(nlmk);
00832       for(uint j = 0; j < nlmk; j++)
00833         {
00834           // check each object
00835           salVal[i][j].resize(nObj);
00836           for(uint k = 0; k < nObj; k++)
00837             {
00838               LDEBUG("sal seg[%3d] lmk[%3d] obj[%3d]", i,j,k);
00839               salVal[i][j][k] = SAL_PRIORITY_WEIGHT *
00840                 itsLandmarkDB->getLandmark(i,j)
00841                 ->matchSalientFeatures(itsInputVO[k]);
00842             }
00843 
00844           // display the database landmark object
00845           //Image<PixRGB<byte> > img = itsLandmarkDB->getLandmark(i,j)
00846           //  ->getObject(0)->getSalAndKeypointImage();
00847           //itsWin->drawImage(img, 0,0);
00848           //Raster::waitForKey();
00849         }
00850     }
00851   LDEBUG("compute saliency dist %11.5f", timer.get()/1000.0F);
00852 
00853   // add the saliency value to the priority value
00854   std::list<GSlocJobData>::iterator itr = itsJobQueue.begin();
00855   while (itr != itsJobQueue.end())
00856     {
00857       float t = (*itr).pVal;
00858       float val = salVal[(*itr).segNum][(*itr).lmkNum][(*itr).objNum];
00859       (*itr).pVal += val;
00860       (*itr).salVal = val;
00861 
00862       LDEBUG("pval[%3d][%3d][%3d]: %f + %f = %f",
00863              (*itr).segNum, (*itr).lmkNum, (*itr).objNum, t, val, (*itr).pVal);
00864       itr++;
00865     }
00866 }
00867 
00868 // ######################################################################
00869 void Beobot2_GistSalLocalizerMasterI::addLocationPriority()
00870 {
00871   // normalizer: setup weight and sigma for decision boundary
00872   Dims mDims = itsTopologicalMap->getMapDims();
00873   Point2D<int> brMap(mDims.w(), mDims.h());
00874   float mDiag = brMap.distance(Point2D<int>(0,0));
00875   float sigma = .1 * mDiag;
00876   LDEBUG("map diagonal: %f -> sigma: %f",  mDiag, sigma);
00877   LDEBUG("curr loc: %d, %f ", itsSegmentLocation, itsSegmentLengthTraveled);
00878 
00879   // get the distance to all landmarks from current belief location
00880   uint nSeg =itsEnvironment->getNumSegment();
00881   std::vector<std::vector<float> > locVal(nSeg);
00882   for(uint i = 0; i < itsEnvironment->getNumSegment(); i++)
00883     {
00884       uint nlmk = itsLandmarkDB->getNumSegLandmark(i);
00885       locVal[i].resize(nlmk);
00886       for(uint j = 0; j < nlmk; j++)
00887         {
00888           std::pair<float,float> locRange =
00889             itsLandmarkDB->getLocationRange(i,j);
00890           LDEBUG("lmk[%3d][%3d]: (%f,%f)", i,j,
00891                  locRange.first, locRange.second);
00892 
00893           // if the location is within the range of the landmark
00894           if(itsSegmentLocation == i &&
00895              itsSegmentLengthTraveled >= locRange.first &&
00896              itsSegmentLengthTraveled <= locRange.second  )
00897             {
00898               locVal[i][j] = 0.0;
00899               LDEBUG("dist[%d][%d]: within -> %f",i,j, locVal[i][j]);
00900             }
00901           else
00902             {
00903               // get distance to the first seen location
00904               float fdist = itsTopologicalMap->
00905                 getDistance(itsSegmentLocation, itsSegmentLengthTraveled,
00906                             i, locRange.first);
00907 
00908               // get distance to the last seen location
00909               float ldist = itsTopologicalMap->
00910                 getDistance(itsSegmentLocation, itsSegmentLengthTraveled,
00911                             i, locRange.second);
00912 
00913               // get the minimum distance
00914               float dist = std::min(fdist,ldist);
00915 
00916               LDEBUG("f - l: %d [%f -> %f][%f -> %f]",
00917                      i, locRange.first, fdist, locRange.second, ldist);
00918 
00919               // get distances to nodes in between
00920               std::vector<std::pair<uint,float> > betLocs =
00921                 itsTopologicalMap->getNodeLocationsInInterval
00922                 (i, locRange.first, locRange.second);
00923               for(uint k = 0; k < betLocs.size(); k++)
00924                 {
00925                   float bdist = itsTopologicalMap->
00926                     getDistance(itsSegmentLocation, itsSegmentLengthTraveled,
00927                                 i, betLocs[k].second);
00928                   if(dist > bdist) dist = bdist;
00929 
00930                   LDEBUG("bet: %d [%f -> %f]", i, betLocs[k].second, bdist);
00931                 }
00932 
00933               // normalize gaussian val to 1.0 invert the values
00934               locVal[i][j] = LOCN_PRIORITY_WEIGHT *
00935                 (1.0 - pow(M_E, -dist*dist/(2.0*sigma*sigma)));
00936               LDEBUG("dist[%d][%d]: %f ->%f",i,j, dist, locVal[i][j]);
00937             }
00938         }
00939     }
00940 
00941   // add the location value to the priority value
00942   std::list<GSlocJobData>::iterator itr = itsJobQueue.begin();
00943   while (itr != itsJobQueue.end())
00944     {
00945       float t = (*itr).pVal;
00946       float val = locVal[(*itr).segNum][(*itr).lmkNum];
00947       (*itr).pVal += val;
00948       (*itr).locVal = val;
00949 
00950       LDEBUG("pval[%3d][%3d][%3d]: %f -> %f",
00951              (*itr).segNum, (*itr).lmkNum, (*itr).objNum, t, (*itr).pVal);
00952       itr++;
00953     }
00954 }
00955 
00956 // ######################################################################
00957 rutz::shared_ptr<Histogram>
00958 Beobot2_GistSalLocalizerMasterI::getSegmentBeliefHistogram()
00959 {
00960   itsSegmentBeliefHistogram->clear();
00961 
00962   for(uint i = 0; i < NUM_PARTICLES; i++)
00963     {
00964       itsSegmentBeliefHistogram->
00965         addValue(itsBeliefParticles[i].segnum, 1.0F);
00966     }
00967 
00968   //! print the histogram profile
00969   uint nsegment = itsTopologicalMap->getSegmentNum();
00970   for(uint i = 0; i < nsegment; i++)
00971     LDEBUG("[%d]: %d", i, uint(itsSegmentBeliefHistogram->getValue(i)));
00972   return itsSegmentBeliefHistogram;
00973 }
00974 
00975 // ######################################################################
00976 void Beobot2_GistSalLocalizerMasterI::saveLocalizerResults()
00977 {
00978   // check the ground truth
00979   float xGT = -1.0F, yGT = -1.0F; float error = 0.0F;
00980   uint  snumRes = 0; float ltravRes = -1.0F;
00981 
00982   its_fnum_mutex.lock();
00983   int index = itsInputFnum;
00984   its_fnum_mutex.unlock();
00985 
00986   if(itsLtravGT != -1.0F)
00987     {
00988       itsEnvironment->getLocationFloat(itsSnumGT, itsLtravGT, xGT, yGT);
00989       snumRes  = itsSegmentLocation;
00990       ltravRes = itsSegmentLengthTraveled;
00991       error = itsTopologicalMap->
00992         getDistance(itsSnumGT, itsLtravGT, snumRes, ltravRes);
00993       LDEBUG("Ground Truth [%d %f] vs [%d %f]: error: %f",
00994              itsSnumGT, itsLtravGT, snumRes, ltravRes, error);
00995     }
00996 
00997   bool saveDisplay = true;
00998   if(saveDisplay)
00999     {
01000       // TIME: takes 10 - 20ms
01001       Image<PixRGB<byte> > dispIma(getDisplayImage());  
01002       itsResultWin->setTitle(sformat("%d", index).c_str());
01003       itsResultWin->drawImage(dispIma, 0, 0);
01004 
01005       // save it to be mpeg encoded
01006       std::string saveFName =  itsSavePrefix + sformat("_RES_%07d.ppm", index);
01007       LDEBUG("saving: %s",saveFName.c_str());
01008       //Raster::WriteRGB(dispIma,saveFName);
01009 
01010       // save the particle locations
01011       // in case we need to restart in the middle
01012       std::string belFName = itsSavePrefix + sformat("_bel_%07d.txt", index);
01013       FILE *bfp; LDEBUG("belief file: %s", belFName.c_str());
01014       if((bfp = fopen(belFName.c_str(),"wt")) == NULL)
01015         LFATAL("Belief file: %s not found", belFName.c_str());
01016       for(uint i = 0; i < itsBeliefParticles.size(); i++)
01017         {
01018           std::string bel = sformat("%d %f ",
01019                                     itsBeliefParticles[i].segnum,
01020                                     itsBeliefParticles[i].lentrav);
01021           bel += std::string("\n");
01022           fputs(bel.c_str(), bfp);
01023         }
01024       fclose (bfp);
01025     }  
01026 
01027   // save result in a file by appending to the file
01028   std::string resFName = itsSavePrefix + sformat("_results.txt");
01029   FILE *rFile = fopen(resFName.c_str(), "at");
01030   if (rFile != NULL)
01031     {
01032       LDEBUG("saving result to %s", resFName.c_str());
01033       std::string line =
01034         sformat("%5d %3d %8.5f %3d %8.5f %10.6f",
01035                 index, itsSnumGT, itsLtravGT, snumRes, ltravRes, error);
01036 
01037       its_num_busy_workers_mutex.lock();
01038       bool noBusyWorkers = (itsNumBusyWorkers == 0);
01039       its_num_busy_workers_mutex.unlock();
01040 
01041       if(noBusyWorkers)
01042         {
01043           uint ninput = itsMatchFound.size();
01044           line += sformat(" %d", ninput);
01045           for(uint i = 0; i < ninput; i++)
01046             line += sformat("%3d %6d", int(itsMatchFound[i]),
01047                             itsNumObjectSearch[i]);
01048         }
01049       else line += sformat(" 0");
01050 
01051       LINFO("%s", line.c_str());
01052       line += std::string("\n");
01053 
01054       fputs(line.c_str(), rFile);
01055       fclose (rFile);
01056     }
01057   else LINFO("can't create file: %s", resFName.c_str());
01058 }
01059 
01060 // ######################################################################
01061 void Beobot2_GistSalLocalizerMasterI::updateBelief()
01062 {
01063   // set the most likely location
01064   setLocation();
01065 }
01066 
01067 // ######################################################################
01068 void Beobot2_GistSalLocalizerMasterI::actionUpdateBelief()
01069 {
01070   std::vector<int> stotal(itsTopologicalMap->getSegmentNum());
01071   for(uint i = 0; i < stotal.size(); i++) stotal[i] = 0;
01072 
01073   // apply the motor movement + noise to each particles
01074   for(uint i = 0; i < NUM_PARTICLES; i++)
01075     {
01076       uint  snum  = itsBeliefParticles[i].segnum;
01077       float ltrav = itsBeliefParticles[i].lentrav;
01078       LDEBUG("particle[%d]: %d, %f", i, snum, ltrav);
01079 
01080       // apply the motor command
01081       float nltrav = ltrav + itsRobotDx;
01082 
01083       // assume std odometry error of .02ft
01084       float mscale = itsTopologicalMap->getMapScale();
01085       float err    = STD_ODO_ERROR/mscale;
01086 
01087       // add noise from a Gaussian distribution (Box-Muller method)
01088       float r1 = float(rand()/(RAND_MAX + 1.0));
01089       float r2 = float(rand()/(RAND_MAX + 1.0));
01090       double r = err * sqrt( -2.0 * log(r1));
01091       double phi = 2.0 * M_PI * r2;
01092       nltrav += (r * cos(phi));
01093 
01094       // if nltrav is now improper
01095       // FIX: NEED THE SPILL OVER EFFECT
01096       if(nltrav < 0.0F) nltrav = 0.0F;
01097       if(nltrav > 1.0F) nltrav = 1.0F;
01098       itsBeliefParticles[i].lentrav = nltrav;
01099       //stotal[snum]++;
01100 
01101       // convert to Point2D<int>
01102       Point2D<int> loc = itsTopologicalMap->
01103         getLocation(itsBeliefParticles[i].segnum,
01104                     itsBeliefParticles[i].lentrav);
01105       itsBeliefLocations[i] = loc;
01106     }
01107 }
01108 
01109 // ######################################################################
01110 void Beobot2_GistSalLocalizerMasterI::segmentUpdateBelief()
01111 {
01112   // accweight is for easy calculation for choosing a random particle
01113   std::vector<float> weight(NUM_PARTICLES);
01114   std::vector<float> accweight(NUM_PARTICLES);
01115 
01116   // for each particles
01117   float accw = 0.0F;
01118   for(uint i = 0; i < NUM_PARTICLES; i++)
01119     {
01120       uint  snum  = itsBeliefParticles[i].segnum;
01121       float ltrav = itsBeliefParticles[i].lentrav;
01122       LDEBUG("particle[%4d]: %d, %f", i, snum, ltrav);
01123 
01124       // get likelihood score
01125       // score(snum)/sum(score(all segments) * score(snum)
01126       // with the denominator taken out
01127       // + 0.25 for the 50% memory of previous time
01128       float pscore = itsSegmentHistogram->getValue(snum);
01129       float score  = (pscore * pscore) + 0.25f;
01130       LDEBUG("score: %f * %f = %f", pscore, pscore, score);
01131 
01132       // update weight
01133       weight[i] = score;
01134       accw += score;
01135       accweight[i] = accw;
01136     }
01137   for(uint i = 0; i < NUM_PARTICLES; i++)
01138     LDEBUG("p[%4d]: w: %f %f ",i, weight[i], accweight[i]);
01139   LDEBUG("accw: %f",accw);
01140 
01141   // add 1% noise weight
01142   accw *= 1.01F; LDEBUG("accw+ noise: %f",accw);
01143 
01144   // weighted resample NUM_PARTICLES particles
01145   std::vector<GSparticle> tbelief(NUM_PARTICLES);
01146   uint nsegment = itsTopologicalMap->getSegmentNum();
01147   std::vector<int> stotal(nsegment);
01148   for(uint i = 0; i < stotal.size(); i++) stotal[i] = 0;
01149 
01150   for(uint i = 0; i < NUM_PARTICLES; i++)
01151     {
01152       // draw a random value between 0 and accw
01153       float rval  = accw * float(rand()/(RAND_MAX + 1.0));
01154 
01155       // get the random index
01156       uint sind = NUM_PARTICLES;
01157       for(uint j = 0; j < NUM_PARTICLES; j++)
01158         if(rval < accweight[j]) { sind = j; j = NUM_PARTICLES; }
01159       LDEBUG("rval: %f -> %d", rval, sind);
01160 
01161       // if need to create a random particle
01162       if(sind == NUM_PARTICLES)
01163         {
01164           // create initial random particles
01165           float t  = rand()/(RAND_MAX + 1.0);
01166           float t2 = rand()/(RAND_MAX + 1.0);
01167 
01168           uint  snum  = uint ((0)    + ((nsegment) * t ));
01169           float ltrav = float((0.0F) + ((1.0F    ) * t2));
01170           tbelief[i] = GSparticle(snum, ltrav);
01171           stotal[snum]++;
01172           LDEBUG("rand particle[%d]: (%d, %f)", i, snum, ltrav);
01173         }
01174       else
01175         {
01176           tbelief[i] = itsBeliefParticles[sind];
01177           stotal[itsBeliefParticles[sind].segnum]++;
01178           LDEBUG("old  particle[%d]", sind);
01179         }
01180    }
01181 
01182   for(uint i = 0; i < stotal.size(); i++) LDEBUG("seg[%d]: %d",i, stotal[i]);
01183 
01184   // copy all the particles to our belief
01185   for(uint i = 0; i < NUM_PARTICLES; i++)
01186     {
01187       itsBeliefParticles[i] = tbelief[i];
01188 
01189       // convert to Point2D<int>
01190       Point2D<int> loc = itsTopologicalMap->
01191         getLocation(tbelief[i].segnum, tbelief[i].lentrav);
01192       itsBeliefLocations[i] = loc;
01193     }
01194 }
01195 
01196 // ######################################################################
01197 void Beobot2_GistSalLocalizerMasterI::objectUpdateBelief()
01198 {
01199   // make sure at least 1 object is found
01200   uint c = 0;
01201   for(uint i = 0; i < itsMatchFound.size(); i++) if(itsMatchFound[i]) c++;
01202   if(c == 0) return;
01203 
01204   // setup weight and sigma for decision boundary
01205   Dims mDims = itsTopologicalMap->getMapDims();
01206   Point2D<int> brMap(mDims.w(), mDims.h());
01207   float mDiag = brMap.distance(Point2D<int>(0,0));
01208   float sigma = .05*mDiag;
01209   LDEBUG("map diagonal: %f -> sigma: %f", mDiag, sigma);
01210 
01211   // accweight is for easy calculation for choosing a random particle
01212   std::vector<float> weight(NUM_PARTICLES);
01213   std::vector<float> accweight(NUM_PARTICLES);
01214 
01215   // for each particles
01216   float accw = 0.0F;
01217   for(uint i = 0; i < NUM_PARTICLES; i++)
01218     {
01219       uint  snum  = itsBeliefParticles[i].segnum;
01220       float ltrav = itsBeliefParticles[i].lentrav;
01221       LDEBUG("particle[%d]: %d, %f", i, snum, ltrav);
01222 
01223       // go through each object found
01224       float pObjObs = 1.0;
01225       for(uint index = 0; index < itsMatchFound.size(); index++)
01226         {
01227           if(itsMatchFound[index])
01228             {
01229               // get the location of the object found
01230               uint  snumMatch  = itsSegNumMatch[index];
01231               float ltravMatch = itsLenTravMatch[index];
01232               LDEBUG("Match[%d]: [%d %f]", index, snumMatch, ltravMatch);
01233 
01234               // get the distance between the two points
01235               float dist = itsTopologicalMap->
01236                 getDistance(snum, ltrav, snumMatch, ltravMatch);
01237 
01238               float pOMatch = 1.0/(sigma * sqrt(2.0 * M_PI)) *
01239                 pow(M_E, -dist*dist/(2.0*sigma*sigma));
01240               pObjObs *= pOMatch;
01241 
01242               LDEBUG("dist: %f -> pOMatch: %f -> %f", dist, pOMatch, pObjObs);
01243             }
01244         }
01245 
01246       // update weight
01247       weight[i] = pObjObs;
01248       accweight[i] = weight[i] + accw;
01249       accw = accweight[i];
01250     }
01251 
01252   LDEBUG("accw: %f",accw);
01253   // add 20% weight - because objects are more exacting
01254   accw *= 1.20F;
01255   LDEBUG("accw+ noise: %f",accw);
01256 
01257   // weighted resample NUM_PARTICLES particles
01258   std::vector<GSparticle> tbelief;
01259   std::vector<int> stotal(itsTopologicalMap->getSegmentNum());
01260   for(uint i = 0; i < stotal.size(); i++) stotal[i] = 0;
01261 
01262   uint nsegment = itsTopologicalMap->getSegmentNum();
01263 
01264   for(uint i = 0; i < NUM_PARTICLES; i++)
01265     LDEBUG("p[%d]: %f %f ",i, weight[i], accweight[i]);
01266 
01267   for(uint i = 0; i < NUM_PARTICLES; i++)
01268     {
01269       // draw a random value between 0 and accw
01270       float rval  = accw * float(rand()/(RAND_MAX + 1.0));
01271 
01272       // get the random index
01273       uint sind = NUM_PARTICLES;
01274       for(uint j = 0; j < NUM_PARTICLES; j++)
01275         if(rval < accweight[j]) { sind = j; j = NUM_PARTICLES; }
01276       LDEBUG("rval: %f -> %d", rval, sind);
01277 
01278       // if need to create a random particle
01279       if(sind == NUM_PARTICLES)
01280         {
01281           // create initial random particles
01282           float t  = rand()/(RAND_MAX + 1.0);
01283           float t2 = rand()/(RAND_MAX + 1.0);
01284 
01285           uint  snum  = uint ((0)    + ((nsegment) * t ));
01286           float ltrav = float((0.0F) + ((1.0F    ) * t2));
01287           tbelief.push_back(GSparticle(snum, ltrav));
01288           stotal[snum]++;
01289           LDEBUG("rand particle[%d]: (%d, %f)", i, snum, ltrav);
01290         }
01291       else
01292         {
01293           tbelief.push_back(itsBeliefParticles[sind]);
01294           stotal[itsBeliefParticles[sind].segnum]++;
01295           LDEBUG("old  particle[%d]", sind);
01296         }
01297     }
01298 
01299   for(uint i = 0; i < stotal.size(); i++) LDEBUG("[%d]: %d",i, stotal[i]);
01300 
01301   // copy all the particles to our belief
01302   for(uint i = 0; i < NUM_PARTICLES; i++)
01303     {
01304       itsBeliefParticles[i] = tbelief[i];
01305 
01306       // convert to Point2D<int>
01307       Point2D<int> loc = itsTopologicalMap->
01308         getLocation(tbelief[i].segnum, tbelief[i].lentrav);
01309       itsBeliefLocations[i] = loc;
01310     }
01311 }
01312 
01313 // ######################################################################
01314 void Beobot2_GistSalLocalizerMasterI::objectUpdateBelief(uint index)
01315 {
01316   // make sure this object is found
01317   if(!itsMatchFound[index]) return;
01318 
01319   // setup weight and sigma for decision boundary
01320   Dims mDims = itsTopologicalMap->getMapDims();
01321   Point2D<int> brMap(mDims.w(), mDims.h());
01322   float mDiag = brMap.distance(Point2D<int>(0,0));
01323   float sigma = .05*mDiag;
01324   LDEBUG("map diagonal: %f -> sigma: %f", mDiag, sigma);
01325 
01326   // accweight is for easy calculation for choosing a random particle
01327   std::vector<float> weight(NUM_PARTICLES);
01328   std::vector<float> accweight(NUM_PARTICLES);
01329 
01330   // for each particles
01331   float accw = 0.0F;
01332   for(uint i = 0; i < NUM_PARTICLES; i++)
01333     {
01334       uint  snum  = itsBeliefParticles[i].segnum;
01335       float ltrav = itsBeliefParticles[i].lentrav;
01336       LDEBUG("particle[%d]: %d, %f", i, snum, ltrav);
01337 
01338       // get the location of the object found
01339       uint  snumMatch  = itsSegNumMatch[index];
01340       float ltravMatch = itsLenTravMatch[index];
01341       LDEBUG("Match[%d]: [%d %f]", index, snumMatch, ltravMatch);
01342 
01343       // get the distance between the two points
01344       float dist = itsTopologicalMap->
01345         getDistance(snum, ltrav, snumMatch, ltravMatch);
01346 
01347       float pObjObs = 1.0/(sigma * sqrt(2.0 * M_PI)) *
01348         pow(M_E, -dist*dist/(2.0*sigma*sigma));
01349       LDEBUG("dist: %f -> %f", dist, pObjObs);
01350 
01351       // update weight
01352       weight[i] = pObjObs;
01353       accweight[i] = weight[i] + accw;
01354       accw = accweight[i];
01355     }
01356 
01357   LDEBUG("accw: %f",accw);
01358   // add 20% weight - because objects are more exacting
01359   accw *= 1.20F;
01360   LDEBUG("accw+ noise: %f",accw);
01361 
01362   // weighted resample NUM_PARTICLES particles
01363   std::vector<GSparticle> tbelief;
01364   std::vector<int> stotal(itsTopologicalMap->getSegmentNum());
01365   for(uint i = 0; i < stotal.size(); i++) stotal[i] = 0;
01366 
01367   uint nsegment = itsTopologicalMap->getSegmentNum();
01368 
01369   for(uint i = 0; i < NUM_PARTICLES; i++)
01370     LDEBUG("p[%d]: %f %f ",i, weight[i], accweight[i]);
01371 
01372   for(uint i = 0; i < NUM_PARTICLES; i++)
01373     {
01374       // draw a random value between 0 and accw
01375       float rval  = accw * float(rand()/(RAND_MAX + 1.0));
01376 
01377       // get the random index
01378       uint sind = NUM_PARTICLES;
01379       for(uint j = 0; j < NUM_PARTICLES; j++)
01380         if(rval < accweight[j]) { sind = j; j = NUM_PARTICLES; }
01381       LDEBUG("rval: %f -> %d", rval, sind);
01382 
01383       // if need to create a random particle
01384       if(sind == NUM_PARTICLES)
01385         {
01386           // create initial random particles
01387           float t  = rand()/(RAND_MAX + 1.0);
01388           float t2 = rand()/(RAND_MAX + 1.0);
01389 
01390           uint  snum  = uint ((0)    + ((nsegment) * t ));
01391           float ltrav = float((0.0F) + ((1.0F    ) * t2));
01392           tbelief.push_back(GSparticle(snum, ltrav));
01393           stotal[snum]++;
01394           LDEBUG("rand particle[%d]: (%d, %f)", i, snum, ltrav);
01395         }
01396       else
01397         {
01398           tbelief.push_back(itsBeliefParticles[sind]);
01399           stotal[itsBeliefParticles[sind].segnum]++;
01400           LDEBUG("old  particle[%d]", sind);
01401         }
01402     }
01403 
01404   for(uint i = 0; i < stotal.size(); i++) LDEBUG("[%d]: %d",i, stotal[i]);
01405 
01406   // copy all the particles to our belief
01407   for(uint i = 0; i < NUM_PARTICLES; i++)
01408     {
01409       itsBeliefParticles[i] = tbelief[i];
01410 
01411       // convert to Point2D<int>
01412       Point2D<int> loc = itsTopologicalMap->
01413         getLocation(tbelief[i].segnum, tbelief[i].lentrav);
01414       itsBeliefLocations[i] = loc;
01415     }
01416 }
01417 
01418 // ######################################################################
01419 void Beobot2_GistSalLocalizerMasterI::setLocation()
01420 {
01421   // for each point
01422   float maxscore = 0.0F;
01423   for(uint i = 0; i < itsBeliefLocations.size(); i++)
01424     {
01425       // get distances with the neighbors closer than MAX_ERROR_DIST
01426       float score = 0.0F;
01427       Point2D<int> a = itsBeliefLocations[i];
01428       uint aseg = itsBeliefParticles[i].segnum;
01429       for(uint j = 0; j < itsBeliefLocations.size(); j++)
01430         {
01431           Point2D<int> b = itsBeliefLocations[j];
01432           float dist = a.distance(b);
01433 
01434           uint bseg = itsBeliefParticles[j].segnum;
01435           float cscore = 0.0; float sthresh = MAX_LOC_ERROR/2.0;
01436           if(dist < sthresh) // (0.0  ... 2.5] -> (1.0 -> 0.8]
01437             {
01438               cscore = (1.0  - (dist - 0.0)/sthresh * 0.2);
01439             }
01440           else if(dist < sthresh*2) // 2.5 ... 5.0] -> [0.8 ... 0.2]
01441             {
01442               cscore = (0.8 - (dist - sthresh)/sthresh * 0.6);
01443             }
01444           if(aseg != bseg) cscore *= .5;
01445           score += cscore;
01446         }
01447 
01448       // update max location
01449       if(score > maxscore)
01450         {
01451           maxscore = score;
01452           itsLocation = itsBeliefLocations[i];
01453           itsSegmentLocation = itsBeliefParticles[i].segnum;
01454           itsSegmentLengthTraveled = itsBeliefParticles[i].lentrav;
01455         }
01456     }
01457 
01458   LDEBUG("max score: %f: (%d, %d) = [%d %f]", maxscore,
01459          itsLocation.i, itsLocation.j,
01460          itsSegmentLocation, itsSegmentLengthTraveled);
01461 }
01462 
01463 // ######################################################################
01464 Image<PixRGB<byte> >
01465 Beobot2_GistSalLocalizerMasterI::getSalImage
01466 ( Image<PixRGB<byte> > ima,
01467   std::vector<rutz::shared_ptr<VisualObject> > inputVO,
01468   std::vector<Point2D<int> > objOffset,
01469   std::vector<bool> found)
01470 {
01471   int w = ima.getWidth();  int h = ima.getHeight();
01472   Image<PixRGB<byte> > dispIma(w,h,ZEROS);
01473   inplacePaste(dispIma,ima, Point2D<int>(0,0));
01474 
01475   // display the salient regions
01476   // and indicate if each is found or not
01477   LDEBUG("number of input objects: %"ZU, inputVO.size());
01478   for(uint i = 0; i < inputVO.size(); i++)
01479     {
01480       Rectangle r(objOffset[i], inputVO[i]->getImage().getDims());
01481       Point2D<int> salpt = objOffset[i] + inputVO[i]->getSalPoint();
01482       if(!found[i])
01483         {
01484           drawRect(dispIma,r,PixRGB<byte>(255,0,0));
01485           drawDisk(dispIma, salpt, 3, PixRGB<byte>(255,0,0));
01486         }
01487       else
01488         {
01489           drawRect(dispIma,r,PixRGB<byte>(0,255,0));
01490           drawDisk(dispIma, salpt, 3, PixRGB<byte>(0,255,0));
01491         }
01492       LDEBUG("found: %d", int(found[i]));
01493 
01494       std::string ntext(sformat("%d", i));
01495       writeText(dispIma, objOffset[i] + inputVO[i]->getSalPoint(),
01496                 ntext.c_str());
01497     }
01498 
01499   return dispIma;
01500 }
01501 
01502 
01503 // ######################################################################
01504 Image<PixRGB<byte> >
01505 Beobot2_GistSalLocalizerMasterI::getBeliefImage(uint w, uint h, int &scale)
01506 {
01507   Image< PixRGB<byte> > res(w,h,ZEROS);
01508 
01509   // get the map from the topolagical map class
01510   Image< PixRGB<byte> > mapImg = itsTopologicalMap->getMapImage(w, h);
01511   Dims d = itsTopologicalMap->getMapDims();
01512 
01513   // add the particles on the map
01514   scale = int(mapImg.getWidth()/d.w());
01515   for(uint i = 0; i < itsBeliefParticles.size(); i++)
01516     {
01517       // get the point
01518       Point2D<int> loc = itsBeliefLocations[i] * scale;
01519       LDEBUG("point: %d %d", loc.i, loc.j);
01520 
01521       drawDisk(mapImg, loc, 2, PixRGB<byte>(0,255,255));
01522     }
01523 
01524   // draw circle to the most likely location of the object
01525   drawDisk(mapImg, itsLocation*scale, 6, PixRGB<byte>(0,0,255));
01526   drawCircle(mapImg, itsLocation*scale, int((MAX_LOC_ERROR/4)*scale),
01527              PixRGB<byte>(0,255,0), 2);
01528   drawCircle(mapImg, itsLocation*scale, int(MAX_LOC_ERROR*scale),
01529              PixRGB<byte>(0,0,255), 2);
01530 
01531   inplacePaste(res, mapImg, Point2D<int>(0,0));
01532 
01533   // get the segment belief histogram
01534   rutz::shared_ptr<Histogram> shist = getSegmentBeliefHistogram();
01535 
01536   // check which side the histogram is going to be appended to
01537   uint wslack = w - mapImg.getWidth();
01538   uint hslack = h - mapImg.getHeight();
01539   if(hslack >= wslack)
01540     {
01541       Image<byte> sHistImg =
01542         shist->getHistogramImage(w, hslack, 0.0F, float(NUM_PARTICLES));
01543       inplacePaste(res, Image<PixRGB<byte> >(sHistImg),
01544                    Point2D<int>(0,mapImg.getHeight()));
01545     }
01546   else
01547     {
01548       Image<byte> sHistImg =
01549         shist->getHistogramImage(h, wslack, 0.0F, float(NUM_PARTICLES));
01550 
01551       Image<PixRGB<byte> >
01552         t = Image<PixRGB<byte> >(flipHoriz(transpose(sHistImg)));
01553       inplacePaste(res, t, Point2D<int>(mapImg.getWidth(), 0));
01554     }
01555   return res;
01556 }
01557 
01558 
01559 // ######################################################################
01560 Image<PixRGB<byte> > Beobot2_GistSalLocalizerMasterI::getDisplayImage()
01561 {
01562   // if search is not done don't need to get those information
01563   Image<PixRGB<byte> > ima = itsInputImage;
01564   uint w = ima.getWidth(); uint h = ima.getHeight();
01565   Image<PixRGB<byte> > dispIma(5*w,3*h,ZEROS);
01566 
01567   uint ninput = itsInputVO.size();
01568   std::vector<bool> mfound(ninput);
01569   std::vector<rutz::shared_ptr<VisualObject> > iObject(ninput);
01570   std::vector<Point2D<int> > iOffset(ninput);
01571   for(uint i = 0; i < ninput; i++)
01572     {
01573       mfound[i]     = itsMatchFound[i];
01574       iObject[i]    = itsInputVO[i];
01575       iOffset[i]    = itsInputObjOffset[i];
01576     }
01577 
01578   // display the results
01579   Image<PixRGB<byte> > salIma = getSalImage(ima, iObject, iOffset, mfound);
01580   inplacePaste(dispIma, zoomXY(salIma), Point2D<int>(0,0));
01581 
01582   // display the gist histogram
01583   Image<byte> gistHistImg =
01584     itsSegmentHistogram->getHistogramImage(w*3, h, 0.0F, 1.0F);
01585   inplacePaste(dispIma, Image<PixRGB<byte> >(gistHistImg), Point2D<int>(0,2*h));
01586 
01587   // display the localization belief
01588   int scale;
01589   Image<PixRGB<byte> > beliefImg = getBeliefImage(w*2, h*3, scale);
01590 
01591   // draw the ground truth
01592   float xGT = -1.0F, yGT = -1.0F;
01593   itsEnvironment->getLocationFloat(itsSnumGT, itsLtravGT, xGT, yGT);
01594   if(itsLtravGT != -1.0F)
01595     {
01596       Point2D<int> loc(int(xGT*scale + .5), int(yGT*scale + .5));
01597       LDEBUG("Ground Truth disp %f %f -> %d %d", xGT, yGT, loc.i, loc.j);
01598       drawDisk(beliefImg,loc, 4, PixRGB<byte>(255,0,0));
01599     }
01600 
01601   // show where the objects indicate its position to be
01602   uint numObjectFound = 0;
01603   for(uint i = 0; i < ninput; i++)
01604     {
01605       if(mfound[i])
01606         {
01607           numObjectFound++;
01608           uint  snum  = itsSegNumMatch[i];
01609           float ltrav = itsLenTravMatch[i];
01610           float x, y;
01611           itsEnvironment->getLocationFloat(snum, ltrav, x, y);
01612           Point2D<int> loc(int(x*scale + .5), int(y*scale + .5));
01613           LDEBUG("obj[%d] res: %f %f -> %d %d",i, x, y, loc.i, loc.j);
01614           drawDisk(beliefImg, loc, 3, PixRGB<byte>(255,255,0));
01615         }
01616     }
01617   inplacePaste(dispIma, beliefImg, Point2D<int>(3*w,0));
01618 
01619   // display a found object found
01620   uint fcount = 0;
01621   for(uint i = 0; i < ninput; i++)
01622     {
01623       if(mfound[i] && fcount == 0)
01624         {
01625           fcount++;
01626 
01627           //display the first object match found
01628           Image<PixRGB<byte> >matchIma = itsVOmatchImage[i];
01629           std::string ntext(sformat("object[%d]", i));
01630           writeText(matchIma, Point2D<int>(0,0), ntext.c_str());
01631           inplacePaste(dispIma, matchIma, Point2D<int>(2*w,0));
01632         }
01633     }
01634   if(fcount == 0) writeText(dispIma, Point2D<int>(2*w,0),"no objects found");
01635 
01636   return dispIma;
01637 }
01638 
01639 
01640 // ######################################################################
01641 Image<double>
01642 hack(uint fNum, uint &snumGT, float &ltravGT, float &dx, float &dy)
01643 {
01644 //   // ACB IROS 07 test
01645 //   uint sfnum [9] = {  377,  496,  541,  401,  304,  555,  486,  327,  307 };
01646 //   uint ssfnum[9] = {    0,  377,  873, 1414, 1815, 2119, 2674, 3160, 3487 };
01647 
01648 //   // AnFpark IROS 07 test
01649 //   uint sfnum [9] = {  866,  569,  896,  496,  769, 1250,  588,  844,  919 };
01650 //   uint ssfnum[9] = {    0,  866, 1435, 2331, 2827, 3596, 4846, 5434, 6278 };
01651 
01652 //   // FDFpark IROS 07 test
01653 //   uint sfnum [9] = {  782,  813,  869,  795,  857, 1434,  839, 1149,  749 };
01654 //   uint ssfnum[9] = {    0,  782, 1595, 2464, 3259, 4116, 5550, 6389, 7538 };
01655 
01656   // =====================================================================
01657   // ACB_1 IEEE-TR 07 test T_A
01658   // std::string stem("../data/PAMI07/ACB/gist/ACB");
01659   // uint sfnum [9] = {  387,  440,  465,  359,  307,  556,  438,  290,  341 };
01660   // uint ssfnum[9] = {    0,  387,  827, 1292, 1651, 1958, 2514, 2952, 3242 };
01661 
01662 //   // ACB_2 IEEE-TR 07 test T_B
01663 //   std::string stem("/lab/tmpib/u/siagian/PAMI07/ACB/gist/ACB");
01664 //   uint sfnum [9] = {  410,  436,  485,  321,  337,  495,  445,  247,  373 };
01665 //   uint ssfnum[9] = {    0,  410,  846, 1331, 1652, 1989, 2484, 2929, 3176 };
01666 
01667 //   // ACB_3 IEEE-TR 07 test T_E
01668 //   std::string stem("/lab/tmpib/u/siagian/PAMI07/ACB/gist/ACB");
01669 //   uint sfnum [9] = {  388,  461,  463,  305,  321,  534,  398,  274,  313 };
01670 //   uint ssfnum[9] = {    0,  388,  849, 1312, 1617, 1938, 2472, 2870, 3144 };
01671 
01672 //   // ACB_4 IEEE-TR 07 test T_F
01673 //   std::string stem("/lab/tmpib/u/siagian/PAMI07/ACB/gist/ACB");
01674 //   uint sfnum [9] = {  411,  438,  474,  249,  319,  502,  400,  288,  296 };
01675 //   uint ssfnum[9] = {    0,  411,  849, 1323, 1572, 1891, 2393, 2793, 3081 };
01676 
01677 //   // AnFpark_1 IEEE-TR 07 test D
01678 //   std::string stem("../data/PAMI07/AnFpark/gist/AnFpark");
01679 //   uint sfnum [9] = {  698,  570,  865,  488,  617, 1001,  422,  598,  747 };
01680 //   uint ssfnum[9] = {    0,  698, 1268, 2133, 2621, 3238, 4239, 4661, 5259 };
01681 
01682 //   // AnFpark_2 IEEE-TR 07 test J
01683 //   std::string stem("/lab/tmpib/u/siagian/PAMI07/AnFpark/gist/AnFpark");
01684 //   uint sfnum [9] = {  802,  328,  977,  597,  770, 1122,  570,  692,  809 };
01685 //   uint ssfnum[9] = {    0,  802, 1130, 2107, 2704, 3474, 4596, 5166, 5858 };
01686 
01687 //   // AnFpark_3 IEEE-TR 07 test T_A
01688 //   std::string stem("/lab/tmpib/u/siagian/PAMI07/AnFpark/gist/AnFpark");
01689 //   uint sfnum [9] = {  891,  474,  968,  688,  774, 1003,  561,  797,  862 };
01690 //   uint ssfnum[9] = {    0,  891, 1365, 2333, 3021, 3795, 4798, 5359, 6156 };
01691 
01692 //   // AnFpark_4 IEEE-TR 07 test T_B
01693 //   std::string stem("/lab/tmpib/u/siagian/PAMI07/AnFpark/gist/AnFpark");
01694 //   uint sfnum [9] = {  746,  474,  963,  632,  777, 1098,  399,  768,  849 };
01695 //   uint ssfnum[9] = {    0,  746, 1220, 2183, 2815, 3592, 4690, 5089, 5857 };
01696 
01697 //   // FDFpark_1 IEEE-TR 07 test T_C
01698 //   std::string stem("../data/PAMI07/FDFpark/gist/FDFpark");
01699 //   uint sfnum [9] = {  881,  788,  858,  837,  831, 1680, 1037, 1172,  739 };
01700 //   uint ssfnum[9] = {    0,  881, 1669, 2527, 3364, 4195, 5875, 6912, 8084 };
01701 
01702 //   // FDFpark_2 IEEE-TR 07 test T_D
01703 //   std::string stem("/lab/tmpib/u/siagian/PAMI07/FDFpark/gist/FDFpark");
01704 //   uint sfnum [9] = {  670,  740,  696,  740,  748, 1565,  923, 1211,  825 };
01705 //   uint ssfnum[9] = {    0,  670, 1410, 2106, 2846, 3594, 5159, 6082, 7293 };
01706 
01707 //   // FDFpark_3 IEEE-TR 07 test T_H
01708 //   std::string stem("/lab/tmpib/u/siagian/PAMI07/FDFpark/gist/FDFpark");
01709 //   uint sfnum [9] = {  847,  797,  922,  837,  694, 1712,  857, 1355,  794 };
01710 //   uint ssfnum[9] = {    0,  847, 1644, 2566, 3403, 4097, 5809, 6666, 8021 };
01711 
01712 //   // FDFpark_4 IEEE-TR 07 test T_I
01713 //   std::string stem("/lab/tmpib/u/siagian/PAMI07/FDFpark/gist/FDFpark");
01714 //   uint sfnum [9] = {  953,  878,  870,  821,  854, 1672,  894, 1270,  743 };
01715 //   uint ssfnum[9] = {    0,  953, 1831, 2701, 3522, 4376, 6048, 6942, 8212 };
01716 
01717   // =====================================================================
01718 //   // ACB_1 ICRA 08 test T_A
01719 //   std::string stem("/lab/tmpib/u/siagian/PAMI07/ACB/gist/ACB");
01720 //   uint sfnum [9] = {  387,  440,  465,  359,  307,  556,  438,  290,  341 };
01721 //   uint ssfnum[9] = {    0,  387,  827, 1292, 1651, 1958, 2514, 2952, 3242 };
01722 
01723 //   // AnFpark_1 IEEE-TR 07 test D
01724 //   std::string stem("/lab/tmpib/u/siagian/PAMI07/AnFpark/gist/AnFpark");
01725 //   uint sfnum [9] = {  698,  570,  865,  488,  617, 1001,  422,  598,  747 };
01726 //   uint ssfnum[9] = {    0,  698, 1268, 2133, 2621, 3238, 4239, 4661, 5259 };
01727 
01728 //   // FDFpark_1 IEEE-TR 07 test T_C
01729 //   std::string stem("/lab/tmpib/u/siagian/PAMI07/FDFpark/gist/FDFpark");
01730 //   uint sfnum [9] = {  881,  788,  858,  837,  831, 1680, 1037, 1172,  739 };
01731 //   uint ssfnum[9] = {    0,  881, 1669, 2527, 3364, 4195, 5875, 6912, 8084 };
01732 
01733   // HNB testing for Loc & Nav
01734   // std::string stem("../data/HNB_T/gist/");
01735   // uint sfnum [4] = { 655,  653,  521,  353 };
01736   // uint ssfnum[4] = {   0,  655, 1308, 1829 };
01737   //uint ssfnum[4] = { 21696, 22351, 23004, 23525 };
01738 
01739   std::string stem("../data/Equad/gist/");
01740   uint sfnum [4] = { 2363,  567, 2361,  571 };
01741   uint ssfnum[4] = {    0, 2363, 2930, 5291 };
01742 
01743 
01744   //FILE *gfp;
01745   int afnum = 0;
01746   for(uint i = 0; i < 4; i++)
01747     {
01748       if((fNum >= ssfnum[i]) && (fNum < ssfnum[i]+sfnum[i]))
01749         {
01750           stem = stem + sformat("_T_%dC", i+1);
01751           //stem = stem + sformat("%dD", i+1);
01752           afnum = fNum - ssfnum[i];
01753           dx = 1.0/float(sfnum[i]); dy = 0.0F;
01754           snumGT = i; ltravGT = float(afnum)/float(sfnum[i]);
01755           break;
01756         }
01757     }
01758 
01759 //   ltravGT = fNum/866.0; dx = 1/866.0; afnum = fNum;
01760 //   stem = sformat("/lab/tmpib/u/siagian/PAMI07/ACB/gist/ACB1B");
01761 
01762   // std::string gfname = stem + sformat("_%03d.gist", afnum);
01763   Image<double> cgist(1, 714, NO_INIT);
01764   // if((gfp = fopen(gfname.c_str(),"rb")) != NULL)
01765   //   {
01766   //     Image<double>::iterator aptr = cgist.beginw();
01767   //     for(uint i = 0; i < 714; i++)
01768   //       {
01769   //         double val;  fread(&val, sizeof(double), 1, gfp);
01770   //         *aptr++ = val;
01771   //       }
01772   //     LDEBUG("gist file found: %s ground truth: %d %f",
01773   //            gfname.c_str(), snumGT, ltravGT);
01774 
01775   //     fclose(gfp);
01776   //   }
01777   // else LFATAL("gist file NOT found: %s", gfname.c_str());
01778 
01779   // add random error to the robot movement
01780   dy =+ 0.0F; //FIX
01781 
01782   return cgist;
01783 }
01784 
01785 #endif
01786 
01787 // ######################################################################
01788 /* So things look consistent in everyone's emacs... */
01789 /* Local Variables: */
01790 /* indent-tabs-mode: nil */
01791 /* End: */
Generated on Sun May 8 08:41:18 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3