beobot-followColor-master.C

Go to the documentation of this file.
00001 /*!@file Beobot/beobot-followColor-master.C color segment following - master
00002   adapted from VFAT/test-segmentImageMC.C                               */
00003 
00004 // //////////////////////////////////////////////////////////////////// //
00005 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
00006 // University of Southern California (USC) and the iLab at USC.         //
00007 // See http://iLab.usc.edu for information about this project.          //
00008 // //////////////////////////////////////////////////////////////////// //
00009 // Major portions of the iLab Neuromorphic Vision Toolkit are protected //
00010 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
00011 // in Visual Environments, and Applications'' by Christof Koch and      //
00012 // Laurent Itti, California Institute of Technology, 2001 (patent       //
00013 // pending; application number 09/912,225 filed July 23, 2001; see      //
00014 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
00015 // //////////////////////////////////////////////////////////////////// //
00016 // This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
00017 //                                                                      //
00018 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
00019 // redistribute it and/or modify it under the terms of the GNU General  //
00020 // Public License as published by the Free Software Foundation; either  //
00021 // version 2 of the License, or (at your option) any later version.     //
00022 //                                                                      //
00023 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
00024 // that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
00025 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
00026 // PURPOSE.  See the GNU General Public License for more details.       //
00027 //                                                                      //
00028 // You should have received a copy of the GNU General Public License    //
00029 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
00030 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
00031 // Boston, MA 02111-1307 USA.                                           //
00032 // //////////////////////////////////////////////////////////////////// //
00033 //
00034 // Primary maintainer for this file:  Christian Siagian <siagian@usc.edu>
00035 // $HeadURL: svn://isvn.usc.edu/software/invt/trunk/saliency/src/Beobot/beobot-followColor-master.C $
00036 // $Id: beobot-followColor-master.C 7353 2006-10-30 15:27:31Z rjpeters $
00037 //
00038 
00039 #include "Beowulf/Beowulf.H"
00040 #include "Component/ModelManager.H"
00041 #include "Devices/FrameGrabberConfigurator.H"
00042 #include "Devices/DeviceOpts.H"
00043 #include "Raster/Raster.H"
00044 #include "Image/Image.H"
00045 #include "Transport/FrameIstream.H"
00046 #include "GUI/XWinManaged.H"
00047 #include "GUI/XWindow.H"
00048 
00049 #include "Beobot/BeobotControl.H"
00050 #include "Util/Timer.H"
00051 #include "Util/Types.H"
00052 #include "Util/log.H"
00053 #include "VFAT/segmentImageTrackMC.H"
00054 #include <cstdio>
00055 #include <cstdlib>
00056 #include <signal.h>
00057 
00058 #include "Beobot/BeobotConfig.H"
00059 
00060 // number of frames over which framerate info is averaged:
00061 #define NAVG 20
00062 
00063 static bool goforever = true;
00064 
00065 // ######################################################################
00066 void terminate(int s)
00067 { LERROR("*** INTERRUPT ***"); goforever = false; exit(1); }
00068 
00069 // ######################################################################
00070 //! The main routine. Grab frames, process, send commands to slave node.
00071 int main(const int argc, const char **argv)
00072 {
00073   // instantiate a model manager
00074   ModelManager manager("Follow Color Segments - Master");
00075 
00076 //   nub::soft_ref<Beowulf>
00077 //     beo(new Beowulf(manager, "Beowulf Master", "BeowulfMaster", true));
00078 //   manager.addSubComponent(beo);
00079 
00080   BeobotConfig bbc;
00081   nub::soft_ref<BeoChip> b(new BeoChip(manager));
00082   manager.addSubComponent(b);
00083 
00084   nub::soft_ref<FrameGrabberConfigurator>
00085     gbc(new FrameGrabberConfigurator(manager));
00086   manager.addSubComponent(gbc);
00087 
00088 //   manager.exportOptions(MC_RECURSE);
00089 
00090 //   // frame grabber setup
00091 //   // NOTE: don't have to put the option --fg-type=1394
00092 //   manager.setOptionValString(&OPT_FrameGrabberType, "1394");
00093 //   manager.setOptionValString(&OPT_FrameGrabberDims, "160x120");
00094 //   manager.setOptionValString(&OPT_FrameGrabberMode, "YUV444");
00095 //   manager.setOptionValString(&OPT_FrameGrabberNbuf, "20");
00096 
00097   // parse command-line
00098   if( manager.parseCommandLine( argc, argv, "", 0, 0 ) == false ) return(1);
00099 
00100   // let's configure our serial device:
00101   b->setModelParamVal("BeoChipDeviceName", std::string("/dev/ttyS0"));
00102   // NOTE: may want to add a BeoChip listener
00103 
00104   // configure the camera
00105   nub::soft_ref<FrameIstream> gb = gbc->getFrameGrabber();
00106   if (gb.isInvalid())
00107     LFATAL("You need to select a frame grabber type via the "
00108            "--fg-type=XX command-line option for this program "
00109           "to be useful");
00110   int width = gb->getWidth(), height = gb->getHeight();
00111 
00112   // let's get all our ModelComponent instances started:
00113   manager.start();
00114 
00115   // get the frame grabber to start streaming:
00116   gb->startStream();
00117 
00118   // display window
00119   XWindow wini(Dims(width, height), 0, 0, "test-input window");
00120   XWindow wino(Dims(width/4, height/4), 0, 0, "test-output window 2");
00121   XWindow winAux(Dims(500, 450), 0, 0, "Channel levels");
00122   Image< PixRGB<byte> > ima; Image< PixRGB<float> > fima;
00123   Image< PixRGB<byte> > display;
00124   Image<PixH2SV2<float> > H2SVimage;
00125 
00126   TCPmessage rmsg;      // buffer to receive messages from nodes
00127   TCPmessage smsg;      // buffer to send messages to nodes
00128 
00129   // catch signals and redirect them to terminate for clean exit:
00130   signal(SIGHUP, terminate); signal(SIGINT, terminate);
00131   signal(SIGQUIT, terminate); signal(SIGTERM, terminate);
00132   signal(SIGALRM, terminate);
00133 
00134   // reset the beochip:
00135   LINFO("Resetting BeoChip...");
00136   b->resetChip(); sleep(1);
00137 
00138   // calibrate the servos
00139   b->calibrateServo(bbc.steerServoNum, bbc.steerNeutralVal,
00140                     bbc.steerMinVal, bbc.steerMaxVal);
00141   b->calibrateServo(bbc.speedServoNum, bbc.speedNeutralVal,
00142                     bbc.speedMinVal, bbc.speedMaxVal);
00143   b->calibrateServo(bbc.gearServoNum, bbc.gearNeutralVal,
00144                     bbc.gearMinVal, bbc.gearMaxVal);
00145 
00146   // keep the gear at the lowest speed/highest Ntorque
00147   b->setServoRaw(bbc.gearServoNum, bbc.gearMinVal);
00148 
00149   // timer initialization
00150   Timer tim; Timer camPause;       // to pause the move command
00151   camPause.reset();
00152   uint64 t[NAVG]; int frame = 0;
00153 
00154   // ######################################################################
00155   //! extracted color signature
00156 
00157   // H1 - H2 - S - V
00158   std::vector<float> color(4,0.0F);
00159   //BLUE
00160   //  color[0] = 0.350962; color[1] = 0.645527;
00161   //color[2] = 0.313523; color[3] = 0.720654;
00162 
00163   // YELLOW
00164   //color[0] = 0.8; color[1] = 0.44;
00165   //color[2] = 0.65; color[3] = 0.82;
00166 
00167   // RED
00168   color[0] = 0.75; color[1] = 0.87;
00169   color[2] = 0.48; color[3] = 0.70;
00170 
00171   //! +/- tollerance value on mean for track
00172   std::vector<float> std(4,0.0F);
00173   std[0] = 0.339556; std[1] = 0.368726;
00174   std[2] = 0.609608; std[3] = 0.34012;
00175 
00176   //! normalizer over color values (highest value possible)
00177   std::vector<float> norm(4,0.0F);
00178   norm[0] = 1.0F; norm[1] = 1.0F;
00179   norm[2] = 1.0F; norm[3] = 1.0F;
00180 
00181   //! how many standard deviations out to adapt, higher means less bias
00182   std::vector<float> adapt(4,0.0F);
00183   adapt[0] = 3.5F; adapt[1] = 3.5F;
00184   adapt[2] = 3.5F; adapt[3] = 3.5F;
00185 
00186   //! highest value for color adaptation possible (hard boundary)
00187   std::vector<float> upperBound(4,0.0F);
00188   upperBound[0] = color[0] + 0.45F; upperBound[1] = color[1] + 0.45F;
00189   upperBound[2] = color[2] + 0.55F; upperBound[3] = color[3] + 0.55F;
00190 
00191   //! lowest value for color adaptation possible (hard boundary)
00192   std::vector<float> lowerBound(4,0.0F);
00193   lowerBound[0] = color[0] - 0.45F; lowerBound[1] = color[1] - 0.45F;
00194   lowerBound[2] = color[2] - 0.55F; lowerBound[3] = color[3] - 0.55F;
00195 
00196   int wi = width/4;  int hi = height/4;
00197   segmentImageTrackMC<float,unsigned int, 4> segmenter(wi*hi);
00198   segmenter.SITsetTrackColor(&color,&std,&norm,&adapt,&upperBound,&lowerBound);
00199 
00200   // Limit area of consideration to within the image
00201   segmenter.SITsetFrame(&wi,&hi);
00202 
00203   // Set display colors for output of tracking. Strictly asthetic
00204   segmenter.SITsetCircleColor(0,255,0);
00205   segmenter.SITsetBoxColor(255,255,0,0,255,255);
00206   segmenter.SITsetUseSmoothing(true,10);
00207   // ######################################################################
00208 
00209   // get ready for main loop:
00210   //  int32 rframe = 0, raction = 0;
00211   while(goforever)
00212   {
00213     tim.reset();
00214     ima = gb->readRGB();
00215     //uint64 t0 = tim.get();  // to measure display time
00216 
00217     // segment image on each input frame
00218     Image<PixRGB<byte> > Aux; Aux.resize(100,450,true);
00219     H2SVimage = ima;    // NOTE: this line takes 50ms.
00220     display = ima;
00221     segmenter.SITtrackImageAny(H2SVimage,&display,&Aux,true);
00222 
00223     // Retrieve and Draw all our output images
00224     Image<byte> temp = segmenter.SITreturnCandidateImage();
00225     wini.drawImage(display);
00226     wino.drawImage(temp);
00227     winAux.drawImage(Aux);
00228 
00229     // if we find the tracked object
00230     float st,sp,gr;
00231     if(true || !segmenter.SITreturnLOT())
00232       {
00233         int x, y, m; unsigned int minX, maxX, minY, maxY;
00234         segmenter.SITgetBlobPosition(x,y);
00235         segmenter.SITgetBlobWeight(m);
00236         segmenter.SITgetMinMaxBoundry(&minX, &maxX, &minY, &maxY);
00237         LINFO("x = %d y = %d m = %d", x, y, m);
00238         LDEBUG("[%3d %3d %3d %3d] -> %d",
00239                minX, maxX, minY, maxY,
00240                (maxX - minX) * (maxY - minY));
00241 
00242         int nBlobs = segmenter.SITnumberBlobs();
00243         for(int i = 0; i < nBlobs; i++)
00244           {
00245             int bx = segmenter.SITgetBlobPosX(i);
00246             int by = segmenter.SITgetBlobPosY(i);
00247             int bm = segmenter.SITgetBlobMass(i);
00248             LDEBUG("bx = %d by = %d bm = %d", bx, by, bm);
00249           }
00250 
00251         // steer is controlled by blob x-coor
00252         st = 0.0f;
00253         if(x < 120)      st = -0.8f;
00254         else if(x > 200) st =  0.8f;
00255         else             st = float((x - 120.0)/80.0*1.6 - 0.8);
00256 
00257         // speed is controlled by size of blob
00258         if(m <= 200)
00259           { sp = 0.3f; st = 0.0f; }
00260         if(m > 200 && m < 2000)
00261           sp = (m - 200.0) / 1800.0 * 0.4;
00262         else if(m >= 2000 && m < 6000)
00263           sp = (8000.0 - m) / 6000.0f * 0.4;
00264         else if(m >= 6000 && m < 10000)
00265           { sp = (6000.0 - m)/4000.0 * 0.3; st = 0.0; }
00266         else  { sp = 0.3f; st = 0.0f; }
00267 
00268         // stay at the lowest gear: hi torque/low speed
00269         gr = 0.0;
00270       }
00271     else
00272       {
00273         LINFO("lost of track - will look for it");
00274 
00275         st = 0.0;
00276         sp = 0.4;
00277         gr = 0.0;
00278       }
00279     LINFO("st = %f sp = %f gr = %f", st, sp, gr);
00280 
00281     // make sure command is not too taxing on the motors
00282     // can only send suggestion to stop or move faster
00283     // do we need to delay sending commands?
00284 
00285     // send action command to Board B
00286 //     smsg.reset(rframe, raction);
00287 //     smsg.addFloat(st); smsg.addFloat(sp); smsg.addFloat(gr);
00288 //     beo->send(0, smsg);
00289       b->setServo(bbc.steerServoNum, st);
00290       b->setServo(bbc.speedServoNum, sp);
00291 
00292     // compute and show framerate over the last NAVG frames:
00293     t[frame % NAVG] = tim.get();
00294     //t0 = t[frame % NAVG] - t0; if (t0 > 28) LINFO("Display took %llums", t0);
00295 
00296     if (frame % NAVG == 0 && frame > 0)
00297       {
00298         uint64 avg = 0; for (int i = 0; i < NAVG; i ++) avg += t[i];
00299         float avg2 = 1000.0 / (float)avg * NAVG;
00300         printf("Framerate: %.1f fps\n", avg2);
00301       }
00302     frame ++;
00303   }
00304 
00305   manager.stop();
00306   return 0;
00307 }
00308 
00309 // ######################################################################
00310 /* So things look consistent in everyone's emacs... */
00311 /* Local Variables: */
00312 /* indent-tabs-mode: nil */
00313 /* End: */
Generated on Sun May 8 08:40:11 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3