beobot-GSnav-dorsal.C

Go to the documentation of this file.
00001 /*!@file Beobot/beobot-GSnav-dorsal.C Robot navigation using saliency and gist.
00002   Run beobot-GSnav-master at CPU_A to run Gist-Saliency model
00003   Run beobot-GSnav        at CPU_B to run SIFT                          */
00004 
00005 // //////////////////////////////////////////////////////////////////// //
00006 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00007 // University of Southern California (USC) and the iLab at USC.         //
00008 // See http://iLab.usc.edu for information about this project.          //
00009 // //////////////////////////////////////////////////////////////////// //
00010 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00011 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00012 // in Visual Environments, and Applications'' by Christof Koch and      //
00013 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00014 // pending; application number 09/912,225 filed July 23, 2001; see      //
00015 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00016 // //////////////////////////////////////////////////////////////////// //
00017 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00018 //                                                                      //
00019 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00020 // redistribute it and/or modify it under the terms of the GNU General  //
00021 // Public License as published by the Free Software Foundation; either  //
00022 // version 2 of the License, or (at your option) any later version.     //
00023 //                                                                      //
00024 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00025 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00026 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00027 // PURPOSE.  See the GNU General Public License for more details.       //
00028 //                                                                      //
00029 // You should have received a copy of the GNU General Public License    //
00030 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00031 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00032 // Boston, MA 02111-1307 USA.                                           //
00033 // ///////////////////////////////////////////////////////////////////////
00034 //
00035 // Primary maintainer for this file: Christian Siagian <siagian@usc.edu>
00036 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Beobot/beobot-GSnav-dorsal.C $
00037 // $Id: beobot-GSnav-dorsal.C 12782 2010-02-05 22:14:30Z irock $
00038 //
00039 //////////////////////////////////////////////////////////////////////////
00040 // beobot-Gist-Sal-Nav.C <input_train.txt>
00041 //
00042 //
00043 // This is an on-going project for biologically-plausible
00044 // mobile-robotics navigation.
00045 // It accepts any inputs: video  clip <input.mpg>, camera feed, frames.
00046 //
00047 // The system uses Gist to recognize places and saliency
00048 // to get better localization within the place
00049 // The program also is made to be streamline for fast processing using
00050 // parallel computation. That is the V1 features of different channels are
00051 // computed in parallel
00052 //
00053 // Currently it is able to recognize places through the use of gist features.
00054 // The place classifier uses a neural networks,
00055 // passed in a form of <input_train.txt> -
00056 // the same file is used in the training phase by train-FFN.C.
00057 //
00058 // Related files of interest: GistEstimator.C (and .H) and
00059 // GistEstimatorConfigurator.C (and .H) used by Brain.C to compute
00060 // the gist features.
00061 // test-Gist.C uses GistEstimator to extract gist features from an image.
00062 //
00063 // In parallel we use saliency to get a better spatial resolution
00064 // as well as better place accuracy. The saliency model is used to obtain
00065 // salient locations. We then use ShapeEstimator algorithm to segment out
00066 // the sub-region to get a landmark. Using SIFT we can identify the object,
00067 // create a database, etc.
00068 //
00069 // for localization, path planning we perform landmark-hopping
00070 // to get to the final destination
00071 //
00072 //
00073 //
00074 
00075 #include "Image/OpenCVUtil.H"  // must be first to avoid conflicting defs of int64, uint64
00076 
00077 #include "Beowulf/Beowulf.H"
00078 #include "Component/ModelManager.H"
00079 #include "Raster/Raster.H"
00080 #include "GUI/XWinManaged.H"
00081 #include "Image/Image.H"
00082 #include "Image/Pixels.H"
00083 #include "Util/Timer.H"
00084 
00085 #include "Beobot/beobot-GSnav-def.H"
00086 #include "Beobot/BeobotBrainMT.H"
00087 #include <signal.h>
00088 
00089 #include "Image/ShapeOps.H"
00090 #include "Image/CutPaste.H"     // for inplacePaste()
00091 #include "Image/MathOps.H"      // for findMax
00092 #include "Image/DrawOps.H"
00093 
00094 #define WINSIZE           7
00095 #define templThresh       2000.0F
00096 
00097 static bool goforever = true;  //!< Will turn false on interrupt signal
00098 
00099 // ######################################################################
00100 // ######################################################################
00101 
00102 //! get the request sent by Visual Cortex
00103 void getTrackCommand
00104 ( TCPmessage &rmsg, int32 rframe, int32 rnode, int32 raction,
00105   ImageSet<float> &cmap,
00106   bool &resetCurrLandmark, std::vector<Point2D<int> > &clmpt,
00107   bool &resetNextLandmark, std::vector<Point2D<int> > &nlmpt);
00108 
00109 //! process the request of the Visual Cortex
00110 void processTrackCommand
00111 ( ImageSet<float> cmap,
00112   std::vector<ImageSet<float> > &clmbias, std::vector<Point2D<int> > &clmbiasOffset,
00113   std::vector<ImageSet<float> > &nlmbias, std::vector<Point2D<int> > &nlmbiasOffset,
00114   bool resetCurrLandmark,
00115   std::vector<Point2D<int> > &clmpt, std::vector<Point2D<int> > &prevClmptsc,
00116   bool resetNextLandmark,
00117   std::vector<Point2D<int> > &nlmpt, std::vector<Point2D<int> > &prevNlmptsc,
00118   rutz::shared_ptr<XWinManaged> dispWin);
00119 
00120 //! setup the tracking result packet to be sent to Visual Cortex
00121 void setupTrackingResultPacket
00122 (TCPmessage  &smsg, int rframe,
00123  std::vector<Point2D<int> > clmpt, std::vector<Point2D<int> > nlmpt);
00124 
00125 //! resets the bias template to the region around the point passed
00126 ImageSet<float> setNewBias
00127 ( Point2D<int> inTrackLoc, Point2D<int> &biasOffset, ImageSet<float> cmap,
00128   rutz::shared_ptr<XWinManaged> dispWin);
00129 
00130 //! track the object at the point
00131 Point2D<int> trackPoint
00132 ( ImageSet<float> cmap, ImageSet<float> &bias, Point2D<int> biasOffset,
00133   Point2D<int> trackLoc, rutz::shared_ptr<XWinManaged> dispWin);
00134 
00135 //! update the template used for tracking
00136 void updateTemplate
00137 ( Point2D<int> upLeft, ImageSet<float> cmap, ImageSet<float> &bias,
00138   rutz::shared_ptr<XWinManaged> dispWin);
00139 
00140 //! get the biased saliency map for tracking
00141 Image<float> getBiasedSMap(ImageSet<float> cmap, ImageSet<float> bias,
00142                            rutz::shared_ptr<XWinManaged> dispWin);
00143 
00144 // ######################################################################
00145 //! Signal handler (e.g., for control-C)
00146 void terminate(int s) { LERROR("*** INTERRUPT ***"); goforever = false; }
00147 
00148 // ######################################################################
00149 int main(const int argc, const char **argv)
00150 {
00151   MYLOGVERB = LOG_INFO;
00152 
00153   // instantiate a model manager:
00154   ModelManager manager("beobot Navigation using Gist and Saliency - Dorsal");
00155 
00156   // Instantiate our various ModelComponents:
00157   nub::soft_ref<Beowulf>
00158     beo(new Beowulf(manager, "Beowulf Slave", "BeowulfSlave", false));
00159   manager.addSubComponent(beo);
00160 
00161   // Parse command-line:
00162   if (manager.parseCommandLine(argc, argv, "", 0, 0) == false)
00163     return(1);
00164 
00165   // setup signal handling:
00166   signal(SIGHUP,  terminate); signal(SIGINT,  terminate);
00167   signal(SIGQUIT, terminate); signal(SIGTERM, terminate);
00168 
00169   TCPmessage rmsg;            // message being received and to process
00170   TCPmessage smsg;            // message being sent
00171   int32 rframe, raction, rnode = -1;
00172 
00173   // let's get all our ModelComponent instances started:
00174   manager.start();
00175 
00176   // receive the image dimension first
00177   while(!beo->receive(rnode, rmsg, rframe, raction, 5));
00178   LINFO("Dorsal size: %d", rmsg.getSize());
00179   const int fstart = int(rmsg.getElementInt32());
00180   const int w = int(rmsg.getElementInt32());
00181   const int h = int(rmsg.getElementInt32());
00182   const int opMode = int(rmsg.getElementInt32());
00183   int nSegment = int(rmsg.getElementInt32());
00184   int currSegNum = int(rmsg.getElementInt32());
00185   std::string envFName = rmsg.getElementString();
00186   std::string testRunFPrefix = rmsg.getElementString();
00187   std::string saveFilePath  = rmsg.getElementString();
00188 
00189   int ldpos = envFName.find_last_of('.');
00190   std::string testRunEnvFName = envFName.substr(0, ldpos) +
00191     std::string("-") + testRunFPrefix + std::string(".env");
00192 
00193   LINFO("fstart: %d", fstart);
00194   LINFO("envFName: %s", envFName.c_str());
00195   LINFO("Where we save data: %s", saveFilePath.c_str());
00196   LINFO("test run prefix: %s", testRunFPrefix.c_str());
00197   LINFO("testRunEnvFName: %s", testRunEnvFName.c_str());
00198   LINFO("Image dimension %d by %d", w,h);
00199   switch(opMode)
00200     {
00201     case TRAIN_MODE:
00202       LINFO("number of segments: %d", nSegment);
00203       LINFO("current segment: %d", currSegNum);
00204       LINFO("TRAIN_MODE: build landmark DB");
00205       break;
00206     case TEST_MODE:
00207       LINFO("TEST_MODE: let's roll");
00208       break;
00209     default: LERROR("Unknown operation mode");
00210     }
00211   rmsg.reset(rframe, raction);
00212 
00213   // send a message of ready to go
00214   smsg.reset(rframe, INIT_DONE);
00215   beo->send(rnode, smsg);
00216 
00217   // image buffer and window for display:
00218   Image<PixRGB<byte> > disp(w * 4, h, ZEROS); disp += PixRGB<byte>(128);
00219   rutz::shared_ptr<XWinManaged> dispWin;
00220   //dispWin.reset(new XWinManaged(Dims(w*4,h), 0, 0, "Dorsal display"));
00221 
00222   // tracker and current results
00223   std::vector<ImageSet<float> > clmbias(NUM_CHANNELS);
00224   std::vector<Point2D<int> > clmbiasOffset(NUM_CHANNELS);
00225   std::vector<ImageSet<float> > nlmbias(NUM_CHANNELS);
00226   std::vector<Point2D<int> > nlmbiasOffset(NUM_CHANNELS);
00227   std::vector<Point2D<int> > prevClmptsc;
00228   std::vector<Point2D<int> > prevNlmptsc;
00229 
00230   // MAIN LOOP
00231   LINFO("MAIN LOOP");
00232   Timer tim(1000000); uint64 t[NAVG]; float frate = 0.0f; uint fcount = 0;
00233   while(goforever)
00234     {
00235       // if we get a new message from V1
00236       // check the type of action to take with the data
00237       if(beo->receive(rnode, rmsg, rframe, raction, 1))
00238         {
00239           // abort: break out of the loop and finish the operation
00240           if(raction == ABORT)
00241             { LINFO("done: BREAK"); goforever = false; break; }
00242 
00243           // tracking task
00244           if(raction == TRACK_LM)
00245             {
00246               LINFO("message received: TRACK_LM");
00247 
00248               tim.reset();
00249               // get all the Visual Cortex information
00250               // FIX: need the sal map??
00251               ImageSet<float> cmap;
00252               bool resetCurrLandmark, resetNextLandmark;
00253               std::vector<Point2D<int> > clmpt;   std::vector<Point2D<int> > nlmpt;
00254               //std::vector<Point2D<int> > clmptsc; std::vector<Point2D<int> > nlmptsc;
00255               getTrackCommand
00256                 (rmsg, rframe, rnode, raction, cmap,
00257                  resetCurrLandmark, clmpt, resetNextLandmark, nlmpt);
00258 
00259               // process the tracking task
00260               processTrackCommand
00261                 (cmap, clmbias, clmbiasOffset, nlmbias, nlmbiasOffset,
00262                  resetCurrLandmark, clmpt, prevClmptsc,
00263                  resetNextLandmark, nlmpt, prevNlmptsc, dispWin);
00264 
00265               // return the tracking results
00266               setupTrackingResultPacket(smsg, rframe, clmpt, nlmpt);
00267               beo->send(rnode, smsg);
00268 
00269               LINFO("done TRACK_LM\n");
00270 
00271               // compute and show framerate over the last NAVG frames:
00272               t[fcount % NAVG] = tim.get(); fcount++; tim.reset();
00273               if (fcount % NAVG == 0 && fcount > 0)
00274                 {
00275                   uint64 avg = 0ULL;
00276                   for(int i = 0; i < NAVG; i ++) avg += t[i];
00277                   frate = 1000000.0F / float(avg) * float(NAVG);
00278                   LINFO("[%6d] Frame rate: %6.3f fps -> %8.3f ms/frame",
00279                         fcount, frate, 1000.0/frate);
00280                 }
00281             }
00282         }
00283     }
00284 
00285   // Received abort signal
00286   smsg.reset(rframe, raction);
00287   beo->send(rnode, smsg);
00288   LINFO("received ABORT signal");
00289 
00290   // ending operations
00291   switch(opMode)
00292     {
00293     case TRAIN_MODE:
00294       break;
00295 
00296     case TEST_MODE:
00297       // report some statistical data
00298       break;
00299     default:
00300       LERROR("Unknown operation mode");
00301     }
00302 
00303   // we got broken:
00304   manager.stop();
00305   return 0;
00306 }
00307 
00308 // ######################################################################
00309 void getTrackCommand
00310 ( TCPmessage &rmsg, int32 rframe, int32 rnode, int32 raction,
00311   ImageSet<float> &cmap,
00312   bool &resetCurrLandmark, std::vector<Point2D<int> > &clmpt,
00313   bool &resetNextLandmark, std::vector<Point2D<int> > &nlmpt )
00314 {
00315   cmap = rmsg.getElementFloatImaSet();
00316 
00317   int tempbool = int(rmsg.getElementInt32());
00318   if (tempbool == 0) resetCurrLandmark = false;
00319   else  resetCurrLandmark = true;
00320 
00321   tempbool = int(rmsg.getElementInt32());
00322   if (tempbool == 0) resetNextLandmark = false;
00323   else  resetNextLandmark = true;
00324 
00325   LINFO("[%4d] reset currLM? %d nextLM? %d",
00326         rframe, resetCurrLandmark, resetNextLandmark);
00327 
00328   uint nClmpt = int(rmsg.getElementInt32());
00329   clmpt.clear();
00330   for(uint i = 0; i < nClmpt; i++)
00331     {
00332       uint cI = int(rmsg.getElementInt32());
00333       uint cJ = int(rmsg.getElementInt32());
00334       clmpt.push_back(Point2D<int>(cI,cJ));
00335       LINFO("currLM[%d]: (%3d, %3d)", i, clmpt[i].i, clmpt[i].j);
00336     }
00337 
00338   uint nNlmpt = int(rmsg.getElementInt32());
00339   nlmpt.clear();
00340   for(uint i = 0; i < nNlmpt; i++)
00341     {
00342       uint nI = int(rmsg.getElementInt32());
00343       uint nJ = int(rmsg.getElementInt32());
00344       nlmpt.push_back(Point2D<int>(nI,nJ));
00345       LINFO("nextLM[%d]: (%3d, %3d)", i, nlmpt[i].i, nlmpt[i].j);
00346     }
00347 
00348   rmsg.reset(rframe, raction);
00349 }
00350 
00351 // ######################################################################
00352 void processTrackCommand
00353 ( ImageSet<float> cmap,
00354   std::vector<ImageSet<float> > &clmbias, std::vector<Point2D<int> > &clmbiasOffset,
00355   std::vector<ImageSet<float> > &nlmbias, std::vector<Point2D<int> > &nlmbiasOffset,
00356   bool resetCurrLandmark,
00357   std::vector<Point2D<int> > &clmpt, std::vector<Point2D<int> > &prevClmptsc,
00358   bool resetNextLandmark,
00359   std::vector<Point2D<int> > &nlmpt, std::vector<Point2D<int> > &prevNlmptsc,
00360   rutz::shared_ptr<XWinManaged> dispWin)
00361 {
00362   int smscale = (int)(pow(2,sml));
00363 
00364   // current landmark points:
00365   if(resetCurrLandmark)
00366     { prevClmptsc.clear(); clmbias.clear(); clmbiasOffset.clear(); }
00367   for(uint i = 0; i < clmpt.size(); i++)
00368     {
00369 
00370       // if we are resetting current landmark
00371       if(resetCurrLandmark)
00372         {
00373           if(!clmpt[i].isValid())  LFATAL("invalid currLmk[%d]", i);
00374           prevClmptsc.push_back(Point2D<int>(clmpt[i].i/smscale,
00375                                              clmpt[i].j/smscale));
00376           Point2D<int> tempOffset;
00377           clmbias.push_back(setNewBias(prevClmptsc[i], tempOffset, cmap,
00378                                        dispWin));
00379           clmbiasOffset.push_back(tempOffset);
00380         }
00381       // else we are tracking (pt still not lost)
00382       else if(prevClmptsc[i].isValid())
00383         {
00384           LINFO("tracking current Landmark[%d]", i);
00385           prevClmptsc[i] = trackPoint
00386             (cmap, clmbias[i], clmbiasOffset[i], prevClmptsc[i], dispWin);
00387         }
00388       // else it's previously lost
00389       else { LINFO("lost current Landmark[%d]", i); }
00390 
00391       if(prevClmptsc[i].isValid())
00392         clmpt[i] = Point2D<int>(prevClmptsc[i].i*smscale, prevClmptsc[i].j*smscale);
00393       else  clmpt[i] = Point2D<int>(-1,-1);
00394       LINFO("current landmark[%d] result: [%d,%d] -> [%d,%d]",
00395             i, clmpt[i].i, clmpt[i].j, prevClmptsc[i].i, prevClmptsc[i].j);
00396     }
00397 
00398   // next landmark point:
00399   if(resetNextLandmark)
00400     { prevNlmptsc.clear(); nlmbias.clear(); nlmbiasOffset.clear(); }
00401   for(uint i = 0; i < nlmpt.size(); i++)
00402     {
00403       // if we are resetting and it's a valid point
00404       if(resetNextLandmark)
00405         {
00406           if(!nlmpt[i].isValid())  LFATAL("invalid nextLmk[%d]", i);
00407           prevNlmptsc.push_back(Point2D<int>(nlmpt[i].i/smscale,
00408                                         nlmpt[i].j/smscale));
00409           Point2D<int> tempOffset;
00410           nlmbias.push_back(setNewBias(prevNlmptsc[i], tempOffset, cmap,
00411                                        dispWin));
00412           nlmbiasOffset.push_back(tempOffset);
00413         }
00414       // else we are tracking (pt still not lost)
00415       else if(prevNlmptsc[i].isValid())
00416         {
00417           LINFO("tracking next Landmark[%d]", i);
00418           prevNlmptsc[i] = trackPoint
00419             (cmap, nlmbias[i], nlmbiasOffset[i], prevNlmptsc[i], dispWin);
00420         }
00421       // else it's previously lost
00422       else { LINFO("lost next Landmark[%d]", i); }
00423 
00424       // resulting tracking/resetting
00425       if(prevNlmptsc[i].isValid())
00426         nlmpt[i] = Point2D<int>(prevNlmptsc[i].i*smscale, prevNlmptsc[i].j*smscale);
00427       else  nlmpt[i] = Point2D<int>(-1,-1);
00428       LINFO("next landmark[%d] result: [%d,%d] -> [%d,%d]",
00429             i, nlmpt[i].i, nlmpt[i].j, prevNlmptsc[i].i, prevNlmptsc[i].j);
00430     }
00431 }
00432 
00433 // ######################################################################
00434 void setupTrackingResultPacket
00435 (TCPmessage  &smsg, int rframe,
00436  std::vector<Point2D<int> > clmpt, std::vector<Point2D<int> > nlmpt)
00437 {
00438   smsg.reset(rframe, TRACK_LM_RES);
00439   smsg.addInt32(int32(clmpt.size()));
00440   for(uint i = 0; i < clmpt.size(); i++)
00441     {
00442       smsg.addInt32(int32(clmpt[i].i));
00443       smsg.addInt32(int32(clmpt[i].j));
00444       LINFO("curr[%d]: (%d, %d)", i, clmpt[i].i, clmpt[i].j);
00445     }
00446 
00447   smsg.addInt32(int32(nlmpt.size()));
00448   for(uint i = 0; i < nlmpt.size(); i++)
00449     {
00450       smsg.addInt32(int32(nlmpt[i].i));
00451       smsg.addInt32(int32(nlmpt[i].j));
00452       LINFO("next[%d]: (%d %d)", i, nlmpt[i].i, nlmpt[i].j);
00453     }
00454 }
00455 
00456 // ######################################################################
00457 ImageSet<float> setNewBias
00458 (Point2D<int> inTrackLoc, Point2D<int> &biasOffset, ImageSet<float> cmap,
00459  rutz::shared_ptr<XWinManaged> dispWin)
00460 {
00461   int w = cmap[0].getWidth();
00462   int h = cmap[0].getHeight();
00463 
00464   ImageSet<float> bias(NUM_CHANNELS);
00465 
00466   // set bias offset
00467   if(inTrackLoc.i < (WINSIZE/2))
00468     biasOffset.i = inTrackLoc.i;
00469   else if(inTrackLoc.i > ((w - 1) - (WINSIZE/2)))
00470     biasOffset.i = WINSIZE - (w - inTrackLoc.i);
00471   else
00472     biasOffset.i = WINSIZE/2;
00473 
00474   if(inTrackLoc.j < (WINSIZE/2))
00475     biasOffset.j = inTrackLoc.j;
00476   else if(inTrackLoc.j > ((h - 1) - (WINSIZE/2)))
00477     biasOffset.j = WINSIZE - (h - inTrackLoc.j);
00478   else
00479     biasOffset.j = WINSIZE/2;
00480 
00481   LINFO("Set new bias[%d,%d]: offset: (%d, %d)",
00482         inTrackLoc.i, inTrackLoc.j, biasOffset.i, biasOffset.j);
00483 
00484   // get the features at the loc point
00485   for(int i = 0; i < NUM_CHANNELS; i++)
00486     {
00487       Point2D<int> upLeftsc(inTrackLoc.i - biasOffset.i,
00488                        inTrackLoc.j - biasOffset.j);
00489       Image<float> target = crop(cmap[i], upLeftsc, Dims(WINSIZE,WINSIZE));
00490       bias[i] = target;
00491 
00492 //       LINFO("bias[%d]", i);
00493 //       int scale = (int)(pow(2,sml));
00494 //       Image<float> disp(2*w*scale, h*scale, ZEROS);
00495 //       Image<float> cmapDisp = zoomXY(cmap[i], scale);
00496 //       Image<float> biasDisp = zoomXY(bias[i], scale);
00497 
00498 //       Point2D<int> sp(inTrackLoc.i*scale, inTrackLoc.j*scale);
00499 //       Point2D<int> upLeft(upLeftsc.i*scale, upLeftsc.j*scale);
00500 //       Rectangle br(upLeft, Dims(WINSIZE*scale, WINSIZE*scale));
00501 
00502 //       float mn, mx; getMinMax(cmapDisp,mn,mx);
00503 //       drawRect(cmapDisp, br, mx);
00504 //       drawDisk(cmapDisp, sp, 3, mn);
00505 
00506 //       inplacePaste(disp, cmapDisp, Point2D<int>(0,0));
00507 //       inplacePaste(disp, biasDisp, Point2D<int>(w*scale, 0) + upLeft);
00508 //       dispWin->drawImage(disp,0,0); Raster::waitForKey();
00509     }
00510   return bias;
00511 }
00512 
00513 // ######################################################################
00514 Point2D<int> trackPoint
00515 ( ImageSet<float> cmap, ImageSet<float> &bias, Point2D<int> biasOffset,
00516   Point2D<int> trackLoc, rutz::shared_ptr<XWinManaged> dispWin)
00517 {
00518   int w = cmap[0].getWidth();
00519   int h = cmap[0].getHeight();
00520 //   int scale = (int)(pow(2,sml));
00521 //   Point2D<int> bOffset(scale*(WINSIZE/2), scale*(WINSIZE/2));
00522 //   Image<PixRGB<byte> > disp(4*w*scale, h*scale, ZEROS);
00523 
00524   // match templates
00525   Image<float> smap = getBiasedSMap(cmap, bias, dispWin);
00526 //   Image<float> smapDisp = zoomXY(smap, scale);
00527 //   float mn, mx; getMinMax(smapDisp, mn, mx); LINFO("mn:%f mx:%f", mn,mx);
00528 //   inplaceNormalize(smapDisp, 0.0F, 255.0F);
00529 //   inplacePaste(disp, Image<PixRGB<byte> >(toRGB(smapDisp)), bOffset);
00530 //   Raster::WriteRGB(Image<PixRGB<byte> >(toRGB(smapDisp)), "smap.ppm");
00531 
00532   // add a value to saliency based on distance from last point
00533   int i = 0; float maxDist = sqrt(w*w + h*h); //LINFO("maxDist: %f", maxDist);
00534   int wsmap = smap.getWidth();
00535 //   int hsmap = smap.getHeight();
00536 //   Image<float> dmap(wsmap, hsmap, ZEROS);
00537 //   Image<float>::iterator itr2 = dmap.beginw();
00538   Point2D<int> prevLoc(trackLoc.i - biasOffset.i, trackLoc.j - biasOffset.j);
00539   for (Image<float>::iterator itr = smap.beginw(), stop = smap.endw();
00540        itr != stop; ++itr, i++)
00541     {
00542       int x = i % wsmap;
00543       int y = i / wsmap;
00544       float dist = (prevLoc.distance(Point2D<int>(x,y))+.1)/maxDist;
00545       *itr = *itr * dist;
00546 
00547 //       *itr2 = dist; ++itr2;
00548     }
00549 
00550 //   Image<float> smapDisp2 = zoomXY(smap, scale);
00551 //   inplaceNormalize(smapDisp2, 0.0F, 255.0F);
00552 //   inplacePaste(disp, Image<PixRGB<byte> >(toRGB(smapDisp2)),
00553 //                Point2D<int>(w*scale,0)+bOffset);
00554 //   Raster::WriteRGB(Image<PixRGB<byte> >(toRGB(smapDisp2)), "smap2.ppm");
00555 
00556   // get the min val location
00557   // since the smap is corroded by WINSIZE all around,
00558   // winner coordinate point is actually the topleft of the bias window
00559   float minval; Point2D<int> upLeft; findMin(smap, upLeft, minval);
00560 
00561 //   Image<float> smapDisp3 = zoomXY(smap, scale);
00562 //   Point2D<int> cwsc(upLeft.i*scale + scale/2, upLeft.j*scale + scale/2);
00563 //   inplaceNormalize(smapDisp3, 0.0F, 255.0F);
00564 //   Image<PixRGB<byte> > csmapDisp3(w*scale, h*scale, ZEROS);
00565 //   inplacePaste(csmapDisp3, Image<PixRGB<byte> >(toRGB(smapDisp3)), bOffset);
00566 //   drawDisk(csmapDisp3, cwsc + bOffset, 3, PixRGB<byte>(255, 0, 0));
00567 //   drawRect(csmapDisp3,
00568 //            Rectangle(upLeft*scale,
00569 //                      Dims((WINSIZE+1)*scale-1, (WINSIZE+1)*scale-1)),
00570 //            PixRGB<byte>(255,0,0));
00571 //   inplacePaste(disp, csmapDisp3, Point2D<int>(2*w*scale,0));
00572 //   Raster::WriteRGB(csmapDisp3, "csmap.ppm");
00573 
00574 //   Image<float> dmapDisp = zoomXY(dmap, scale);
00575 //   inplaceNormalize(dmapDisp, 0.0F, 255.0F);
00576 //   inplacePaste(disp, Image<PixRGB<byte> >(toRGB(dmapDisp)),
00577 //                Point2D<int>(3*w*scale,0)+bOffset);
00578 
00579 //   dispWin->drawImage(disp, 0, 0);
00580 //   Raster::waitForKey();
00581 
00582   // update the template
00583   updateTemplate(upLeft, cmap, bias, dispWin);
00584 
00585   // get new tracking point
00586   Point2D<int> newTrackLoc = upLeft + biasOffset;
00587   return newTrackLoc;
00588 }
00589 
00590 // ######################################################################
00591 void updateTemplate
00592 ( Point2D<int> upLeft, ImageSet<float> cmap, ImageSet<float> &bias,
00593   rutz::shared_ptr<XWinManaged> dispWin)
00594 {
00595   double dist = 0;
00596   ImageSet<float> newBias(NUM_CHANNELS);
00597 
00598   for(int i = 0; i < NUM_CHANNELS; i++)
00599     {
00600       Image<float> target = crop(cmap[i], upLeft, Dims(WINSIZE,WINSIZE));
00601 
00602       // take more of the old template but still incorporate the new template
00603       newBias[i] = bias[i]*0.9 + target*(1 - 0.9);
00604       dist += distance(bias[i], newBias[i]);
00605     }
00606 
00607   // if the difference is too big, then do not update the template
00608   LINFO("Distance %f (thresh: %f)", dist, templThresh);
00609   if (dist < templThresh)
00610     {
00611       bias = newBias;
00612     }
00613   else LINFO("not adding bias");
00614 
00615   // did we lose the tracking completely?
00616   //float winDist = lastWinner.distance(trackLoc);
00617 }
00618 
00619 // ######################################################################
00620 Image<float> getBiasedSMap(ImageSet<float> cmap, ImageSet<float> bias,
00621                            rutz::shared_ptr<XWinManaged> dispWin)
00622 {
00623 #ifndef HAVE_OPENCV
00624   LFATAL("OpenCV must be installed to use this function");
00625   return Image<float>();
00626 #else
00627 
00628   int w = cmap[0].getWidth();
00629   int h = cmap[0].getHeight();
00630   //   int scale = (int)(pow(2,sml));
00631 
00632   Image<float> biasedCMap(w - WINSIZE + 1, h - WINSIZE + 1, ZEROS);
00633   Image<float> res(w - WINSIZE + 1, h - WINSIZE + 1, ZEROS);
00634 
00635   // add the bias of all the channels
00636   for(uint i = 0; i < NUM_CHANNELS; i++)
00637     {
00638       cvMatchTemplate(img2ipl(cmap[i]), img2ipl(bias[i]),
00639                       img2ipl(biasedCMap), CV_TM_SQDIFF); //CV_TM_CCOEFF);
00640 
00641       // biasedCMap = correlation(cmap[i], bias[i]);
00642 
00643 //       LINFO("bias[%d]", i);
00644 //       Image<float> disp(2*w*scale, h*scale, ZEROS);
00645 //       Image<float> cmapDisp = zoomXY(cmap[i], scale);
00646 //       Image<float> biasDisp = zoomXY(biasedCMap, scale);
00647 //       inplacePaste(disp, cmapDisp, Point2D<int>(0,0));
00648 //       inplacePaste(disp, biasDisp, Point2D<int>(w*scale,0));
00649 //       dispWin->drawImage(disp,0,0);
00650 //       Raster::waitForKey();
00651 
00652       // Add to saliency map: //save the cmap
00653       res += biasedCMap;
00654     }
00655 
00656   return res;
00657 
00658 #endif
00659 }
00660 
00661 // ######################################################################
00662 /* So things look consistent in everyone's emacs... */
00663 /* Local Variables: */
00664 /* indent-tabs-mode: nil */
00665 /* End: */
Generated on Sun May 8 08:40:11 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3