test-Head.C

Go to the documentation of this file.
00001 /*!@file Demo/test-Head.C Test Head  */
00002 
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 <elazary@usc.edu>
00034 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Demo/test-Head.C $
00035 // $Id: test-Head.C 9412 2008-03-10 23:10:15Z farhan $
00036 //
00037 
00038 //
00039 #include "Component/ModelManager.H"
00040 #include "Devices/DeviceOpts.H"
00041 #include "Devices/FrameGrabberConfigurator.H"
00042 #include "Devices/FrameGrabberFactory.H"
00043 #include "GUI/XWindow.H"
00044 #include "Image/DrawOps.H"
00045 #include "Image/CutPaste.H"
00046 #include "Image/Image.H"
00047 #include "Image/Pixels.H"
00048 #include "Image/MathOps.H"
00049 #include "Neuro/EnvVisualCortex.H"
00050 #include "Util/Timer.H"
00051 #include "Util/log.H"
00052 #include "Learn/Bayes.H"
00053 #include "Devices/BeoHead.H"
00054 #include "Envision/env_image_ops.h"
00055 
00056 #include "GUI/DebugWin.H"
00057 #include <ctype.h>
00058 #include <deque>
00059 #include <iterator>
00060 #include <stdlib.h> // for atoi(), malloc(), free()
00061 #include <string>
00062 #include <vector>
00063 #include <map>
00064 
00065 #define NumFeatureNames 7
00066 const char *featureNames[7] = {
00067         "red/green",
00068         "blue/yellow",
00069         "intensity",
00070         "steerable(01/04)",
00071         "steerable(02/04)",
00072         "steerable(03/04)",
00073         "steerable(04/04)"
00074         };
00075 
00076 
00077 void display(Image<PixRGB<byte> > &leftImg,
00078     const Image<byte> &leftSmap,
00079     const Point2D<int> &leftWinner,
00080     const byte maxVal);
00081 void display(const Image<PixRGB<byte> > &img,
00082     const Image<PixRGB<byte> > &smap, Point2D<int> &winner, Rectangle &rect);
00083 Rectangle getBoundBox();
00084 void showHist(std::vector<double> &hist, int pos);
00085 void showProb(int pos);
00086 void showFeatures(std::vector<double> &fv, int pos);
00087 void findMinMax(std::vector<double> &vec, double &min, double &max);
00088 
00089 ModelManager *mgr;
00090 XWinManaged *xwin;
00091 XWinManaged *debugXWin;
00092 Timer timer;
00093 Image<PixRGB<byte> > disp;
00094 Point2D<int> curWinner;
00095 Point2D<int> learnPos;
00096 Dims imageDims;
00097 Bayes* bayesNet;
00098 byte SmaxVal = 0;
00099 
00100 struct BiasParam
00101 {
00102   double mean;
00103   double var;
00104 };
00105 
00106 #define NBINS 16
00107 #define NFEATURES NBINS
00108 #define FSIZE 75
00109 
00110 std::map<const std::string, std::vector<double> > featuresHist;
00111 std::vector<double> featuresVal(NumFeatureNames,0);
00112 
00113 
00114 enum State {TRAIN, BIAS, MOVE, NO_BIAS};
00115 State state = NO_BIAS;
00116 Point2D<int> lastWinner;
00117 int smap_level = -1;
00118 
00119 void normalize(std::vector<double> &hist)
00120 {
00121   double sum = 0;
00122   for(uint i=0; i<hist.size(); i++)
00123   {
00124     sum += hist[i];
00125   }
00126 
00127   for(uint i=0; i<hist.size(); i++)
00128   {
00129     if (sum > 0)
00130       hist[i] *= (1/sum);
00131     else
00132       hist[i] = 0;
00133   }
00134 }
00135 
00136 //distance based on the Bhattacharvva Coefficient
00137 double compBhattHist(const std::vector<double> &a_hist,
00138     const std::vector<double> &b_hist)
00139 {
00140       double bsum = 0;
00141       for(uint i=0; i<a_hist.size(); i++)
00142       {
00143         bsum += sqrt(a_hist[i]*b_hist[i]);
00144       }
00145       return (1-sqrt(1-bsum));
00146 }
00147 
00148 //distance based on equlicdean
00149 double compEquHist(const std::vector<double> &a_hist,
00150     const std::vector<double> &b_hist)
00151 {
00152       double bsum = 0;
00153       for(uint i=0; i<a_hist.size(); i++)
00154       {
00155         bsum += (a_hist[i]-b_hist[i])*(a_hist[i]-b_hist[i]);
00156       }
00157       return (1-sqrt(bsum));
00158 }
00159 
00160 //distance based on prob
00161 double compProbHist(const std::vector<double> &a_hist,
00162     const std::vector<double> &b_hist)
00163 {
00164       double bsum = 0;
00165       for(uint i=0; i<a_hist.size(); i++)
00166       {
00167         double mean = bayesNet->getMean(0, i);
00168         double var = bayesNet->getStdevSq(0, i);
00169         double val = a_hist[i];
00170         if (var != 0)
00171         {
00172           double delta = -(val - mean) * (val - mean);
00173           double newVal = log(exp(delta/(2*var))/sqrt(2*M_PI*var));
00174           //if (newVal < -1000) newVal = -1000;
00175           //src[i] = (intg32)newVal + 1000; //10000-abs(src[i]-featuresValues[std::string(uname)]);
00176           //printf("%f:%f %f:%f = %f\n", mean,var, val, b_hist[i], newVal);
00177           bsum += newVal;
00178         }
00179       }
00180       //printf("--%f\n\n", bsum);
00181       return (exp(bsum)/a_hist.size());
00182 }
00183 
00184 int cMapLearnProc(const char *tagName, struct env_image *cmap)
00185 {
00186   intg32* src = cmap->pixels;
00187   int winX = learnPos.i >> smap_level; //shift the winner to the submap level
00188   int winY = learnPos.j >> smap_level; //shift the winner to the submap level
00189 
00190   int foveaWidth = (FSIZE >> smap_level)/2;
00191   int foveaHeight = (FSIZE >> smap_level)/2;
00192 
00193   //normalize the values from 0 to 255;
00194   env_max_normalize_none_inplace(cmap, 0, 255);
00195 
00196   int featureIdx = -1;
00197   for(int i=0; i<NumFeatureNames; i++)
00198   {
00199           if (!(strcmp(featureNames[i], tagName))){
00200                 featureIdx = i;
00201                 break;
00202         }
00203   }
00204 
00205   //std::vector<double> hist(NBINS,1);
00206   double maxVal = 0; int i= 0;
00207   for(int y=winY-foveaHeight; y<winY+foveaHeight; y++)
00208     for(int x=winX-foveaWidth; x<winX+foveaWidth; x++)
00209     {
00210       byte val = src[y*cmap->dims.w+x];
00211       //if (val > maxVal) maxVal = val;
00212                         maxVal += val;
00213                         i++;
00214       //hist[(int)val/(256/NBINS)]++;
00215     }
00216                 maxVal = maxVal; ///i;
00217 
00218  // normalize(hist);*/
00219 
00220   /*featuresHist[std::string(tagName)] = hist;*/
00221 
00222   featuresVal[featureIdx] = maxVal; //src[winY*cmap->dims.w+winX];
00223 
00224   return 0;
00225 }
00226 
00227 int cMapLeftBiasProc(const char *tagName, struct env_image *cmap)
00228 {
00229   intg32* src = cmap->pixels;
00230   //const env_size_t sz = env_img_size(cmap);
00231   uint winX = learnPos.i >> smap_level; //shift the winner to the submap level
00232   uint winY = learnPos.j >> smap_level; //shift the winner to the submap level
00233 
00234   uint foveaWidth = (FSIZE >> smap_level)/2;
00235   uint foveaHeight = (FSIZE >> smap_level)/2;
00236 
00237   //normalize the values from 0 to 255;
00238 
00239   int featureIdx = -1;
00240   for(int i=0; i<NumFeatureNames; i++)
00241   {
00242           if (!(strcmp(featureNames[i], tagName))){
00243                 featureIdx = i;
00244                 break;
00245         }
00246   }
00247   if (featureIdx != -1)
00248   {
00249                 struct env_image *outImg = new struct env_image;
00250                 env_img_init(outImg, cmap->dims);
00251                 intg32* outSrc = outImg->pixels;
00252 
00253           env_max_normalize_none_inplace(cmap, 0, 255);
00254           double targetMean = bayesNet->getMean(0, featureIdx);
00255           double targetVar = bayesNet->getStdevSq(0, featureIdx);
00256 
00257           featuresVal[featureIdx] = src[winY*cmap->dims.w+winX];
00258 
00259   for(uint winY=0; winY<cmap->dims.h; winY++)
00260     for(uint winX=0; winX<cmap->dims.w; winX++)
00261 
00262     {
00263 
00264       if (winY>=foveaHeight && winY<cmap->dims.h-foveaHeight &&
00265           winX>=foveaWidth && winX<cmap->dims.w-foveaWidth)
00266       {
00267                                         double maxVal = 0; int i= 0;
00268                                         for(uint fy=winY-foveaHeight; fy<winY+foveaHeight; fy++)
00269                                                         for(uint fx=winX-foveaWidth; fx<winX+foveaWidth; fx++)
00270                                                         {
00271                                                                         byte val = src[fy*cmap->dims.w+fx];
00272                                                                         maxVal += val;
00273                                                                         i++;
00274                                                         }
00275                                         maxVal = maxVal; ///i;
00276 
00277                           double val = maxVal; //src[y*cmap->dims.w+x];
00278                           double delta = -(val - targetMean) * (val - targetMean);
00279                           double newVal = (exp(delta/(2*targetVar))/sqrt(2*M_PI*targetVar))*1000;
00280                           //if (newVal < -1000) newVal = -1000;
00281         outSrc[(winY)*cmap->dims.w+(winX)] = (intg32)newVal;
00282                         } else {
00283         outSrc[(winY)*cmap->dims.w+(winX)] = 0;
00284                   }
00285                 }
00286         env_img_swap(cmap, outImg);
00287         env_img_make_empty(outImg);
00288 
00289         }
00290 
00291   return 0;
00292 }
00293 
00294 int cMapRightBiasProc(const char *tagName, struct env_image *cmap)
00295 {
00296   intg32* src = cmap->pixels;
00297 
00298   uint foveaWidth = (FSIZE >> smap_level)/2;
00299   uint foveaHeight = (FSIZE >> smap_level)/2;
00300 
00301 
00302   struct env_image *outImg = new struct env_image;
00303   env_img_init(outImg, cmap->dims);
00304   intg32* outSrc = outImg->pixels;
00305 
00306   //normalize the values from 0 to 255;
00307   env_max_normalize_none_inplace(cmap, 0, 255);
00308 
00309   std::vector<double> fhist = featuresHist[std::string(tagName)];
00310 
00311   for(uint winY=0; winY<cmap->dims.h; winY++)
00312     for(uint winX=0; winX<cmap->dims.w; winX++)
00313 
00314     {
00315 
00316       if (winY>=foveaHeight && winY<cmap->dims.h-foveaHeight &&
00317           winX>=foveaWidth && winX<cmap->dims.w-foveaWidth)
00318       {
00319         std::vector<double> hist(NBINS, 1);
00320         //build histogram
00321         for(uint y=winY-foveaHeight; y<winY+foveaHeight; y++)
00322           for(uint x=winX-foveaWidth; x<winX+foveaWidth; x++)
00323           {
00324             byte val = src[y*cmap->dims.w+x];
00325             hist[(int)val/(256/NBINS)]++;
00326           }
00327         normalize(hist);
00328 
00329         //double compVal = compEquHist(hist, fhist);
00330         double compVal = compBhattHist(hist, fhist);
00331         //double compVal = compProbHist(hist, fhist);
00332 
00333         outSrc[(winY)*cmap->dims.w+(winX)] = (int)(compVal*256000000);
00334 
00335       } else {
00336         //zero out the borders
00337         outSrc[(winY)*cmap->dims.w+(winX)] = 0;
00338       }
00339     }
00340 
00341   env_img_swap(cmap, outImg);
00342   env_img_make_empty(outImg);
00343 
00344   //showHist(winHist, 256);
00345   return 0;
00346 }
00347 
00348 bool debug = 0;
00349 bool training = false;
00350 bool keepTraining = false;
00351 bool tracking = false;
00352 int  trackingTime = 0;
00353 
00354 int main(int argc, const char **argv)
00355 {
00356   // Instantiate a ModelManager:
00357   mgr = new ModelManager("USC Robot Head");
00358 
00359   nub::ref<FrameGrabberConfigurator>
00360     gbc(new FrameGrabberConfigurator(*mgr));
00361   mgr->addSubComponent(gbc);
00362 
00363   nub::ref<EnvVisualCortex> leftEvc(new EnvVisualCortex(*mgr));
00364   mgr->addSubComponent(leftEvc);
00365 
00366   nub::soft_ref<BeoHead> beoHead(new BeoHead(*mgr));
00367   mgr->addSubComponent(beoHead);
00368 
00369   mgr->exportOptions(MC_RECURSE);
00370   mgr->setOptionValString(&OPT_EvcMaxnormType, "None");
00371   //mgr->setOptionValString(&OPT_EvcLevelSpec, "3,4,3,4,3");
00372   //mgr->setOptionValString(&OPT_EvcLevelSpec, "2,4,3,4,3");
00373   //cmin, cmac, smin,smax
00374   mgr->setOptionValString(&OPT_EvcLevelSpec, "2,4,1,1,3");
00375 
00376   leftEvc->setFweight(0);
00377   leftEvc->setMweight(0);
00378   //leftEvc->setCweight(0);
00379  //leftEvc->setOweight(0);
00380 
00381   // Parse command-line:
00382   if (mgr->parseCommandLine(argc, argv, "", 0, 0) == false) return(1);
00383 
00384   nub::soft_ref<FrameIstream> gbLeft(makeV4Lgrabber(*mgr));
00385   gbLeft->setModelParamVal("FrameGrabberDevice",std::string("/dev/video0"));
00386   gbLeft->setModelParamVal("FrameGrabberChannel",1);
00387   mgr->addSubComponent(gbLeft);
00388 
00389  // mgr->removeSubComponent(*gbc);
00390  // gbc.reset(NULL); // fully de-allocate the object and its children
00391 
00392 
00393   // do post-command-line configs:
00394   if (gbLeft.isInvalid())
00395     LFATAL("You need to select a frame grabber type via the "
00396         "--fg-type=XX command-line option for this program "
00397         "to be useful");
00398   imageDims = Dims(gbLeft->getWidth(), gbLeft->getHeight());
00399 
00400   xwin = new XWinManaged(Dims(imageDims.w()*2,imageDims.h()*2+20), -1, -1, "ILab Robot Head Demo");
00401   debugXWin = new XWinManaged(Dims(255+10,255), -1, -1, "Debug Win");
00402   disp = Image<PixRGB<byte> >(imageDims.w(),imageDims.h()+20, ZEROS);
00403 
00404   // let's get all our ModelComponent instances started:
00405   mgr->start();
00406 
00407   smap_level = leftEvc->getMapLevel();
00408 
00409   //init the bayes net
00410  // bayesNet = new Bayes(NFEATURES, 1);
00411   bayesNet = new Bayes(NumFeatureNames, 1);
00412 
00413   //start streaming
00414   gbLeft->startStream();
00415 
00416   Image<double> featureValuesImg(72,1,NO_INIT);
00417 
00418   timer.reset();
00419 
00420   byte leftMaxVal;
00421   Point2D<int> leftMaxPos;
00422   double currentLeftPanPos = 0;
00423   double currentLeftTiltPos = 0;
00424 
00425   int frame = 0;
00426 
00427   beoHead->relaxNeck();
00428 
00429   while(1) {
00430     //grab the images
00431     Image< PixRGB<byte> > leftImg = gbLeft->readRGB();
00432 
00433     //pass the images to the vc
00434 
00435     leftEvc->input(leftImg);
00436 
00437     Image<float> leftVcxmap = leftEvc->getVCXmap();
00438 
00439     //make the right eye dominate
00440     //TODO: need to bias based on the eppolar line
00441     if (lastWinner.isValid())
00442     {
00443       int w = leftVcxmap.getWidth();
00444       int i = 0;
00445       for (Image<float>::iterator itr = leftVcxmap.beginw(), stop = leftVcxmap.endw();
00446           itr != stop; ++itr, i++) {
00447         int x = i % w;
00448         int y = i / w;
00449         float dist = lastWinner.distance(Point2D<int>(x,y));
00450         *itr = *itr - (1*dist);
00451       }
00452     }
00453 
00454 
00455     inplaceNormalize(leftVcxmap, 0.0F, 255.0F);
00456     Image<byte> leftSmap = leftVcxmap;
00457 
00458     findMax(leftSmap, leftMaxPos, leftMaxVal);
00459 
00460     lastWinner = leftMaxPos;
00461 
00462     Point2D<int> clickPos = xwin->getLastMouseClick();
00463 
00464     if (state == BIAS || state == MOVE)
00465       curWinner = Point2D<int>(leftMaxPos.i *(1<<smap_level),
00466           leftMaxPos.j*(1<<smap_level));
00467 
00468     if (state == TRAIN) //bias after training
00469     {
00470     //  state = BIAS;
00471    //   leftEvc->setSubmapPostProc(&cMapLeftBiasProc);
00472     //  lastWinner.i = -1; lastWinner.j = -1;
00473     }
00474 
00475     if (clickPos.isValid())
00476     {
00477       switch(state)
00478       {
00479         case TRAIN:
00480           state = BIAS;
00481           printf("Biasing smap\n");
00482          // leftEvc->setSubmapPreProc(&biasProc);
00483          //rightEvc->setSubmapPreProc(&biasProc);
00484           leftEvc->setSubmapPostProc(&cMapLeftBiasProc);
00485           //leftEvc->setSubmapPostProc(NULL);
00486           //curWinner = clickPos;
00487           lastWinner.i = -1; lastWinner.j = -1;
00488           //evc->setSubmapPreProc(&learnProc);
00489           break;
00490         case BIAS:
00491           printf("Moving\n");
00492           printf("TC: %i %i %f %f\n", leftMaxPos.i, leftMaxPos.j,
00493              currentLeftPanPos, currentLeftTiltPos);
00494           //state = MOVE;
00495           state = NO_BIAS;
00496           break;
00497         case MOVE:
00498           printf("Next pos\n");
00499           state = BIAS;
00500           /*state = NO_BIAS;
00501           curWinner.i = -1;
00502           curWinner.j = -1;
00503           leftEvc->setSubmapPreProc(NULL);
00504           leftEvc->setSubmapPostProc(NULL);*/
00505           break;
00506         case NO_BIAS:
00507           state = TRAIN;
00508           learnPos = clickPos;
00509           //learnPos = curWinner;
00510           curWinner = clickPos;
00511           //leftEvc->setSubmapPreProc(&learnProc);
00512           leftEvc->setSubmapPostProc(&cMapLearnProc);
00513           printf("Learning from %ix%i\n", learnPos.i, learnPos.j);
00514           break;
00515       }
00516     }
00517 
00518     if (state == BIAS && frame > 200)
00519     {
00520         printf("Moving\n");
00521         printf("TC: %i %i %f %f\n", leftMaxPos.i, leftMaxPos.j,
00522                             currentLeftPanPos, currentLeftTiltPos);
00523        // state = MOVE;
00524     }
00525 
00526     /*for(int i=0; i<20; i++)
00527     {
00528       printf("%f:%f ",
00529           bayesNet->getMean(0, i),
00530           bayesNet->getStdevSq(0, i));
00531     }
00532     printf("\n");*/
00533 
00534     //if (featuresHist["intensity"].size() > 0 && state==TRAIN)
00535     if (state==TRAIN)
00536     {
00537       bayesNet->learn(featuresVal, (uint)0);
00538     }
00539 
00540 
00541     //move head
00542     if (state == MOVE && frame > 20)
00543     {
00544      // printf("(%ix%i) p=%f t=%f\n",
00545      //     clickPos.i, clickPos.j,
00546      //     currentLeftPanPos, currentLeftTiltPos);
00547 
00548       double leftPanErr = (leftSmap.getWidth()/2)-leftMaxPos.i;
00549       //double leftPanErr = (leftImg.getWidth()/2)-clickPos.i;
00550 
00551       double leftTiltErr = (leftSmap.getHeight()/2)-leftMaxPos.j;
00552       //double leftTiltErr = (leftImg.getHeight()/2)-clickPos.j;
00553 
00554       if (fabs(leftPanErr) > 1.0 ||
00555           fabs(leftTiltErr) > 1.0)
00556       {
00557           //printf("TC: 0 0 %f %f\n", currentLeftPanPos, currentLeftTiltPos);
00558           //state = BIAS;
00559 
00560 
00561           //currentLeftPanPos += 0.01*leftPanErr;
00562           //currentLeftTiltPos += 0.01*leftTiltErr;
00563 
00564               currentLeftPanPos += (-0.0597*leftMaxPos.i+1.16);
00565               currentLeftTiltPos += (-0.0638*leftMaxPos.j + 0.942);
00566               //printf("%i %f %f\n", leftMaxPos.i, currentLeftPanPos, (-0.0598*leftMaxPos.i+1.16));
00567               //state = BIAS;
00568 
00569 
00570 
00571               beoHead->setLeftEyePan(currentLeftPanPos);
00572               beoHead->setLeftEyeTilt(currentLeftTiltPos);
00573 
00574               beoHead->setRightEyePan(currentLeftPanPos);
00575               beoHead->setRightEyeTilt(currentLeftTiltPos);
00576 
00577       }
00578 
00579       frame = 0;
00580     }
00581 
00582     int key = xwin->getLastKeyPress();
00583 
00584     if (key != -1)
00585     {
00586       switch(key)
00587       {
00588         case 98: //up
00589           currentLeftTiltPos += 0.05;
00590           if (currentLeftTiltPos > 1.0) currentLeftTiltPos = 1.0;
00591           break;
00592         case 104: //down
00593           currentLeftTiltPos -= 0.05;
00594           if (currentLeftTiltPos < -1.0) currentLeftTiltPos = -1.0;
00595           break;
00596         case 100: //left;
00597           currentLeftPanPos -= 0.05;
00598           if (currentLeftPanPos < -1.0) currentLeftPanPos = -1.0;
00599           break;
00600         case 102: //right
00601           currentLeftPanPos += 0.05;
00602           if (currentLeftPanPos > 1.0) currentLeftPanPos = 1.0;
00603           break;
00604       }
00605 
00606       printf("%f %f\n", currentLeftPanPos, currentLeftTiltPos);
00607       beoHead->setLeftEyePan(currentLeftPanPos);
00608       beoHead->setLeftEyeTilt(currentLeftTiltPos);
00609 
00610     }
00611 
00612    // showHist(featuresHist["intensity"], 0);
00613  //   if (state==TRAIN)
00614        showFeatures(featuresVal, 0);
00615  //   else
00616  //      showProb(0);
00617 
00618     display(leftImg, leftSmap, leftMaxPos,
00619         SmaxVal);
00620     frame++;
00621   }
00622 
00623 
00624   // stop all our ModelComponents
00625   mgr->stop();
00626 
00627   // all done!
00628   return 0;
00629 }
00630 
00631 void display(Image<PixRGB<byte> > &leftImg,
00632     const Image<byte> &leftSmap,
00633     const Point2D<int> &leftWinner,
00634     const byte maxVal)
00635 {
00636   static int avgn = 0;
00637   static uint64 avgtime = 0;
00638   static double fps = 0;
00639   char msg[255];
00640 
00641   //Left Image
00642   drawCircle(leftImg,
00643       Point2D<int>(leftWinner.i *(1<<smap_level), leftWinner.j*(1<<smap_level)),
00644       30, PixRGB<byte>(255,0,0));
00645   drawCross(leftImg, Point2D<int>(leftImg.getWidth()/2, leftImg.getHeight()/2),
00646           PixRGB<byte>(0,255,0));
00647   sprintf(msg, "%i", maxVal);
00648   writeText(leftImg,
00649       Point2D<int>(leftWinner.i *(1<<smap_level), leftWinner.j*(1<<smap_level)),
00650         msg, PixRGB<byte>(255), PixRGB<byte>(127));
00651 
00652   xwin->drawImage(leftImg, 0, 0);
00653   Image<PixRGB<byte> > leftSmapDisp = toRGB(quickInterpolate(leftSmap, 1 << smap_level));
00654   xwin->drawImage(leftSmapDisp, 0, leftImg.getHeight());
00655 
00656 
00657   //calculate fps
00658   avgn++;
00659   avgtime += timer.getReset();
00660   if (avgn == 20)
00661   {
00662     fps = 1000.0F / double(avgtime) * double(avgn);
00663     avgtime = 0;
00664     avgn = 0;
00665   }
00666 
00667   sprintf(msg, "%.1ffps ", fps);
00668   switch (state)
00669   {
00670     case TRAIN: sprintf(msg, "%s Train       ", msg); break;
00671     case BIAS: sprintf(msg, "%s Bias         ", msg); break;
00672     case MOVE: sprintf(msg, "%s Move         ", msg); break;
00673     case NO_BIAS: sprintf(msg, "%s No Bias       ", msg); break;
00674   }
00675 
00676 
00677   Image<PixRGB<byte> > infoImg(leftImg.getWidth()*2, 20, NO_INIT);
00678   writeText(infoImg, Point2D<int>(0,0), msg,
00679         PixRGB<byte>(255), PixRGB<byte>(127));
00680   xwin->drawImage(infoImg, 0, leftImg.getHeight()*2);
00681 
00682 }
00683 
00684 void display(const Image<PixRGB<byte> > &img, const Image<PixRGB<byte> > &out,
00685     Point2D<int> &winner, Rectangle &rect)
00686 {
00687   static int avgn = 0;
00688   static uint64 avgtime = 0;
00689   static double fps = 0;
00690   char msg[255];
00691 
00692   inplacePaste(disp, img, Point2D<int>(0,0));
00693   drawCircle(disp, Point2D<int>(winner.i, winner.j), 30, PixRGB<byte>(255,0,0));
00694   drawRect(disp, rect, PixRGB<byte>(255,0,0));
00695   inplacePaste(disp, out, Point2D<int>(img.getWidth(), 0));
00696 
00697   //calculate fps
00698   avgn++;
00699   avgtime += timer.getReset();
00700   if (avgn == 20)
00701   {
00702     fps = 1000.0F / double(avgtime) * double(avgn);
00703     avgtime = 0;
00704     avgn = 0;
00705   }
00706 
00707   sprintf(msg, "%.1ffps ", fps);
00708 
00709   writeText(disp, Point2D<int>(0,img.getHeight()), msg,
00710         PixRGB<byte>(255), PixRGB<byte>(127));
00711 
00712   xwin->drawImage(disp);
00713 
00714 }
00715 
00716 Rectangle getBoundBox()
00717 {
00718   return Rectangle::tlbrI(130,130,190,190);
00719 }
00720 
00721 void showHist(std::vector<double> &hist, int loc)
00722 {
00723   int w = 255, h = 255;
00724 
00725   if (hist.size() == 0) return;
00726 
00727   int dw = w / hist.size();
00728   Image<byte> res(w, h, ZEROS);
00729 
00730   // draw lines for 10% marks:
00731   for (int j = 0; j < 10; j++)
00732     drawLine(res, Point2D<int>(0, int(j * 0.1F * h)),
00733              Point2D<int>(w-1, int(j * 0.1F * h)), byte(64));
00734   drawLine(res, Point2D<int>(0, h-1), Point2D<int>(w-1, h-1), byte(64));
00735 
00736   double minii, maxii;
00737   findMinMax(hist, minii, maxii);
00738 
00739    // uniform histogram
00740   if (maxii == minii) minii = maxii - 1.0F;
00741 
00742   double range = maxii - minii;
00743 
00744   for (uint i = 0; i < hist.size(); i++)
00745     {
00746       int t = abs(h - int((hist[i] - minii) / range * double(h)));
00747 
00748       // if we have at least 1 pixel worth to draw
00749       if (t < h-1)
00750         {
00751           for (int j = 0; j < dw; j++)
00752             drawLine(res,
00753                      Point2D<int>(dw * i + j, t),
00754                      Point2D<int>(dw * i + j, h - 1),
00755                      byte(255));
00756           //drawRect(res, Rectangle::tlbrI(t,dw*i,h-1,dw*i+dw-1), byte(255));
00757         }
00758     }
00759   debugXWin->drawImage(res,loc,0);
00760 }
00761 
00762 void showFeatures(std::vector<double> &fv, int loc)
00763 {
00764   int w = 255, h = 255;
00765 
00766   if (fv.size() == 0) return;
00767 
00768   int dw = w / fv.size();
00769   Image<byte> res(w, h, ZEROS);
00770 
00771   // draw lines for 10% marks:
00772   for (int j = 0; j < 10; j++)
00773     drawLine(res, Point2D<int>(0, int(j * 0.1F * h)),
00774              Point2D<int>(w-1, int(j * 0.1F * h)), byte(64));
00775   drawLine(res, Point2D<int>(0, h-1), Point2D<int>(w-1, h-1), byte(64));
00776 
00777   double minii = 0, maxii = 255;
00778   //findMinMax(fv, minii, maxii);
00779 
00780 
00781    // uniform histogram
00782   if (maxii == minii) minii = maxii - 1.0F;
00783 
00784   double range = maxii - minii;
00785 
00786   for (uint i = 0; i < fv.size(); i++)
00787     {
00788       int t = abs(h - int((fv[i] - minii) / range * double(h)));
00789 
00790       // if we have at least 1 pixel worth to draw
00791       if (t < h-1)
00792         {
00793           for (int j = 0; j < dw; j++)
00794             drawLine(res,
00795                      Point2D<int>(dw * i + j, t),
00796                      Point2D<int>(dw * i + j, h - 1),
00797                      byte(255));
00798           //drawRect(res, Rectangle::tlbrI(t,dw*i,h-1,dw*i+dw-1), byte(255));
00799         }
00800     }
00801   debugXWin->drawImage(res,loc,0);
00802 
00803 
00804 }
00805 
00806 void showProb(int loc)
00807 {
00808   int w = 255, h = 255;
00809   uint size = NumFeatureNames;
00810 
00811   if (size == 0) return;
00812 
00813   int dw = w / size;
00814   Image<PixRGB<byte> > res(w, h, ZEROS);
00815 
00816   // draw lines for 10% marks:
00817   for (int j = 0; j < 10; j++)
00818     drawLine(res, Point2D<int>(0, int(j * 0.1F * h)),
00819              Point2D<int>(w-1, int(j * 0.1F * h)), PixRGB<byte>(64, 64, 64));
00820   drawLine(res, Point2D<int>(0, h-1), Point2D<int>(w-1, h-1), PixRGB<byte>(64, 64, 64));
00821 
00822   double minii = 0, maxii = 255;
00823   //findMinMax(fv, minii, maxii);
00824 
00825 
00826    // uniform histogram
00827   if (maxii == minii) minii = maxii - 1.0F;
00828 
00829   double range = maxii - minii;
00830 
00831   for (uint i = 0; i < size; i++)
00832     {
00833       double mean = bayesNet->getMean(0, i);
00834  //     double var = sqrt(bayesNet->getStdevSq(0, i));
00835 
00836       int t = abs(h - int((mean - minii) / range * double(h)));
00837 
00838       // if we have at least 1 pixel worth to draw
00839       if (t < h-1)
00840         {
00841           for (int j = 0; j < dw; j++)
00842             drawLine(res,
00843                      Point2D<int>(dw * i + j, t),
00844                      Point2D<int>(dw * i + j, h - 1),
00845                      PixRGB<byte>(255, 0, 0));
00846 
00847           //drawRect(res, Rectangle::tlbrI(t,dw*i,h-1,dw*i+dw-1), byte(255));
00848         }
00849     }
00850   debugXWin->drawImage(res,loc,0);
00851 
00852 
00853 }
00854 
00855 void findMinMax(std::vector<double> &vec, double &min, double &max)
00856 {
00857   max = vec[0];
00858   min = max;
00859   for (uint n = 1 ; n < vec.size() ; n++)
00860   {
00861     if (vec[n] > max) max = vec[n];
00862     if (vec[n] < min) min = vec[n];
00863   }
00864 }
00865 
Generated on Sun May 8 08:40:37 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3