GSlocalizer.C

Go to the documentation of this file.
00001 /*!@file Beobot/GSlocalizer.C takes in salient object and gist vector
00002   to localize in the map it has. It also takes in command to go to a
00003   target location                                                       */
00004 // //////////////////////////////////////////////////////////////////// //
00005 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00006 // University of Southern California (USC) and the iLab at USC.         //
00007 // See http://iLab.usc.edu for information about this project.          //
00008 // //////////////////////////////////////////////////////////////////// //
00009 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00010 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00011 // in Visual Environments, and Applications'' by Christof Koch and      //
00012 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00013 // pending; application number 09/912,225 filed July 23, 2001; see      //
00014 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00015 // //////////////////////////////////////////////////////////////////// //
00016 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00017 //                                                                      //
00018 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00019 // redistribute it and/or modify it under the terms of the GNU General  //
00020 // Public License as published by the Free Software Foundation; either  //
00021 // version 2 of the License, or (at your option) any later version.     //
00022 //                                                                      //
00023 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00024 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00025 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00026 // PURPOSE.  See the GNU General Public License for more details.       //
00027 //                                                                      //
00028 // You should have received a copy of the GNU General Public License    //
00029 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00030 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00031 // Boston, MA 02111-1307 USA.                                           //
00032 // //////////////////////////////////////////////////////////////////// //
00033 //
00034 // Primary maintainer for this file: Christian Siagian <siagian@usc.edu>
00035 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Beobot/GSlocalizer.C $
00036 // $Id: GSlocalizer.C 13712 2010-07-28 21:00:40Z itti $
00037 //
00038 
00039 // ######################################################################
00040 
00041 #include "Beobot/GSlocalizer.H"
00042 #include "Image/CutPaste.H"       // for inplacePaste()
00043 #include "Image/MatrixOps.H"      // for transpose()
00044 #include "Image/DrawOps.H"        // for drawing
00045 
00046 // number of particles used
00047 #define NUM_PARTICLES          100
00048 
00049 // maximum allowable localization error (in unit map)
00050 #define MAX_LOC_ERROR          5.0
00051 
00052 // standard deviation for odometry error (in feet)
00053 #define STD_ODO_ERROR          0.02
00054 
00055 // standard deviation for length traveled error (in ltrav [0.0 ... 1.0])
00056 #define STD_LTRAV_ERROR        0.02
00057 
00058 #define GIST_PRIORITY_WEIGHT   0.5
00059 #define SAL_PRIORITY_WEIGHT    0.2
00060 #define LOCN_PRIORITY_WEIGHT   0.3
00061 
00062 // ######################################################################
00063 void *GSlocalizer_threadCompute(void *gsl)
00064 {
00065   GSlocalizer *gsl2 = (GSlocalizer *)gsl;
00066   gsl2->threadCompute();
00067   return NULL;
00068 }
00069 
00070 // ######################################################################
00071 GSlocalizer::GSlocalizer(OptionManager& mgr,
00072                          const std::string& descrName,
00073                          const std::string& tagName)
00074   :
00075   ModelComponent(mgr, descrName, tagName),
00076   itsSegmentBeliefHistogram(new Histogram())
00077 {
00078   // default start and ground truth location:
00079   //  the two formats are not the same
00080   itsSegmentLocation       = 0;
00081   itsSegmentLengthTraveled = 0.0F;
00082   itsLocation              = Point2D<int>(-1,-1);
00083   itsSnumGT                = 0;
00084   itsLtravGT               = 0.0;
00085 
00086   itsIsQueueSorted = false;
00087 
00088   itsOutputReady2 = true;
00089 
00090   itsStopSearch = false;
00091 
00092   itsTimer.reset(new Timer(1000000));
00093 }
00094 
00095 // ######################################################################
00096 void GSlocalizer::setEnvironment(rutz::shared_ptr<Environment> env)
00097 {
00098   itsEnvironment = env;
00099 
00100   //! from its environment: topological map
00101   itsTopologicalMap = env->getTopologicalMap();
00102 
00103   //! from its environment: visual landmark database
00104   itsLandmarkDB = env->getLandmarkDB();
00105 }
00106 
00107 // ######################################################################
00108 void GSlocalizer::setSavePrefix(std::string prefix)
00109 {
00110   itsSavePrefix = prefix;
00111 }
00112 
00113 // ######################################################################
00114 rutz::shared_ptr<Environment> GSlocalizer::getEnvironment()
00115 {
00116   return itsEnvironment;
00117 }
00118 
00119 // ######################################################################
00120 void GSlocalizer::setBeoWulf(nub::soft_ref<Beowulf> beo)
00121 {
00122   itsBeowulf = beo;
00123 }
00124 
00125 // ######################################################################
00126 void GSlocalizer::start1()
00127 {
00128   // start threads. They should go to sleep on the condition since no
00129   // jobs have been queued up yet:
00130   pthread_mutex_init(&jobLock, NULL);
00131   pthread_mutex_init(&fnumLock, NULL);
00132   pthread_mutex_init(&or2Lock, NULL);
00133   pthread_mutex_init(&stopSearchLock, NULL);
00134   pthread_mutex_init(&resLock, NULL);
00135   pthread_mutex_init(&workLock, NULL);
00136   pthread_mutex_init(&particleLock, NULL);
00137   pthread_cond_init(&jobCond, NULL);
00138 
00139   LINFO("Starting with %d threads...", NUM_GSL_THREAD);
00140 
00141   // get our processing threads started:
00142   worker = new pthread_t[NUM_GSL_THREAD];
00143   for (uint i = 0; i < NUM_GSL_THREAD; i ++)
00144     {
00145       pthread_create(&worker[i], NULL, GSlocalizer_threadCompute,
00146                      (void *)this);
00147 
00148       // all threads should go and lock against our job condition.
00149       // Sleep a bit to make sure this really happens:
00150       usleep(100000);
00151     }
00152 
00153   itsNumWorking = 0;
00154 }
00155 
00156 // ######################################################################
00157 void GSlocalizer::stop2()
00158 {
00159   // should cleanup the threads, mutexes, etc...
00160   pthread_cond_destroy(&jobCond);
00161 
00162   //for (uint i = 0; i < NUM_GSL_THREAD; i ++)
00163   //  pthread_delete(&worker[i].....
00164 
00165   delete [] worker;
00166 }
00167 
00168 // ######################################################################
00169 GSlocalizer::~GSlocalizer()
00170 { }
00171 
00172 // ######################################################################
00173 void GSlocalizer::setWindow(rutz::shared_ptr<XWinManaged> inputWin)
00174 {
00175   itsWin = inputWin;
00176 }
00177 
00178 // ######################################################################
00179 void GSlocalizer::initParticles(std::string belFName)
00180 {
00181   uint nsegment = itsTopologicalMap->getSegmentNum();
00182   itsSegmentBeliefHistogram->resize(nsegment);
00183   LINFO("number of segment : %d", nsegment);
00184 
00185   itsBeliefParticles.clear();
00186   itsBeliefLocations.clear();
00187 
00188   // check if the file does not exist or it's a blank entry
00189   FILE *fp; if((fp = fopen(belFName.c_str(),"rb")) == NULL)
00190     {
00191       LINFO("Belief file %s not found", belFName.c_str());
00192       LINFO("create random particles");
00193 
00194       // create initial random particles
00195       for(uint i = 0; i < NUM_PARTICLES; i++)
00196         {
00197           float t  = rand()/(RAND_MAX + 1.0);
00198           float t2 = rand()/(RAND_MAX + 1.0);
00199 
00200           uint  snum  = uint ((0)    + ((nsegment) * t ));
00201           float ltrav = float((0.0F) + ((1.0F    ) * t2));
00202           itsBeliefParticles.push_back(GSparticle(snum, ltrav));
00203         }
00204     }
00205   else
00206     {
00207       LINFO("Belief file %s found", belFName.c_str());
00208 
00209       // get the particles
00210       for(uint i = 0; i < NUM_PARTICLES; i++)
00211         {
00212           char inLine[200]; if (fgets(inLine, 200, fp) == NULL) LFATAL("fgets failed");
00213           uint snum; float ltrav;
00214           sscanf(inLine, "%d %f", &snum, &ltrav);
00215           itsBeliefParticles.push_back(GSparticle(snum, ltrav));
00216         }
00217       Raster::waitForKey();
00218     }
00219 
00220   // fill in the locations as well
00221   for(uint i = 0; i < NUM_PARTICLES; i++)
00222     {
00223       uint  snum  = itsBeliefParticles[i].segnum;
00224       float ltrav = itsBeliefParticles[i].lentrav;
00225 
00226       // convert to Point2D<int>
00227       Point2D<int> loc = itsTopologicalMap->getLocation(snum, ltrav);
00228       itsBeliefLocations.push_back(loc);
00229 
00230       LDEBUG("particle[%4u]: (%3u, %10.6f) = (%4d %4d)",
00231              i, snum, ltrav, loc.i, loc.j);
00232     }
00233 }
00234 
00235 // ######################################################################
00236 std::vector<GSparticle> GSlocalizer::getBeliefParticles()
00237 {
00238   std::vector<GSparticle> beliefParticles(itsBeliefParticles.size());
00239 
00240   pthread_mutex_lock(&particleLock);
00241   for(uint i = 0; i < itsBeliefParticles.size(); i++)
00242     {
00243       beliefParticles[i] =
00244         GSparticle(itsBeliefParticles[i].segnum,
00245                    itsBeliefParticles[i].lentrav);
00246     }
00247   pthread_mutex_unlock(&particleLock);
00248 
00249   return beliefParticles;
00250 }
00251 
00252 // ######################################################################
00253 rutz::shared_ptr<Histogram> GSlocalizer::getSegmentBeliefHistogram()
00254 {
00255   itsSegmentBeliefHistogram->clear();
00256 
00257   for(uint i = 0; i < NUM_PARTICLES; i++)
00258     {
00259       itsSegmentBeliefHistogram->
00260         addValue(itsBeliefParticles[i].segnum, 1.0F);
00261     }
00262 
00263   //! print the histogram profile
00264   uint nsegment = itsTopologicalMap->getSegmentNum();
00265   for(uint i = 0; i < nsegment; i++)
00266     LDEBUG("[%d]: %d", i, uint(itsSegmentBeliefHistogram->getValue(i)));
00267   return itsSegmentBeliefHistogram;
00268 }
00269 
00270 // ######################################################################
00271 bool GSlocalizer::outputReady()
00272 {
00273   bool ret = false;
00274   uint njobs; uint nworking;
00275   pthread_mutex_lock(&jobLock);
00276   njobs = itsJobQueue.size();
00277   pthread_mutex_unlock(&jobLock);
00278 
00279   pthread_mutex_lock(&workLock);
00280   nworking = itsNumWorking;
00281   pthread_mutex_unlock(&workLock);
00282 
00283   ret = (njobs == 0U && nworking == 0U);
00284   LDEBUG("jobs left: %u, still working: %d", njobs, nworking);
00285   return ret;
00286 }
00287 
00288 // ######################################################################
00289 bool GSlocalizer::isMatchFound(uint index)
00290 {
00291   bool ret = false;
00292   pthread_mutex_lock(&resLock);
00293   ASSERT(index < itsMatchFound.size());
00294   ret = itsMatchFound[index];
00295   pthread_mutex_unlock(&resLock);
00296 
00297   return ret;
00298 }
00299 
00300 // ######################################################################
00301 GSlocJobData GSlocalizer::getMatch(uint index)
00302 {
00303   GSlocJobData lmkMatch;
00304   pthread_mutex_lock(&resLock);
00305   ASSERT(index < itsLmkMatch.size());
00306   lmkMatch = itsLmkMatch[index];
00307   pthread_mutex_unlock(&resLock);
00308 
00309   return lmkMatch;
00310 }
00311 
00312 // ######################################################################
00313 Image<PixRGB<byte> > GSlocalizer::getInputImage()
00314 {
00315   return itsInputImage;
00316 }
00317 
00318 // ######################################################################
00319 uint GSlocalizer::getNumInputObject()
00320 {
00321   return itsInputVO.size();
00322 }
00323 
00324 // ######################################################################
00325 rutz::shared_ptr<VisualObject> GSlocalizer::getInputVO(uint index)
00326 {
00327   rutz::shared_ptr<VisualObject> retvo;
00328   //pthread_mutex_lock(&resLock);
00329   ASSERT(index < itsInputVO.size());
00330   retvo = itsInputVO[index];
00331   //pthread_mutex_unlock(&resLock);
00332 
00333   return retvo;
00334 }
00335 
00336 // ######################################################################
00337 Image<double> GSlocalizer::getInputGist()
00338 {
00339   Image<double> retigist;
00340   //pthread_mutex_lock(&resLock);
00341   retigist = itsInputGist;
00342   //pthread_mutex_unlock(&resLock);
00343 
00344   return retigist;
00345 }
00346 
00347 // ######################################################################
00348 Point2D<int> GSlocalizer::getInputObjOffset(uint index)
00349 {
00350   Point2D<int> retpt;
00351   //pthread_mutex_lock(&resLock);
00352   ASSERT(index < itsInputObjOffset.size());
00353   retpt = itsInputObjOffset[index];
00354   //pthread_mutex_unlock(&resLock);
00355 
00356   return retpt;
00357 }
00358 
00359 // ######################################################################
00360 rutz::shared_ptr<VisualObjectMatch> GSlocalizer::getVOmatch(uint index)
00361 {
00362   rutz::shared_ptr<VisualObjectMatch> retMatch;
00363   pthread_mutex_lock(&resLock);
00364   ASSERT(index < itsVOmatch.size());
00365   retMatch = itsVOmatch[index];
00366   pthread_mutex_unlock(&resLock);
00367 
00368   return retMatch;
00369 }
00370 
00371 // ######################################################################
00372 int GSlocalizer::getInputFnum()
00373 {
00374   int retFnum;
00375   pthread_mutex_lock(&fnumLock);
00376   retFnum = itsInputFnum;
00377   pthread_mutex_unlock(&fnumLock);
00378 
00379   return retFnum;
00380 }
00381 
00382 // ######################################################################
00383 int GSlocalizer::getSearchInputFnum()
00384 {
00385   int retFnum;
00386   //pthread_mutex_lock(&resLock);
00387   retFnum = itsSearchInputFnum;
00388   //pthread_mutex_unlock(&resLock);
00389 
00390   return retFnum;
00391 }
00392 
00393 // ######################################################################
00394 rutz::shared_ptr<Histogram> GSlocalizer::getSegmentHistogram()
00395 {
00396   return itsSegmentHistogram;
00397 }
00398 
00399 // ######################################################################
00400 uint GSlocalizer::getSegmentNumberMatch(uint index)
00401 {
00402   ASSERT(index < itsMatchFound.size());
00403   return itsSegNumMatch[index];
00404 }
00405 
00406 // ######################################################################
00407 float GSlocalizer::getLengthTraveledMatch(uint index)
00408 {
00409   ASSERT(index < itsMatchFound.size());
00410   return itsLenTravMatch[index];
00411 }
00412 
00413 // ######################################################################
00414 uint GSlocalizer::getNumObjectSearch(uint index)
00415 {
00416   uint nObjSearch;
00417   pthread_mutex_lock(&resLock);
00418   ASSERT(index < itsNumObjectSearch.size());
00419   nObjSearch = itsNumObjectSearch[index];
00420   pthread_mutex_unlock(&resLock);
00421 
00422   return nObjSearch;
00423 }
00424 
00425 // ######################################################################
00426 void GSlocalizer::input
00427 ( Image<PixRGB<byte> > ima,
00428   std::vector<rutz::shared_ptr<VisualObject> > inputVO,
00429   std::vector<Point2D<int> > inputObjOffset, int inputFnum, Image<double> cgist,
00430   float dx, float dy)
00431 {
00432   pthread_mutex_lock(&fnumLock);
00433   itsInputFnum = inputFnum;
00434   pthread_mutex_unlock(&fnumLock);
00435 
00436   // store the robot movement
00437   itsRobotDx = dx;
00438   itsRobotDy = dy;
00439 
00440   // calculate the segment prediction
00441   itsInputGist = cgist;
00442   itsSegmentHistogram = itsEnvironment->classifySegNum(itsInputGist);
00443 
00444   // apply action model and segment observation model
00445   pthread_mutex_lock(&particleLock);
00446   actionUpdateBelief();
00447   segmentUpdateBelief();
00448   pthread_mutex_unlock(&particleLock);
00449 
00450   // if the object recognition system is not running
00451   bool outputReady2;
00452   pthread_mutex_lock(&or2Lock);
00453   outputReady2 = itsOutputReady2;
00454   pthread_mutex_unlock(&or2Lock);
00455 
00456   // if there is no search or resetting
00457   if(outputReady2)
00458     {
00459       pthread_mutex_lock(&or2Lock);
00460       itsOutputReady2 = false;
00461       pthread_mutex_unlock(&or2Lock);
00462 
00463       pthread_mutex_lock(&jobLock);
00464       itsJobQueue.clear();
00465       itsSearchInputFnum = inputFnum;
00466       LINFO("[%6d] NEW salregs", itsSearchInputFnum);
00467 
00468       // store the input image, visual-objects
00469       itsInputImage = ima;              // FIX: <- need to be updated
00470       itsInputVO.clear();
00471       itsVOKeypointsComputed.clear();
00472       itsInputObjOffset.clear();
00473       uint inputSize = inputVO.size();
00474       for(uint i = 0; i < inputSize; i++)
00475         {
00476           itsInputVO.push_back(inputVO[i]);
00477           itsVOKeypointsComputed.push_back(false);
00478           itsInputObjOffset.push_back(inputObjOffset[i]);
00479         }
00480       pthread_mutex_unlock(&jobLock);
00481 
00482       // resize the result storage
00483       pthread_mutex_lock(&resLock);
00484       itsMatchFound.clear();
00485       itsVOmatch.clear();         itsVOmatch.resize(inputSize);
00486       itsLmkMatch.clear();        itsLmkMatch.resize(inputSize);
00487       itsSegNumMatch.clear();     itsSegNumMatch.resize(inputSize);
00488       itsLenTravMatch.clear();    itsLenTravMatch.resize(inputSize);
00489       itsNumObjectSearch.clear(); itsNumObjectSearch.resize(inputSize);
00490       for(uint i = 0; i < inputSize; i++) itsMatchFound.push_back(false);
00491       for(uint i = 0; i < inputSize; i++) itsNumObjectSearch[i] = 0;
00492       pthread_mutex_unlock(&resLock);
00493 
00494       // call the search priority function
00495       pthread_mutex_lock(&jobLock);
00496       setSearchPriority();
00497 
00498       // we will prioritize using saliency in the search loop
00499 
00500       // so sort the queue then
00501       itsIsQueueSorted = false;
00502 
00503       pthread_mutex_unlock(&jobLock);
00504 
00505       // broadcast on job queue condition to wake up worker threads:
00506       pthread_cond_broadcast(&jobCond);
00507     }
00508   else
00509     {
00510       LDEBUG("[%6d] NO salregs", inputFnum);
00511 
00512       // FIX: funky project forward stuff
00513     }
00514 }
00515 
00516 // ######################################################################
00517 void GSlocalizer::setGroundTruth(uint snum, float ltrav)
00518 {
00519   itsSnumGT  = snum;
00520   itsLtravGT = ltrav;
00521 }
00522 
00523 // ######################################################################
00524 void GSlocalizer::getGroundTruth(uint &snum, float &ltrav)
00525 {
00526   snum  = itsSnumGT;
00527   ltrav = itsLtravGT;
00528 }
00529 
00530 // ######################################################################
00531 //! set the search priority for landmark DB
00532 void GSlocalizer::setSearchPriority()
00533 {
00534   // search priority is:
00535   // GIST_PRIORITY_WEIGHT * segment priority +
00536   // LOCN_PRIORITY_WEIGHT * current location priority +
00537   // SAL_PRIORITY_WEIGHT * saliency priority
00538   //   (sal is done in the search loop)
00539 
00540   itsTimer->reset();
00541   // create jobs for each landmark - object combination
00542   for(uint i = 0; i < itsEnvironment->getNumSegment(); i++)
00543     {
00544       uint nlmk = itsLandmarkDB->getNumSegLandmark(i);
00545       LDEBUG("itsLandmarkDB[%d]: %d", i, nlmk);
00546       for(uint j = 0; j < nlmk; j++)
00547         {
00548           // check each object
00549           for(uint l = 0; l < itsInputVO.size(); l++)
00550             {
00551               uint nObj = itsLandmarkDB->getLandmark(i,j)->numObjects();
00552               uint k = 0;
00553               while(k < nObj)
00554                 {
00555                   uint k2 = k + N_OBJECT_BLOCK - 1;
00556                   if(k2 > nObj-1) k2 = nObj - 1;
00557                   itsJobQueue.push_back(GSlocJobData(l, i, j, k, k2));
00558                   LDEBUG("match obj[%d] lDB[%3d][%3d]:[%3d,%3d]",
00559                          l, i,j,k,k2);
00560                   k = k2 + 1;
00561                 }
00562             }
00563         }
00564     }
00565   LDEBUG("setting jobs %11.5f", itsTimer->get()/1000.0F);
00566 
00567   // FIX: GOOD LANDMARKS ARE FOUND IN MULTIPLE RUNS !
00568 
00569   // set the order of search to random values
00570   // not actually used, just for baseline to put in ICRA08
00571   //addRandomPriority();
00572 
00573   // FIX: always load the last 10 matched landmarks first
00574   // we do this by adding a value of 3.0
00575 
00576   // add the segment priority
00577   itsTimer->reset();
00578   addSegmentPriority();
00579   LDEBUG("segment      %11.5f", itsTimer->get()/1000.0F);
00580 
00581   // add the current location priority
00582   itsTimer->reset();
00583   addLocationPriority();
00584   LDEBUG("location     %11.5f", itsTimer->get()/1000.0F);
00585 
00586   // information for when to quit
00587   itsNumJobs           = itsJobQueue.size();
00588   itsNumJobsProcessed  = 0;
00589   itsLastSuccessfulJob = 0;
00590   itsNumObjectFound    = 0;
00591 }
00592 
00593 // ######################################################################
00594 void GSlocalizer::addRandomPriority()
00595 {
00596   // add a random value to the priority value
00597   std::list<GSlocJobData>::iterator itr = itsJobQueue.begin();
00598   while (itr != itsJobQueue.end())
00599     {
00600       // flip the value
00601       float val = float(rand()/(RAND_MAX + 1.0));
00602       (*itr).pVal += val;
00603       itr++;
00604     }
00605 }
00606 
00607 // ######################################################################
00608 void GSlocalizer::addSegmentPriority()
00609 {
00610   // add the segment value to the priority value
00611   std::list<GSlocJobData>::iterator itr = itsJobQueue.begin();
00612   while (itr != itsJobQueue.end())
00613     {
00614       // flip the value
00615       float val = GIST_PRIORITY_WEIGHT *
00616         (1.0 - itsSegmentHistogram->getValue((*itr).segNum));
00617 
00618       (*itr).pVal += val;
00619       (*itr).segVal = val;
00620       itr++;
00621     }
00622 }
00623 
00624 // ######################################################################
00625 void GSlocalizer::addSaliencyPriority()
00626 {
00627   uint nObj = itsInputVO.size();
00628 
00629   // go through each landmark - object combination
00630   itsTimer->reset();
00631   uint nSeg = itsEnvironment->getNumSegment();
00632   std::vector<std::vector<std::vector<float> > > salVal(nSeg);
00633   for(uint i = 0; i < itsEnvironment->getNumSegment(); i++)
00634     {
00635       uint nlmk = itsLandmarkDB->getNumSegLandmark(i);
00636       salVal[i].resize(nlmk);
00637       for(uint j = 0; j < nlmk; j++)
00638         {
00639           // check each object
00640           salVal[i][j].resize(nObj);
00641           for(uint k = 0; k < nObj; k++)
00642             {
00643               LDEBUG("sal seg[%3d] lmk[%3d] obj[%3d]", i,j,k);
00644               salVal[i][j][k] = SAL_PRIORITY_WEIGHT *
00645                 itsLandmarkDB->getLandmark(i,j)
00646                 ->matchSalientFeatures(itsInputVO[k]);
00647             }
00648 
00649           // display the database landmark object
00650           //Image<PixRGB<byte> > img = itsLandmarkDB->getLandmark(i,j)
00651           //  ->getObject(0)->getSalAndKeypointImage();
00652           //itsWin->drawImage(img, 0,0);
00653           //Raster::waitForKey();
00654         }
00655     }
00656   LDEBUG("compute saliency dist %11.5f", itsTimer->get()/1000.0F);
00657 
00658   // add the saliency value to the priority value
00659   std::list<GSlocJobData>::iterator itr = itsJobQueue.begin();
00660   while (itr != itsJobQueue.end())
00661     {
00662       float t = (*itr).pVal;
00663       float val = salVal[(*itr).segNum][(*itr).lmkNum][(*itr).objNum];
00664       (*itr).pVal += val;
00665       (*itr).salVal = val;
00666 
00667       LDEBUG("pval[%3d][%3d][%3d]: %f + %f = %f",
00668              (*itr).segNum, (*itr).lmkNum, (*itr).objNum, t, val, (*itr).pVal);
00669       itr++;
00670     }
00671 }
00672 
00673 // ######################################################################
00674 void GSlocalizer::addLocationPriority()
00675 {
00676   // normalizer: setup weight and sigma for decision boundary
00677   Dims mDims = itsTopologicalMap->getMapDims();
00678   Point2D<int> brMap(mDims.w(), mDims.h());
00679   float mDiag = brMap.distance(Point2D<int>(0,0));
00680   float sigma = .1 * mDiag;
00681   LDEBUG("map diagonal: %f -> sigma: %f",  mDiag, sigma);
00682   LDEBUG("curr loc: %d, %f ", itsSegmentLocation, itsSegmentLengthTraveled);
00683 
00684   // get the distance to all landmarks from current belief location
00685   uint nSeg =itsEnvironment->getNumSegment();
00686   std::vector<std::vector<float> > locVal(nSeg);
00687   for(uint i = 0; i < itsEnvironment->getNumSegment(); i++)
00688     {
00689       uint nlmk = itsLandmarkDB->getNumSegLandmark(i);
00690       locVal[i].resize(nlmk);
00691       for(uint j = 0; j < nlmk; j++)
00692         {
00693           std::pair<float,float> locRange =
00694             itsLandmarkDB->getLocationRange(i,j);
00695           LDEBUG("lmk[%3d][%3d]: (%f,%f)", i,j,
00696                  locRange.first, locRange.second);
00697 
00698           // if the location is within the range of the landmark
00699           if(itsSegmentLocation == i &&
00700              itsSegmentLengthTraveled >= locRange.first &&
00701              itsSegmentLengthTraveled <= locRange.second  )
00702             {
00703               locVal[i][j] = 0.0;
00704               LDEBUG("dist[%d][%d]: within -> %f",i,j, locVal[i][j]);
00705             }
00706           else
00707             {
00708               // get distance to the first seen location
00709               float fdist = itsTopologicalMap->
00710                 getDistance(itsSegmentLocation, itsSegmentLengthTraveled,
00711                             i, locRange.first);
00712 
00713               // get distance to the last seen location
00714               float ldist = itsTopologicalMap->
00715                 getDistance(itsSegmentLocation, itsSegmentLengthTraveled,
00716                             i, locRange.second);
00717 
00718               // get the minimum distance
00719               float dist = std::min(fdist,ldist);
00720 
00721               LDEBUG("f - l: %d [%f -> %f][%f -> %f]",
00722                      i, locRange.first, fdist, locRange.second, ldist);
00723 
00724               // get distances to nodes in between
00725               std::vector<std::pair<uint,float> > betLocs =
00726                 itsTopologicalMap->getNodeLocationsInInterval
00727                 (i, locRange.first, locRange.second);
00728               for(uint k = 0; k < betLocs.size(); k++)
00729                 {
00730                   float bdist = itsTopologicalMap->
00731                     getDistance(itsSegmentLocation, itsSegmentLengthTraveled,
00732                                 i, betLocs[k].second);
00733                   if(dist > bdist) dist = bdist;
00734 
00735                   LDEBUG("bet: %d [%f -> %f]", i, betLocs[k].second, bdist);
00736                 }
00737 
00738               // normalize gaussian val to 1.0 invert the values
00739               locVal[i][j] = LOCN_PRIORITY_WEIGHT *
00740                 (1.0 - pow(M_E, -dist*dist/(2.0*sigma*sigma)));
00741               LDEBUG("dist[%d][%d]: %f ->%f",i,j, dist, locVal[i][j]);
00742             }
00743         }
00744     }
00745 
00746   // add the location value to the priority value
00747   std::list<GSlocJobData>::iterator itr = itsJobQueue.begin();
00748   while (itr != itsJobQueue.end())
00749     {
00750       float t = (*itr).pVal;
00751       float val = locVal[(*itr).segNum][(*itr).lmkNum];
00752       (*itr).pVal += val;
00753       (*itr).locVal = val;
00754 
00755       LDEBUG("pval[%3d][%3d][%3d]: %f -> %f",
00756              (*itr).segNum, (*itr).lmkNum, (*itr).objNum, t, (*itr).pVal);
00757       itr++;
00758     }
00759 }
00760 
00761 // ######################################################################
00762 void GSlocalizer::stopSearch()
00763 {
00764   //! stop search by cleaning up the queue
00765   pthread_mutex_lock(&jobLock);
00766   itsJobQueue.clear();
00767   pthread_mutex_unlock(&jobLock);
00768 
00769   //! wait until all workers stop working
00770   uint nworking = 1;
00771   while (nworking != 0U)
00772     {
00773       pthread_mutex_lock(&workLock);
00774       nworking = itsNumWorking;
00775       pthread_mutex_unlock(&workLock);
00776       LINFO("still working: %d", nworking);
00777       usleep(1000);
00778     }
00779 }
00780 
00781 // ######################################################################
00782 void GSlocalizer::stopSearch2()
00783 {
00784   pthread_mutex_lock(&stopSearchLock);
00785   itsStopSearch = true;
00786   pthread_mutex_unlock(&stopSearchLock);
00787 }
00788 
00789 // ######################################################################
00790 // The threaded function
00791 void GSlocalizer::threadCompute()
00792 {
00793   pthread_mutex_lock(&resLock);
00794   uint myNum = numWorkers ++;
00795   pthread_mutex_unlock(&resLock);
00796   LINFO("  ... worker %u ready.", myNum);
00797 
00798   while(true)
00799     {
00800       // wait until there's a job to do
00801       pthread_mutex_lock(&jobLock);
00802       GSlocJobData cjob(0, 0, 0, 0, 0); bool nojobs = true;
00803       if (itsJobQueue.empty() == false)
00804         {
00805           if(!itsIsQueueSorted)
00806             {
00807               // doing saliency prioritization is slow
00808               // has to be done outside the input loop
00809 
00810               // add the saliency priority
00811               addSaliencyPriority();
00812 
00813               // push the jobs based on the biasing values
00814               itsTimer->reset();
00815               itsJobQueue.sort();
00816               LDEBUG("sal prior %11.5f", itsTimer->get()/1000.0F);
00817 
00818               // print the priority values
00819               std::list<GSlocJobData>::iterator itr = itsJobQueue.begin();
00820               uint count = 0;
00821               while (itr != itsJobQueue.end())
00822                 {
00823                   LDEBUG("[%5d] pval[%3d][%3d][%3d]: %f + %f + %f = %f", count,
00824                          (*itr).segNum, (*itr).lmkNum, (*itr).objNum,
00825                          (*itr).segVal, (*itr).salVal, (*itr).locVal,
00826                          (*itr).pVal);
00827                   itr++; count++;
00828                 }
00829 
00830               itsIsQueueSorted = true;
00831             }
00832 
00833           cjob = itsJobQueue.front();
00834           itsJobQueue.pop_front();
00835           nojobs = false;
00836         }
00837       else
00838         pthread_cond_wait(&jobCond, &jobLock);
00839       pthread_mutex_unlock(&jobLock);
00840 
00841       // if we don't have a job to do, just wait more:
00842       if (nojobs) continue;
00843 
00844       // else we have something
00845       LDEBUG("T[%4d] match object[%d] itsLandmarkDB[%d][%d]: [ %d, %d ]",
00846              myNum, cjob.objNum, cjob.segNum, cjob.lmkNum,
00847              cjob.voStartNum, cjob.voEndNum);
00848 
00849       // add to busy thread count
00850       pthread_mutex_lock(&workLock);
00851       itsNumWorking++;
00852       pthread_mutex_unlock(&workLock);
00853 
00854       // make sure the VO keypoints are computed
00855       pthread_mutex_lock(&jobLock);
00856       if(!itsVOKeypointsComputed[cjob.objNum])
00857         {
00858           itsInputVO[cjob.objNum]->computeKeypoints();
00859           itsVOKeypointsComputed[cjob.objNum] = true;
00860         }
00861       pthread_mutex_unlock(&jobLock);
00862 
00863       // match the object with the range of database
00864       // we limit the maxScale range to [2/3 ... 3/2] by entering 1.5
00865       rutz::shared_ptr<VisualObjectMatch> cmatch;
00866       int ind = itsLandmarkDB->getLandmark(cjob.segNum, cjob.lmkNum)->
00867         match(itsInputVO[cjob.objNum],
00868               cmatch, cjob.voStartNum, cjob.voEndNum,
00869               15.0F, 0.5F, 2.5F, 4, M_PI/4, 1.5F, .25F);
00870 
00871       pthread_mutex_lock(&jobLock);
00872 
00873       // if match is found
00874       if(ind != -1)
00875         {
00876           LINFO("-> found match[%d]: %s with itsLandmarkDB[%d][%d]\n %s : %s",
00877                 cjob.objNum, itsInputVO[cjob.objNum]->getName().c_str(),
00878                 cjob.segNum, cjob.lmkNum,
00879                 cmatch->getVoRef()->getName().c_str(),
00880                 cmatch->getVoTest()->getName().c_str());
00881 
00882           // clear queue of jobs for this object
00883           uint njobs = 0; uint njobsremoved = 0;
00884           std::list<GSlocJobData>::iterator itr = itsJobQueue.begin();
00885           while (itr != itsJobQueue.end())
00886             {
00887               if((*itr).objNum == cjob.objNum)
00888                 { itr = itsJobQueue.erase(itr); njobsremoved++; }
00889               else
00890                 { itr++; njobs++; }
00891             }
00892           itsNumObjectSearch[cjob.objNum] += (ind - cjob.voStartNum + 1);
00893           itsLastSuccessfulJob = itsNumJobsProcessed;
00894           LINFO("removing %d jobs left to do: %"ZU,
00895                 njobsremoved, itsJobQueue.size());
00896 
00897           // store the match information
00898           // since there is a possibility that there are concurrent threads
00899           // that also just found it we will choose the first match
00900           pthread_mutex_lock(&resLock);
00901           //if(!itsMatchFound[cjob.objNum])
00902           //  {
00903               itsNumObjectFound++;
00904               itsVOmatch[cjob.objNum] = cmatch;
00905               itsLmkMatch[cjob.objNum] =
00906                 GSlocJobData(cjob.objNum, cjob.segNum, cjob.lmkNum, ind, ind);
00907               itsSegNumMatch[cjob.objNum] = cjob.segNum;
00908               itsLenTravMatch[cjob.objNum] =
00909                 itsLandmarkDB->getLenTrav(cjob.segNum, cjob.lmkNum, ind);
00910               itsMatchFound[cjob.objNum] = true;
00911 
00912               // update the belief using individual object
00913               // NOTE: this is for individual object updating
00914               //LINFO("updating objBelief[%d]", cjob.objNum);
00915               //pthread_mutex_lock(&particleLock);
00916               //objectUpdateBelief(cjob.objNum);
00917               //pthread_mutex_unlock(&particleLock);
00918               //  }
00919           pthread_mutex_unlock(&resLock);
00920         }
00921       else
00922         {
00923           pthread_mutex_lock(&resLock);
00924           itsNumObjectSearch[cjob.objNum]
00925             += (cjob.voEndNum - cjob.voStartNum + 1);
00926           pthread_mutex_unlock(&resLock);
00927         }
00928 
00929       // check if stop search is requested
00930       bool stopSearch = false;
00931       pthread_mutex_lock(&stopSearchLock);
00932       stopSearch = itsStopSearch;
00933       pthread_mutex_unlock(&stopSearchLock);
00934 
00935       // update last successful job count
00936       itsNumJobsProcessed++;
00937       int dlast = itsNumJobsProcessed - itsLastSuccessfulJob;
00938 
00939       // stop after matching 3 of 5
00940       // check if the number of matches since last successful one
00941       //   is bigger than a percentage of the queue size
00942       bool earlyExit =
00943         itsNumObjectFound > 2 ||
00944         (itsNumObjectFound == 2 && dlast > (int)(.0167 * itsNumJobs)) ||
00945         (itsNumObjectFound == 1 && dlast > (int)(.033  * itsNumJobs)) ||
00946         (itsNumObjectFound == 0 && dlast > (int)(.05   * itsNumJobs));
00947 
00948       if(itsJobQueue.size() > 0 && (earlyExit || stopSearch))
00949         {
00950           LINFO("EE: %d SS: %d [found: %d, dlast: %d] clear: %"ZU","
00951                 " jobs processed: %d/%d = %f",
00952                 earlyExit, stopSearch,
00953                 itsNumObjectFound, dlast, itsJobQueue.size(),
00954                 itsNumJobsProcessed, itsNumJobs,
00955                 (float)itsNumJobsProcessed/itsNumJobs);
00956           itsJobQueue.clear();
00957         }
00958       uint njobs = itsJobQueue.size();
00959       pthread_mutex_unlock(&jobLock);
00960 
00961       // no longer busy
00962       pthread_mutex_lock(&workLock);
00963       itsNumWorking--;
00964 
00965       // when the last thread is done with its work
00966       // update belief with the object obsevation model
00967       LDEBUG("jobs left: %u, still working: %d", njobs, itsNumWorking);
00968       if(njobs == 0U && itsNumWorking == 0U)
00969         {
00970           // apply object observation model for all objects found
00971           pthread_mutex_lock(&particleLock);
00972           objectUpdateBelief();
00973           pthread_mutex_unlock(&particleLock);
00974 
00975           // save some performance data
00976           uint cfnum = getInputFnum();
00977 
00978           std::string resFName = itsSavePrefix + sformat("_GS_results.txt");
00979           FILE *rFile = fopen(resFName.c_str(), "at");
00980           if (rFile != NULL)
00981             {
00982               uint ninput = getNumInputObject();
00983               std::vector<bool> mfound(ninput);
00984               std::vector<uint> nObjSearch(ninput);
00985               for(uint i = 0; i < ninput; i++)
00986                 {
00987                   mfound[i]     = isMatchFound(i);
00988                   nObjSearch[i] = getNumObjectSearch(i);
00989                 }
00990               LDEBUG("saving result to %s", resFName.c_str());
00991               std::string line = sformat("%5d %d ", cfnum, ninput);
00992               for(uint i = 0; i < ninput; i++)
00993                 line += sformat("%3d %6d", int(mfound[i]), nObjSearch[i]);
00994 
00995               LINFO("%s", line.c_str());
00996               line += std::string("\n");
00997 
00998               fputs(line.c_str(), rFile);
00999               fclose (rFile);
01000             }
01001           else LINFO("can't create file: %s", resFName.c_str());
01002 
01003           // signal to master that it is done
01004           TCPmessage smsg;
01005           smsg.reset(cfnum, SEARCH_LM_RES);
01006           smsg.addInt32(int32(getSearchInputFnum()));
01007           smsg.addInt32(int32(cfnum));
01008           itsBeowulf->send(-1, smsg);
01009 
01010           pthread_mutex_lock(&or2Lock);
01011           itsOutputReady2 = true;
01012           pthread_mutex_unlock(&or2Lock);
01013 
01014           pthread_mutex_lock(&stopSearchLock);
01015           itsStopSearch = false;
01016           pthread_mutex_unlock(&stopSearchLock);
01017 
01018           LINFO("[%d]  DONE SEARCH_LM: %d\n", cfnum, getSearchInputFnum());
01019         }
01020       pthread_mutex_unlock(&workLock);
01021     }
01022 }
01023 
01024 // ######################################################################
01025 void GSlocalizer::updateBelief()
01026 {
01027   // set the most likely location
01028   pthread_mutex_lock(&particleLock);
01029   setLocation();
01030   pthread_mutex_unlock(&particleLock);
01031 }
01032 
01033 // ######################################################################
01034 void GSlocalizer::actionUpdateBelief()
01035 {
01036   std::vector<int> stotal(itsTopologicalMap->getSegmentNum());
01037   for(uint i = 0; i < stotal.size(); i++) stotal[i] = 0;
01038 
01039   // apply the motor movement + noise to each particles
01040   for(uint i = 0; i < NUM_PARTICLES; i++)
01041     {
01042       uint  snum  = itsBeliefParticles[i].segnum;
01043       float ltrav = itsBeliefParticles[i].lentrav;
01044       LDEBUG("particle[%d]: %d, %f", i, snum, ltrav);
01045 
01046       // apply the motor command
01047       float nltrav = ltrav + itsRobotDx;
01048 
01049       // assume std odometry error of .02ft
01050       float mscale = itsTopologicalMap->getMapScale();
01051       float err    = STD_ODO_ERROR/mscale;
01052 
01053       // add noise from a Gaussian distribution (Box-Muller method)
01054       float r1 = float(rand()/(RAND_MAX + 1.0));
01055       float r2 = float(rand()/(RAND_MAX + 1.0));
01056       double r = err * sqrt( -2.0 * log(r1));
01057       double phi = 2.0 * M_PI * r2;
01058       nltrav += (r * cos(phi));
01059 
01060       // if nltrav is now improper
01061       // FIX: NEED THE SPILL OVER EFFECT
01062       if(nltrav < 0.0F) nltrav = 0.0F;
01063       if(nltrav > 1.0F) nltrav = 1.0F;
01064       itsBeliefParticles[i].lentrav = nltrav;
01065       //stotal[snum]++;
01066 
01067       // convert to Point2D<int>
01068       Point2D<int> loc = itsTopologicalMap->
01069         getLocation(itsBeliefParticles[i].segnum,
01070                     itsBeliefParticles[i].lentrav);
01071       itsBeliefLocations[i] = loc;
01072     }
01073 }
01074 
01075 // ######################################################################
01076 void GSlocalizer::segmentUpdateBelief()
01077 {
01078   // accweight is for easy calculation for choosing a random particle
01079   std::vector<float> weight(NUM_PARTICLES);
01080   std::vector<float> accweight(NUM_PARTICLES);
01081 
01082   // for each particles
01083   float accw = 0.0F;
01084   for(uint i = 0; i < NUM_PARTICLES; i++)
01085     {
01086       uint  snum  = itsBeliefParticles[i].segnum;
01087       float ltrav = itsBeliefParticles[i].lentrav;
01088       LDEBUG("particle[%4d]: %d, %f", i, snum, ltrav);
01089 
01090       // get likelihood score
01091       // score(snum)/sum(score(all segments) * score(snum)
01092       // with the denominator taken out
01093       // + 0.25 for the 50% memory of previous time
01094       float pscore = itsSegmentHistogram->getValue(snum);
01095       float score  = (pscore * pscore) + 0.25f;
01096       LDEBUG("score: %f * %f = %f", pscore, pscore, score);
01097 
01098       // update weight
01099       weight[i] = score;
01100       accw += score;
01101       accweight[i] = accw;
01102     }
01103   for(uint i = 0; i < NUM_PARTICLES; i++)
01104     LDEBUG("p[%4d]: w: %f %f ",i, weight[i], accweight[i]);
01105   LDEBUG("accw: %f",accw);
01106 
01107   // add 1% noise weight
01108   accw *= 1.01F; LDEBUG("accw+ noise: %f",accw);
01109 
01110   // weighted resample NUM_PARTICLES particles
01111   std::vector<GSparticle> tbelief(NUM_PARTICLES);
01112   uint nsegment = itsTopologicalMap->getSegmentNum();
01113   std::vector<int> stotal(nsegment);
01114   for(uint i = 0; i < stotal.size(); i++) stotal[i] = 0;
01115 
01116   for(uint i = 0; i < NUM_PARTICLES; i++)
01117     {
01118       // draw a random value between 0 and accw
01119       float rval  = accw * float(rand()/(RAND_MAX + 1.0));
01120 
01121       // get the random index
01122       uint sind = NUM_PARTICLES;
01123       for(uint j = 0; j < NUM_PARTICLES; j++)
01124         if(rval < accweight[j]) { sind = j; j = NUM_PARTICLES; }
01125       LDEBUG("rval: %f -> %d", rval, sind);
01126 
01127       // if need to create a random particle
01128       if(sind == NUM_PARTICLES)
01129         {
01130           // create initial random particles
01131           float t  = rand()/(RAND_MAX + 1.0);
01132           float t2 = rand()/(RAND_MAX + 1.0);
01133 
01134           uint  snum  = uint ((0)    + ((nsegment) * t ));
01135           float ltrav = float((0.0F) + ((1.0F    ) * t2));
01136           tbelief[i] = GSparticle(snum, ltrav);
01137           stotal[snum]++;
01138           LDEBUG("rand particle[%d]: (%d, %f)", i, snum, ltrav);
01139         }
01140       else
01141         {
01142           tbelief[i] = itsBeliefParticles[sind];
01143           stotal[itsBeliefParticles[sind].segnum]++;
01144           LDEBUG("old  particle[%d]", sind);
01145         }
01146    }
01147 
01148   for(uint i = 0; i < stotal.size(); i++) LDEBUG("seg[%d]: %d",i, stotal[i]);
01149 
01150   // copy all the particles to our belief
01151   for(uint i = 0; i < NUM_PARTICLES; i++)
01152     {
01153       itsBeliefParticles[i] = tbelief[i];
01154 
01155       // convert to Point2D<int>
01156       Point2D<int> loc = itsTopologicalMap->
01157         getLocation(tbelief[i].segnum, tbelief[i].lentrav);
01158       itsBeliefLocations[i] = loc;
01159     }
01160 }
01161 
01162 // ######################################################################
01163 void GSlocalizer::objectUpdateBelief()
01164 {
01165   // make sure at least 1 object is found
01166   uint c = 0;
01167   for(uint i = 0; i < itsMatchFound.size(); i++) if(itsMatchFound[i]) c++;
01168   if(c == 0) return;
01169 
01170   // setup weight and sigma for decision boundary
01171   Dims mDims = itsTopologicalMap->getMapDims();
01172   Point2D<int> brMap(mDims.w(), mDims.h());
01173   float mDiag = brMap.distance(Point2D<int>(0,0));
01174   float sigma = .05*mDiag;
01175   LDEBUG("map diagonal: %f -> sigma: %f", mDiag, sigma);
01176 
01177   // accweight is for easy calculation for choosing a random particle
01178   std::vector<float> weight(NUM_PARTICLES);
01179   std::vector<float> accweight(NUM_PARTICLES);
01180 
01181   // for each particles
01182   float accw = 0.0F;
01183   for(uint i = 0; i < NUM_PARTICLES; i++)
01184     {
01185       uint  snum  = itsBeliefParticles[i].segnum;
01186       float ltrav = itsBeliefParticles[i].lentrav;
01187       LDEBUG("particle[%d]: %d, %f", i, snum, ltrav);
01188 
01189       // go through each object found
01190       float pObjObs = 1.0;
01191       for(uint index = 0; index < itsMatchFound.size(); index++)
01192         {
01193           if(itsMatchFound[index])
01194             {
01195               // get the location of the object found
01196               uint  snumMatch  = itsSegNumMatch[index];
01197               float ltravMatch = itsLenTravMatch[index];
01198               LDEBUG("Match[%d]: [%d %f]", index, snumMatch, ltravMatch);
01199 
01200               // get the distance between the two points
01201               float dist = itsTopologicalMap->
01202                 getDistance(snum, ltrav, snumMatch, ltravMatch);
01203 
01204               float pOMatch = 1.0/(sigma * sqrt(2.0 * M_PI)) *
01205                 pow(M_E, -dist*dist/(2.0*sigma*sigma));
01206               pObjObs *= pOMatch;
01207 
01208               LDEBUG("dist: %f -> pOMatch: %f -> %f", dist, pOMatch, pObjObs);
01209             }
01210         }
01211 
01212       // update weight
01213       weight[i] = pObjObs;
01214       accweight[i] = weight[i] + accw;
01215       accw = accweight[i];
01216     }
01217 
01218   LDEBUG("accw: %f",accw);
01219   // add 20% weight - because objects are more exacting
01220   accw *= 1.20F;
01221   LDEBUG("accw+ noise: %f",accw);
01222 
01223   // weighted resample NUM_PARTICLES particles
01224   std::vector<GSparticle> tbelief;
01225   std::vector<int> stotal(itsTopologicalMap->getSegmentNum());
01226   for(uint i = 0; i < stotal.size(); i++) stotal[i] = 0;
01227 
01228   uint nsegment = itsTopologicalMap->getSegmentNum();
01229 
01230   for(uint i = 0; i < NUM_PARTICLES; i++)
01231     LDEBUG("p[%d]: %f %f ",i, weight[i], accweight[i]);
01232 
01233   for(uint i = 0; i < NUM_PARTICLES; i++)
01234     {
01235       // draw a random value between 0 and accw
01236       float rval  = accw * float(rand()/(RAND_MAX + 1.0));
01237 
01238       // get the random index
01239       uint sind = NUM_PARTICLES;
01240       for(uint j = 0; j < NUM_PARTICLES; j++)
01241         if(rval < accweight[j]) { sind = j; j = NUM_PARTICLES; }
01242       LDEBUG("rval: %f -> %d", rval, sind);
01243 
01244       // if need to create a random particle
01245       if(sind == NUM_PARTICLES)
01246         {
01247           // create initial random particles
01248           float t  = rand()/(RAND_MAX + 1.0);
01249           float t2 = rand()/(RAND_MAX + 1.0);
01250 
01251           uint  snum  = uint ((0)    + ((nsegment) * t ));
01252           float ltrav = float((0.0F) + ((1.0F    ) * t2));
01253           tbelief.push_back(GSparticle(snum, ltrav));
01254           stotal[snum]++;
01255           LDEBUG("rand particle[%d]: (%d, %f)", i, snum, ltrav);
01256         }
01257       else
01258         {
01259           tbelief.push_back(itsBeliefParticles[sind]);
01260           stotal[itsBeliefParticles[sind].segnum]++;
01261           LDEBUG("old  particle[%d]", sind);
01262         }
01263     }
01264 
01265   for(uint i = 0; i < stotal.size(); i++) LDEBUG("[%d]: %d",i, stotal[i]);
01266 
01267   // copy all the particles to our belief
01268   for(uint i = 0; i < NUM_PARTICLES; i++)
01269     {
01270       itsBeliefParticles[i] = tbelief[i];
01271 
01272       // convert to Point2D<int>
01273       Point2D<int> loc = itsTopologicalMap->
01274         getLocation(tbelief[i].segnum, tbelief[i].lentrav);
01275       itsBeliefLocations[i] = loc;
01276     }
01277 }
01278 
01279 // ######################################################################
01280 void GSlocalizer::objectUpdateBelief(uint index)
01281 {
01282   // make sure this object is found
01283   if(!itsMatchFound[index]) return;
01284 
01285   // setup weight and sigma for decision boundary
01286   Dims mDims = itsTopologicalMap->getMapDims();
01287   Point2D<int> brMap(mDims.w(), mDims.h());
01288   float mDiag = brMap.distance(Point2D<int>(0,0));
01289   float sigma = .05*mDiag;
01290   LDEBUG("map diagonal: %f -> sigma: %f", mDiag, sigma);
01291 
01292   // accweight is for easy calculation for choosing a random particle
01293   std::vector<float> weight(NUM_PARTICLES);
01294   std::vector<float> accweight(NUM_PARTICLES);
01295 
01296   // for each particles
01297   float accw = 0.0F;
01298   for(uint i = 0; i < NUM_PARTICLES; i++)
01299     {
01300       uint  snum  = itsBeliefParticles[i].segnum;
01301       float ltrav = itsBeliefParticles[i].lentrav;
01302       LDEBUG("particle[%d]: %d, %f", i, snum, ltrav);
01303 
01304       // get the location of the object found
01305       uint  snumMatch  = itsSegNumMatch[index];
01306       float ltravMatch = itsLenTravMatch[index];
01307       LDEBUG("Match[%d]: [%d %f]", index, snumMatch, ltravMatch);
01308 
01309       // get the distance between the two points
01310       float dist = itsTopologicalMap->
01311         getDistance(snum, ltrav, snumMatch, ltravMatch);
01312 
01313       float pObjObs = 1.0/(sigma * sqrt(2.0 * M_PI)) *
01314         pow(M_E, -dist*dist/(2.0*sigma*sigma));
01315       LDEBUG("dist: %f -> %f", dist, pObjObs);
01316 
01317       // update weight
01318       weight[i] = pObjObs;
01319       accweight[i] = weight[i] + accw;
01320       accw = accweight[i];
01321     }
01322 
01323   LDEBUG("accw: %f",accw);
01324   // add 20% weight - because objects are more exacting
01325   accw *= 1.20F;
01326   LDEBUG("accw+ noise: %f",accw);
01327 
01328   // weighted resample NUM_PARTICLES particles
01329   std::vector<GSparticle> tbelief;
01330   std::vector<int> stotal(itsTopologicalMap->getSegmentNum());
01331   for(uint i = 0; i < stotal.size(); i++) stotal[i] = 0;
01332 
01333   uint nsegment = itsTopologicalMap->getSegmentNum();
01334 
01335   for(uint i = 0; i < NUM_PARTICLES; i++)
01336     LDEBUG("p[%d]: %f %f ",i, weight[i], accweight[i]);
01337 
01338   for(uint i = 0; i < NUM_PARTICLES; i++)
01339     {
01340       // draw a random value between 0 and accw
01341       float rval  = accw * float(rand()/(RAND_MAX + 1.0));
01342 
01343       // get the random index
01344       uint sind = NUM_PARTICLES;
01345       for(uint j = 0; j < NUM_PARTICLES; j++)
01346         if(rval < accweight[j]) { sind = j; j = NUM_PARTICLES; }
01347       LDEBUG("rval: %f -> %d", rval, sind);
01348 
01349       // if need to create a random particle
01350       if(sind == NUM_PARTICLES)
01351         {
01352           // create initial random particles
01353           float t  = rand()/(RAND_MAX + 1.0);
01354           float t2 = rand()/(RAND_MAX + 1.0);
01355 
01356           uint  snum  = uint ((0)    + ((nsegment) * t ));
01357           float ltrav = float((0.0F) + ((1.0F    ) * t2));
01358           tbelief.push_back(GSparticle(snum, ltrav));
01359           stotal[snum]++;
01360           LDEBUG("rand particle[%d]: (%d, %f)", i, snum, ltrav);
01361         }
01362       else
01363         {
01364           tbelief.push_back(itsBeliefParticles[sind]);
01365           stotal[itsBeliefParticles[sind].segnum]++;
01366           LDEBUG("old  particle[%d]", sind);
01367         }
01368     }
01369 
01370   for(uint i = 0; i < stotal.size(); i++) LDEBUG("[%d]: %d",i, stotal[i]);
01371 
01372   // copy all the particles to our belief
01373   for(uint i = 0; i < NUM_PARTICLES; i++)
01374     {
01375       itsBeliefParticles[i] = tbelief[i];
01376 
01377       // convert to Point2D<int>
01378       Point2D<int> loc = itsTopologicalMap->
01379         getLocation(tbelief[i].segnum, tbelief[i].lentrav);
01380       itsBeliefLocations[i] = loc;
01381     }
01382 }
01383 
01384 // ######################################################################
01385 void GSlocalizer::setLocation()
01386 {
01387   // for each point
01388   float maxscore = 0.0F;
01389   for(uint i = 0; i < itsBeliefLocations.size(); i++)
01390     {
01391       // get distances with the neighbors closer than MAX_ERROR_DIST
01392       float score = 0.0F;
01393       Point2D<int> a = itsBeliefLocations[i];
01394       uint aseg = itsBeliefParticles[i].segnum;
01395       for(uint j = 0; j < itsBeliefLocations.size(); j++)
01396         {
01397           Point2D<int> b = itsBeliefLocations[j];
01398           float dist = a.distance(b);
01399 
01400           uint bseg = itsBeliefParticles[j].segnum;
01401           float cscore = 0.0; float sthresh = MAX_LOC_ERROR/2.0;
01402           if(dist < sthresh) // (0.0  ... 2.5] -> (1.0 -> 0.8]
01403             {
01404               cscore = (1.0  - (dist - 0.0)/sthresh * 0.2);
01405             }
01406           else if(dist < sthresh*2) // 2.5 ... 5.0] -> [0.8 ... 0.2]
01407             {
01408               cscore = (0.8 - (dist - sthresh)/sthresh * 0.6);
01409             }
01410           if(aseg != bseg) cscore *= .5;
01411           score += cscore;
01412         }
01413 
01414       // update max location
01415       if(score > maxscore)
01416         {
01417           maxscore = score;
01418           itsLocation = itsBeliefLocations[i];
01419           itsSegmentLocation = itsBeliefParticles[i].segnum;
01420           itsSegmentLengthTraveled = itsBeliefParticles[i].lentrav;
01421         }
01422     }
01423 
01424   LDEBUG("max score: %f: (%d, %d) = [%d %f]", maxscore,
01425          itsLocation.i, itsLocation.j,
01426          itsSegmentLocation, itsSegmentLengthTraveled);
01427 }
01428 
01429 // ######################################################################
01430 Point2D<int> GSlocalizer::getLocation()
01431 {
01432   return itsLocation;
01433 }
01434 
01435 // ######################################################################
01436 uint GSlocalizer::getSegmentLocation()
01437 {
01438   return itsSegmentLocation;
01439 }
01440 
01441 // ######################################################################
01442 float GSlocalizer::getSegmentLengthTraveled()
01443 {
01444   return itsSegmentLengthTraveled;
01445 }
01446 
01447 // ######################################################################
01448 Image<PixRGB<byte> > GSlocalizer::getBeliefImage(uint w, uint h, int &scale)
01449 {
01450   Image< PixRGB<byte> > res(w,h,ZEROS);
01451 
01452   // get the map from the topolagical map class
01453   Image< PixRGB<byte> > mapImg = itsTopologicalMap->getMapImage(w, h);
01454   Dims d = itsTopologicalMap->getMapDims();
01455 
01456   // add the particles on the map
01457   scale = int(mapImg.getWidth()/d.w());
01458   for(uint i = 0; i < itsBeliefParticles.size(); i++)
01459     {
01460       // get the point
01461       Point2D<int> loc = itsBeliefLocations[i] * scale;
01462       LDEBUG("point: %d %d", loc.i, loc.j);
01463 
01464       drawDisk(mapImg, loc, 2, PixRGB<byte>(0,255,255));
01465     }
01466 
01467   // draw circle to the most likely location of the object
01468   drawDisk(mapImg, itsLocation*scale, 2, PixRGB<byte>(0,0,255));
01469   drawCircle(mapImg, itsLocation*scale, int(MAX_LOC_ERROR*scale),
01470              PixRGB<byte>(0,0,255), 1);
01471 
01472   inplacePaste(res, mapImg, Point2D<int>(0,0));
01473 
01474   // get the segment belief histogram
01475   rutz::shared_ptr<Histogram> shist = getSegmentBeliefHistogram();
01476 
01477   // check which side the histogram is going to be appended to
01478   uint wslack = w - mapImg.getWidth();
01479   uint hslack = h - mapImg.getHeight();
01480   if(hslack >= wslack)
01481     {
01482       Image<byte> sHistImg =
01483         shist->getHistogramImage(w, hslack, 0.0F, float(NUM_PARTICLES));
01484       inplacePaste(res, Image<PixRGB<byte> >(sHistImg),
01485                    Point2D<int>(0,mapImg.getHeight()));
01486     }
01487   else
01488     {
01489       Image<byte> sHistImg =
01490         shist->getHistogramImage(h, wslack, 0.0F, float(NUM_PARTICLES));
01491 
01492       Image<PixRGB<byte> >
01493         t = Image<PixRGB<byte> >(flipHoriz(transpose(sHistImg)));
01494       inplacePaste(res, t, Point2D<int>(mapImg.getWidth(), 0));
01495     }
01496   return res;
01497 }
01498 
01499 // ######################################################################
01500 Image<PixRGB<byte> > GSlocalizer::getMatchImage(uint index, Dims d)
01501 {
01502   Image< PixRGB<byte> > result;
01503 
01504   ASSERT(index < itsMatchFound.size());
01505   Point2D<int> objOffset1 = itsInputObjOffset[index];
01506   Point2D<int> objOffset2 =
01507     itsLandmarkDB->getLandmark(itsLmkMatch[index].segNum,
01508                                itsLmkMatch[index].lmkNum)
01509     ->getOffsetCoords(itsLmkMatch[index].voStartNum);
01510 
01511   bool isODmatch = (itsInputVO[index] == itsVOmatch[index]->getVoRef());
01512   bool isDOmatch = (itsInputVO[index] == itsVOmatch[index]->getVoTest());
01513 
01514   if(isODmatch)
01515     result = itsVOmatch[index]->getMatchImage(d, objOffset1, objOffset2);
01516   else if(isDOmatch)
01517     result = itsVOmatch[index]->getMatchImage(d, objOffset2, objOffset1);
01518   else
01519     {
01520       LINFO("obj[%d] %s : %s, %s",
01521             index, itsInputVO[index]->getName().c_str(),
01522             itsVOmatch[index]->getVoRef()->getName().c_str(),
01523             itsVOmatch[index]->getVoTest()->getName().c_str());
01524       LFATAL("object neither ref nor tst");
01525     }
01526 
01527   return result;
01528 }
01529 
01530 // ######################################################################
01531 Point2D<int> GSlocalizer::getMotorSignal()
01532 {
01533 
01534 //       LINFO("Zero out [%d,%d]", diff.i, diff.j);
01535 //       // horz component: neg: right, pos: left
01536 
01537 //       // Fix: store the object just found
01538 
01539 //       // provide a clue as to where to go next
01540 
01541 
01542   return Point2D<int>(0,0);
01543 }
01544 
01545 // ######################################################################
01546 /* So things look consistent in everyone's emacs... */
01547 /* Local Variables: */
01548 /* indent-tabs-mode: nil */
01549 /* End: */
Generated on Sun May 8 08:40:12 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3