Beobot2_GistSalLocalizerWorker.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_GistSalLocalizerWorker.C $
00034 // $Id: Beobot2_GistSalLocalizerWorker.C 12962 2010-03-06 02:13:53Z irock $
00035 //
00036 
00037 
00038 #include "Robots/Beobot2/Localization/Beobot2_GistSalLocalizerWorker.H"
00039 
00040 #include "Image/MathOps.H"      // for inPlaceNormalize()
00041 #include "Image/CutPaste.H"     // for inplacePaste()
00042 #include "Image/DrawOps.H"      // for drawing
00043 #include "Image/ShapeOps.H"     // for decXY()
00044 #include "Image/MatrixOps.H"    // for matrixMult(), matrixInv(), etc
00045 
00046 #ifndef BEOBOT2_GISTSALLOCALIZERWORKERI_C
00047 #define BEOBOT2_GISTSALLOCALIZERWORKERI_C
00048 
00049 // ######################################################################
00050 Beobot2_GistSalLocalizerWorkerI::Beobot2_GistSalLocalizerWorkerI
00051 (OptionManager& mgr,const std::string& descrName, const std::string& tagName) :
00052   RobotBrainComponent(mgr, descrName, tagName),
00053   itsInputFnum(-1),
00054   itsLastSearchDone(-1)
00055 {
00056   itsWorkerIndex = 0;
00057   itsNumWorkers  = 1;
00058   itsEmptyQueue  = false;
00059 }
00060 
00061 // ######################################################################
00062 void Beobot2_GistSalLocalizerWorkerI::start1()
00063 {
00064   //uint w = 160; uint h = 120;
00065   //itsInputWin.reset(new XWinManaged(Dims(w, h), 0, 2*(h+30),
00066   //                                  "GSL_W:Input" ));
00067 }
00068 
00069 // ######################################################################
00070 Beobot2_GistSalLocalizerWorkerI::~Beobot2_GistSalLocalizerWorkerI()
00071 { }
00072 
00073 // ######################################################################
00074 void Beobot2_GistSalLocalizerWorkerI::registerTopics()
00075 {
00076   this->registerSubscription("LandmarkSearchQueueMessageTopic");
00077   this->registerSubscription("LandmarkMatchResultMessageTopic");
00078   this->registerSubscription("CancelSearchMessageTopic");
00079 
00080   this->registerPublisher("LandmarkMatchResultMessageTopic");
00081   this->registerPublisher("LandmarkSearchStatMessageTopic");
00082   this->registerPublisher("SearchDoneMessageTopic");
00083 }
00084 
00085 // ######################################################################
00086 void Beobot2_GistSalLocalizerWorkerI::evolve()
00087 {
00088   // always check if we are emptying queue first
00089   its_job_queue_mutex.lock();
00090   if(itsEmptyQueue)
00091     {
00092       itsJobQueue.clear();
00093       itsEmptyQueue = false;
00094 
00095       if(itsLastSearchDone < itsInputFnum)
00096         {
00097           BeobotEvents::SearchDoneMessagePtr msg =
00098             new BeobotEvents::SearchDoneMessage;
00099 
00100           LINFO("Publishing SearchDoneMessage");
00101           publish("SearchDoneMessageTopic", msg);
00102           itsLastSearchDone = itsInputFnum;
00103         }
00104     }
00105   bool jobQueueEmpty = itsJobQueue.empty();
00106   its_job_queue_mutex.unlock();
00107 
00108   if (!jobQueueEmpty)
00109     {
00110       // get the next job
00111       its_job_queue_mutex.lock();
00112       bool hasJob = false;
00113       GSlocJobData cjob(0, 0, 0, 0, 0);
00114 
00115       while(!hasJob && !itsJobQueue.empty())
00116         {
00117           // pop jobs itsNumWorkers at a time
00118           for(uint i = 0; i < itsNumWorkers; i++)
00119             {
00120               if(i == itsWorkerIndex)
00121                 { cjob = itsJobQueue.front(); hasJob = true; }
00122               if(!itsJobQueue.empty())
00123                 itsJobQueue.pop_front();
00124               else
00125                 i = itsNumWorkers;
00126             }
00127 
00128           // if the current object has been found, skip
00129           its_results_mutex.lock();
00130           if(itsMatchFound[cjob.objNum]) hasJob = false;
00131           its_results_mutex.unlock();
00132         }
00133       its_job_queue_mutex.unlock();
00134 
00135       if(hasJob) compute(cjob);
00136       if(itsJobQueue.empty()) itsEmptyQueue = true;
00137     }
00138   else itsEmptyQueue = false;
00139 }
00140 
00141 // ######################################################################
00142 void Beobot2_GistSalLocalizerWorkerI::updateMessage
00143 (const RobotSimEvents::EventMessagePtr& eMsg, const Ice::Current&)
00144 {
00145   // Get a gist-sal message
00146   if(eMsg->ice_isA("::BeobotEvents::LandmarkSearchQueueMessage"))
00147   {
00148     BeobotEvents::LandmarkSearchQueueMessagePtr lsqMsg =
00149       BeobotEvents::LandmarkSearchQueueMessagePtr::dynamicCast(eMsg);
00150 
00151     //Get the current request ID
00152     int currRequestID = lsqMsg->RequestID;
00153     itsInputFnum = currRequestID;
00154 
00155     LINFO("Got a lsqMessage with Request ID = %d", currRequestID);
00156 
00157     // get the inputImage
00158     its_input_info_mutex.lock();
00159     itsInputImage = Ice2Image<PixRGB<byte> >(lsqMsg->currIma);
00160     //itsInputWin->setTitle(sformat("WM: %d",itsInputFnum).c_str());
00161     //itsInputWin->drawImage(itsInputImage, 0, 0);
00162 
00163     // get the salient region information
00164     itsInputVO.clear();
00165     itsVOKeypointsComputed.clear();
00166     itsInputObjOffset.clear();
00167     uint inputSize = lsqMsg->salientRegions.size();
00168     for(uint i = 0; i < inputSize; i++)
00169       {
00170         BeobotEvents::SalientRegion salReg = lsqMsg->salientRegions[i];
00171         LDEBUG("W[%4d] sp[%4d,%4d] rect[%4d,%4d,%4d,%4d]",
00172                i, salReg.salpt.i, salReg.salpt.j,
00173                salReg.objRect.tl.i, salReg.objRect.tl.j,
00174                salReg.objRect.br.i, salReg.objRect.br.j);
00175 
00176         // print the pre-attentive feature vector
00177         std::vector<float> features;
00178         uint fsize = salReg.salFeatures.size();
00179         for(uint j = 0; j < fsize; j++)
00180           {
00181             features.push_back(salReg.salFeatures[j]);
00182             LDEBUG("[%4d]:%7f", j, salReg.salFeatures[j]);
00183           }
00184 
00185         Point2D<int> salpt(salReg.salpt.i, salReg.salpt.j);
00186         Point2D<int> offset( salReg.objRect.tl.i, salReg.objRect.tl.j);
00187         Rectangle rect = Rectangle::tlbrO
00188           (salReg.objRect.tl.j, salReg.objRect.tl.i,
00189            salReg.objRect.br.j, salReg.objRect.br.i);
00190 
00191         // create a visual object for the salient region
00192         Image<PixRGB<byte> > objImg = crop(itsInputImage, rect);
00193 
00194         std::string testRunFPrefix("testRunFPrefix");
00195         std::string iname("iname");
00196         std::string saveFilePath("saveFilePath");
00197 
00198         std::string
00199           iName(sformat("%s_SAL_%07d_%02d",
00200                         testRunFPrefix.c_str(), currRequestID, i));
00201         std::string ifName = iName + std::string(".png");
00202         ifName = saveFilePath + ifName;
00203         rutz::shared_ptr<VisualObject>
00204           vo(new VisualObject
00205              (iName, ifName, objImg, salpt - offset, features,
00206               std::vector< rutz::shared_ptr<Keypoint> >(), false, false));
00207         itsInputVO.push_back(vo);
00208         itsVOKeypointsComputed.push_back(false);
00209         itsInputObjOffset.push_back(offset);
00210 
00211         LDEBUG("[%d] image[%d]: %s sal:[%d,%d] offset:[%d,%d]",
00212                currRequestID, i, iName.c_str(),
00213                (salpt - offset).i, (salpt - offset).j,
00214                offset.i, offset.j);
00215       }
00216     its_input_info_mutex.unlock();
00217 
00218     its_results_mutex.lock();
00219     itsMatchFound.clear();
00220     itsVOmatch.clear();         itsVOmatch.resize(inputSize);
00221     itsLmkMatch.clear();        itsLmkMatch.resize(inputSize);
00222     itsSegNumMatch.clear();     itsSegNumMatch.resize(inputSize);
00223     itsLenTravMatch.clear();    itsLenTravMatch.resize(inputSize);
00224     itsNumObjectSearch.clear(); itsNumObjectSearch.resize(inputSize);
00225     for(uint i = 0; i < inputSize; i++) itsMatchFound.push_back(false);
00226     for(uint i = 0; i < inputSize; i++) itsNumObjectSearch[i] = 0;
00227     itsNumJobsProcessed = 0;
00228     its_results_mutex.unlock();
00229 
00230     // fill the job queue
00231     its_job_queue_mutex.lock();
00232     itsJobQueue.clear();
00233     uint njobs = lsqMsg->jobs.size();
00234     for(uint i = 0; i < njobs; i++)
00235       {
00236         BeobotEvents::LandmarkSearchJob tempJob = lsqMsg->jobs[i];
00237         itsJobQueue.push_back
00238           (GSlocJobData(tempJob.inputSalRegID,
00239                         tempJob.dbSegNum,
00240                         tempJob.dbLmkNum,
00241                         tempJob.dbVOStart,
00242                         tempJob.dbVOEnd));
00243       }
00244 
00245     // print the job queue
00246     std::list<GSlocJobData>::iterator itr = itsJobQueue.begin();
00247     uint count = 0;
00248     while (itr != itsJobQueue.end())
00249       {
00250         LDEBUG("[%5d] match obj[%d] lDB[%3d][%3d]:[%3d,%3d]", count,
00251                (*itr).objNum, (*itr).segNum, (*itr).lmkNum,
00252                (*itr).voStartNum,(*itr).voEndNum);
00253         itr++; count++;
00254       }
00255     its_job_queue_mutex.unlock();
00256   }
00257 
00258   // Got a landmark match results - stop searching for that salient region
00259   else if(eMsg->ice_isA("::BeobotEvents::LandmarkMatchResultMessage"))
00260   {
00261     BeobotEvents::LandmarkMatchResultMessagePtr lmrMsg =
00262       BeobotEvents::LandmarkMatchResultMessagePtr::dynamicCast(eMsg);
00263 
00264     //Get the current request ID
00265     //int currRequestID = gistSalMsg->RequestID;
00266 
00267     BeobotEvents::LandmarkSearchJob tempJob = lmrMsg->matchInfo;
00268     LINFO("Got an lmrMessage");
00269 
00270     LINFO("LMR -> found match[%d]: with itsLandmarkDB[%d][%d]",
00271           tempJob.inputSalRegID, tempJob.dbSegNum, tempJob.dbLmkNum);
00272 
00273     its_results_mutex.lock();
00274     if(!itsMatchFound[tempJob.inputSalRegID])
00275       {
00276         itsMatchFound[tempJob.inputSalRegID]   = true;
00277         itsSegNumMatch[tempJob.inputSalRegID]  = lmrMsg->segNumMatch;
00278         itsLenTravMatch[tempJob.inputSalRegID] = lmrMsg->lenTravMatch;
00279       }
00280     its_results_mutex.unlock();
00281   }
00282 
00283   else if(eMsg->ice_isA("::BeobotEvents::CancelSearchMessage"))
00284     {
00285       its_job_queue_mutex.lock();
00286       itsEmptyQueue = true;
00287       its_job_queue_mutex.unlock();
00288 
00289       its_results_mutex.lock();
00290       LINFO("CancelSearchMessage: %d processed here", itsNumJobsProcessed);
00291       its_results_mutex.unlock();
00292     }
00293 }
00294 
00295 // ######################################################################
00296 void Beobot2_GistSalLocalizerWorkerI::setEnvironment
00297 (rutz::shared_ptr<Environment> env)
00298 {
00299   itsEnvironment = env;
00300 
00301   //! from its environment: topological map
00302   itsTopologicalMap = env->getTopologicalMap();
00303 
00304   //! from its environment: visual landmark database
00305   itsLandmarkDB = env->getLandmarkDB();
00306 }
00307 
00308 // ######################################################################
00309 void Beobot2_GistSalLocalizerWorkerI::setWorkerInformation
00310 (uint index, uint totalNumWorkers)
00311 {
00312   itsWorkerIndex = index;
00313   itsNumWorkers = totalNumWorkers;
00314 }
00315 
00316 // ######################################################################
00317 void Beobot2_GistSalLocalizerWorkerI::compute(GSlocJobData cjob)
00318 {
00319   LDEBUG("T[%4d] match object[%d] itsLandmarkDB[%d][%d]: [ %d, %d ]",
00320          itsWorkerIndex, cjob.objNum, cjob.segNum, cjob.lmkNum,
00321          cjob.voStartNum, cjob.voEndNum);
00322 
00323   its_input_info_mutex.lock();
00324   uint nvo = itsInputVO.size();
00325   its_input_info_mutex.unlock();
00326 
00327   if(nvo <= uint(cjob.objNum) ||
00328      itsLandmarkDB->getNumSegment() <= uint(cjob.segNum) ||
00329      itsLandmarkDB->getNumSegLandmark(cjob.segNum) <= uint(cjob.lmkNum) ||
00330      cjob.voStartNum < 0 ||
00331      itsLandmarkDB->getLandmark(cjob.segNum, cjob.lmkNum)->numObjects()
00332      <= uint(cjob.voStartNum) ||
00333      cjob.voEndNum < 0 ||
00334      itsLandmarkDB->getLandmark(cjob.segNum, cjob.lmkNum)->numObjects()
00335      <= uint(cjob.voEndNum))
00336     {
00337       LINFO("Invalid job[%4d] object[%d] itsLandmarkDB[%d][%d]: [ %d, %d ]",
00338              itsWorkerIndex, cjob.objNum, cjob.segNum, cjob.lmkNum,
00339              cjob.voStartNum, cjob.voEndNum);
00340       return;
00341     }
00342 
00343   // make sure the VO keypoints are computed
00344   its_input_info_mutex.lock();
00345   if(!itsVOKeypointsComputed[cjob.objNum])
00346     {
00347       itsInputVO[cjob.objNum]->computeKeypoints();
00348       itsVOKeypointsComputed[cjob.objNum] = true;
00349     }
00350   its_input_info_mutex.unlock();
00351 
00352   // match the object with the range of database
00353   // we limit the maxScale range to [2/3 ... 3/2] by entering 1.5
00354   // Now (2/10/2010):
00355   // we limit the maxScale range to [3/4 ... 4/3] by entering 1.33
00356   its_input_info_mutex.lock();
00357   rutz::shared_ptr<VisualObjectMatch> cmatch;
00358   int ind = itsLandmarkDB->getLandmark(cjob.segNum, cjob.lmkNum)->
00359     match(itsInputVO[cjob.objNum],
00360           cmatch, cjob.voStartNum, cjob.voEndNum,
00361           15.0F, 0.5F, 2.5F, 4, M_PI/4, 1.33F, .25F);
00362   its_input_info_mutex.unlock();
00363 
00364   // if match is found
00365   uint nObjSearch = 0;
00366   if(ind != -1)
00367     {
00368       LINFO("-> found match[%d]: %s with itsLandmarkDB[%d][%d]\n %s : %s",
00369             cjob.objNum, itsInputVO[cjob.objNum]->getName().c_str(),
00370             cjob.segNum, cjob.lmkNum,
00371             cmatch->getVoRef()->getName().c_str(),
00372             cmatch->getVoTest()->getName().c_str());
00373       nObjSearch = (ind - cjob.voStartNum + 1);
00374 
00375       // store the match information
00376       // since there is a possibility that there are concurrent threads
00377       // that also just found it we will choose the first match
00378       its_results_mutex.lock();
00379       itsVOmatch[cjob.objNum] = cmatch;
00380       itsLmkMatch[cjob.objNum] =
00381         GSlocJobData(cjob.objNum, cjob.segNum, cjob.lmkNum, ind, ind);
00382       itsSegNumMatch[cjob.objNum] = cjob.segNum;
00383       itsLenTravMatch[cjob.objNum] =
00384         itsLandmarkDB->getLenTrav(cjob.segNum, cjob.lmkNum, ind);
00385       itsMatchFound[cjob.objNum] = true;
00386       its_results_mutex.unlock();
00387 
00388       // send the result back to GSLoc_master
00389       BeobotEvents::LandmarkMatchResultMessagePtr msg =
00390         new BeobotEvents::LandmarkMatchResultMessage;
00391 
00392       msg->RequestID = itsInputFnum;
00393       msg->voMatchImage =
00394         Image2Ice(getMatchImage(cjob.objNum, itsInputImage.getDims()));
00395       BeobotEvents::LandmarkSearchJob tempJob;
00396       tempJob.inputSalRegID  = cjob.objNum;
00397       tempJob.dbSegNum       = cjob.segNum;
00398       tempJob.dbLmkNum       = cjob.lmkNum;
00399       tempJob.dbVOStart      = ind;
00400       tempJob.dbVOEnd        = ind;
00401       msg->matchInfo = tempJob;
00402 
00403       // location of the matched database salient region
00404       msg->segNumMatch = cjob.segNum;
00405       msg->lenTravMatch = itsLenTravMatch[cjob.objNum];
00406 
00407       LINFO("Publishing lmrMessage with ID: %d[%4d,%10.7f]",
00408             itsInputFnum, msg->segNumMatch, msg->lenTravMatch);
00409       publish("LandmarkMatchResultMessageTopic", msg);
00410     }
00411   else
00412     {
00413       nObjSearch = cjob.voEndNum - cjob.voStartNum + 1;
00414     }
00415 
00416     BeobotEvents::LandmarkSearchStatMessagePtr msg =
00417       new BeobotEvents::LandmarkSearchStatMessage;
00418 
00419     msg->RequestID     = itsInputFnum;
00420     msg->inputSalRegID = cjob.objNum;
00421     msg->numObjSearch  = nObjSearch;
00422     msg->found         = (ind != -1);
00423 
00424     LDEBUG("Publishing LandmarkSearchStatMessage");
00425     publish("LandmarkSearchStatMessageTopic", msg);
00426 
00427     its_results_mutex.lock();
00428     itsNumJobsProcessed++;
00429     its_results_mutex.unlock();
00430 }
00431 
00432 // ######################################################################
00433 Image<PixRGB<byte> >
00434 Beobot2_GistSalLocalizerWorkerI::getMatchImage(uint index, Dims d)
00435 {
00436   Image< PixRGB<byte> > result;
00437 
00438   its_results_mutex.lock();
00439   uint size = itsMatchFound.size();
00440   its_results_mutex.unlock();
00441 
00442   ASSERT(index < size);
00443   Point2D<int> objOffset1 = itsInputObjOffset[index];
00444   Point2D<int> objOffset2 =
00445     itsLandmarkDB->getLandmark(itsLmkMatch[index].segNum,
00446                                itsLmkMatch[index].lmkNum)
00447     ->getOffsetCoords(itsLmkMatch[index].voStartNum);
00448 
00449   bool isODmatch = (itsInputVO[index] == itsVOmatch[index]->getVoRef());
00450   bool isDOmatch = (itsInputVO[index] == itsVOmatch[index]->getVoTest());
00451 
00452   if(isODmatch)
00453     result = itsVOmatch[index]->getMatchImage(d, objOffset1, objOffset2);
00454   else if(isDOmatch)
00455     result = itsVOmatch[index]->getMatchImage(d, objOffset2, objOffset1);
00456   else
00457     {
00458       LINFO("obj[%d] %s : %s, %s",
00459             index, itsInputVO[index]->getName().c_str(),
00460             itsVOmatch[index]->getVoRef()->getName().c_str(),
00461             itsVOmatch[index]->getVoTest()->getName().c_str());
00462       LFATAL("object neither ref nor tst");
00463     }
00464 
00465   return result;
00466 }
00467 
00468 #endif
00469 
00470 // ######################################################################
00471 /* So things look consistent in everyone's emacs... */
00472 /* Local Variables: */
00473 /* indent-tabs-mode: nil */
00474 /* End: */
00475 
Generated on Sun May 8 08:41:18 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3