TrackFeature.C

Go to the documentation of this file.
00001 /*!@file RCBot/TrackFeature.C track a location using the saliency map and
00002  * template matching                                                    */
00003 // //////////////////////////////////////////////////////////////////// //
00004 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00005 // University of Southern California (USC) and the iLab at USC.         //
00006 // See http://iLab.usc.edu for information about this project.          //
00007 // //////////////////////////////////////////////////////////////////// //
00008 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00009 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00010 // in Visual Environments, and Applications'' by Christof Koch and      //
00011 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00012 // pending; application number 09/912,225 filed July 23, 2001; see      //
00013 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00014 // //////////////////////////////////////////////////////////////////// //
00015 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00016 //                                                                      //
00017 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00018 // redistribute it and/or modify it under the terms of the GNU General  //
00019 // Public License as published by the Free Software Foundation; either  //
00020 // version 2 of the License, or (at your option) any later version.     //
00021 //                                                                      //
00022 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00023 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00024 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00025 // PURPOSE.  See the GNU General Public License for more details.       //
00026 //                                                                      //
00027 // You should have received a copy of the GNU General Public License    //
00028 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00029 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00030 // Boston, MA 02111-1307 USA.                                           //
00031 // //////////////////////////////////////////////////////////////////// //
00032 //
00033 // Primary maintainer for this file: Lior Elazary <lelazary@yahoo.com>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/RCBot/TrackFeature.C $
00035 // $Id: TrackFeature.C 9412 2008-03-10 23:10:15Z farhan $
00036 //
00037 
00038 #include "RCBot/TrackFeature.H"
00039 
00040 //#include "GUI/XWinManaged.H"
00041 //XWinManaged xwin(Dims(320,240), -1, -1, "Track Feature");
00042 
00043 // ######################################################################
00044 TrackFeature::TrackFeature(nub::ref<SaliencyMT> insmt) :
00045   smt(insmt), trackLoc(-1,-1), bias(14), newBias(14)
00046 {
00047   doneTracking = false;
00048   avgtime = 0;
00049   avgn = 0;
00050   fps = 0.0F;
00051 
00052   templThresh = 50.0F;
00053 
00054   width = -1; height = -1;
00055   timer.reset();
00056   start();
00057 }
00058 
00059 // ######################################################################
00060 TrackFeature::~TrackFeature(){}
00061 
00062 // ######################################################################
00063 void TrackFeature::run(void *ptr)
00064 {
00065   LINFO("Starting track thread");
00066   while(!doneTracking){
00067 
00068     // do we have a point to track?
00069     trackLocMutex.lock();
00070     bool isLocValid = (trackLoc.i > (WINSIZE/2) &&
00071                        trackLoc.i < width - (WINSIZE/2) &&
00072                        trackLoc.j > (WINSIZE/2) &&
00073                        trackLoc.j < height - (WINSIZE/2)
00074                        );
00075     trackLocMutex.unlock();
00076 
00077     if (isLocValid){
00078 
00079       // get the lock and wait for images
00080       trackMutex.lock();
00081       trackCond.wait();
00082       // process the image and track
00083       trackMutex.unlock();
00084 
00085       if (smt->outputReady()){
00086         Image<float> SMap = smt->getOutput();
00087         if (SMap.initialized()){
00088           //xwin.drawImage(SMap);
00089           // add a value to saliency based on distance from last point
00090           int w = SMap.getWidth();
00091           int i = 0;
00092           for (Image<float>::iterator itr = SMap.beginw(), stop = SMap.endw();
00093                itr != stop; ++itr, i++) {
00094             int x = i % w;
00095             int y = i / w;
00096             float dist = lastWinner.distance(Point2D<int>(x,y));
00097             *itr = *itr - (0.5*dist);
00098           }
00099           float maxval; Point2D<int> currWinner; findMin(SMap, currWinner, maxval);
00100 
00101           // since the saliency map is smaller than winsize,
00102           // remap the winner point
00103           currWinner.i = currWinner.i + (WINSIZE-1)/2;
00104           currWinner.j = currWinner.j + (WINSIZE-1)/2;
00105 
00106           trackImgMutex.lock();
00107           smt->newInput(img);
00108           trackImgMutex.unlock();
00109 
00110           trackLocMutex.lock();
00111           trackLoc = currWinner;
00112           trackLocMutex.unlock();
00113 
00114           // update the template
00115           updateTemplate();
00116 
00117           // calculate fps
00118           calcFps();
00119 
00120           lastWinner = currWinner;
00121         }
00122       }
00123 
00124     } else {
00125       trackLocMutex.lock();
00126       trackLoc.i = -1;
00127       trackLoc.j = -1;
00128       trackLocMutex.unlock();
00129     }
00130   }
00131 }
00132 
00133 // ######################################################################
00134 void TrackFeature::updateTemplate()
00135 {
00136   double dist = 0;
00137   trackLocMutex.lock();
00138   Point2D<int> currLoc = trackLoc;
00139   trackLocMutex.unlock();
00140   static int badTrack = 0;
00141 
00142   bool isLocValid = (currLoc.i > (WINSIZE/2) &&
00143                      currLoc.i < width - (WINSIZE/2) &&
00144                      currLoc.j > (WINSIZE/2) &&
00145                      currLoc.j < height - (WINSIZE/2));
00146 
00147   if (isLocValid){
00148     for(int i = 0; i < 14; i++){
00149       if (smt->cmaps[i].initialized()){
00150         if (bias[i].initialized()){
00151           Point2D<int> center_fixation(currLoc.i-((WINSIZE-1)/2),
00152                                   currLoc.j-((WINSIZE-1)/2));
00153           Image<float> target = crop(smt->cmaps[i],
00154                                      center_fixation,
00155                                      Dims(WINSIZE,WINSIZE));
00156 
00157           // take more of the old template but still incorporate the new template
00158           newBias[i] = bias[i]*0.9 + target*(1-0.9);
00159           dist += distance(bias[i], newBias[i]);
00160           bias[i] = newBias[i];
00161         }
00162       }
00163     }
00164   } else {
00165     dist = 999999999;
00166   }
00167 
00168   // if the difference is too big, then do not update the template
00169   if (dist < templThresh){
00170     smt->setSMBias(newBias);
00171     badTrack = 0;
00172   } else {
00173     badTrack++;
00174   }
00175   //LINFO("Distance %f", dist);
00176 
00177   // did we lose the tracking completely?
00178   //float winDist = lastWinner.distance(trackLoc);
00179   //if (dist > templThresh){
00180 
00181   // try 3 times
00182   if (badTrack > 3){ // we lost the tracking
00183     trackLocMutex.lock();
00184     trackLoc.i = -1;
00185     trackLoc.j = -1;
00186     trackLocMutex.unlock();
00187   }
00188 }
00189 
00190 // ######################################################################
00191 Point2D<int> TrackFeature::getTrackLoc()
00192 {
00193   trackLocMutex.lock();
00194   Point2D<int> loc = trackLoc;
00195   trackLocMutex.unlock();
00196 
00197   return loc;
00198 }
00199 
00200 // ######################################################################
00201 void TrackFeature::calcFps()
00202 {
00203   avgtime += timer.getReset(); avgn ++;
00204   if (avgn == NAVG)
00205     {
00206       fps = 1000.0F / float(avgtime) * float(avgn);
00207       avgtime = 0; avgn = 0;
00208     }
00209 }
00210 
00211 // ######################################################################
00212 float TrackFeature::getFps(){ return fps; }
00213 
00214 // ######################################################################
00215 void TrackFeature::setImg(Image<PixRGB<byte> > &inImg)
00216 {
00217   trackImgMutex.lock();
00218   img = inImg;
00219   width = img.getWidth();
00220   height = img.getHeight();
00221   trackImgMutex.unlock();
00222   trackCond.signal();
00223 }
00224 
00225 // ######################################################################
00226 void TrackFeature::setTrackLoc(Point2D<int> &inTrackLoc, Image<PixRGB<byte> > &img)
00227 {
00228   trackLocMutex.lock();
00229   LINFO("Setting tracking location");
00230   bool isLocValid = (inTrackLoc.i > (WINSIZE/2) &&
00231                      inTrackLoc.i < width - (WINSIZE/2) &&
00232                      inTrackLoc.j > (WINSIZE/2) &&
00233                      inTrackLoc.j < height - (WINSIZE/2));
00234   trackLocMutex.unlock();
00235 
00236   if (isLocValid)
00237   {
00238     // discard the currently processed saliency map
00239     // build a new unbiased saliency map, and find the highest local value within it
00240 
00241     LINFO("Wait for smt");
00242     while(!smt->outputReady()){ // wait for any processing to finish
00243       usleep(100);
00244     }
00245     LINFO("Done waiting");
00246 
00247     smt->setBiasSM(false);        // let the system know we don't want an unbiased smap
00248     //smt->setSaliencyMapLevel(0); // set the saliency map level to 0
00249     smt->getSMap(img);
00250 
00251     LINFO("Set biases");
00252     // get the features at the loc point
00253     for(int i=0; i<14; i++){
00254       if (smt->cmaps[i].initialized()){
00255         Point2D<int> center_loc(inTrackLoc.i - ((WINSIZE - 1)/2),
00256                            inTrackLoc.j - ((WINSIZE - 1)/2));
00257         Image<float> target = crop(smt->cmaps[i],center_loc, Dims(WINSIZE,WINSIZE));
00258         bias[i] = target;
00259       }
00260     }
00261 
00262     LINFO("Done setting bias");
00263     smt->setSMBias(bias);
00264     smt->setBiasSM(true);
00265 
00266     trackLocMutex.lock();
00267     trackLoc = inTrackLoc; // update the track location
00268     trackLocMutex.unlock();
00269 
00270     smt->newInput(img);
00271   }
00272 }
00273 
00274 // ######################################################################
00275 /* So things look consistent in everyone's emacs... */
00276 /* Local Variables: */
00277 /* indent-tabs-mode: nil */
00278 /* End: */
Generated on Sun May 8 08:41:17 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3