RG_Lane.H

00001 /*!@file Robots2/Beobot2/LaneFollowing/RG_Lane/RG_Lane.H Ice Module to Log data    */
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: Chin-Kai Chang <chinkaic@usc.edu>
00033 // $HeadURL: svn://ilab.usc.edu/trunk/saliency/src/Robots/Beobot2/LaneFollowing/RG_Lane/RG_Lane.H
00034 // $Id: RG_Lane.H 12962 2010-03-06 02:13:53Z irock $
00035 //
00036 
00037 #ifndef RG_LANE
00038 #define RG_LANE
00039 #include "Image/OpenCVUtil.H"
00040 #include "Component/ModelComponent.H"
00041 #include "Component/ModelParam.H"
00042 
00043 #include "Robots/RobotBrain/RobotBrainComponent.H"
00044 #include "Util/Timer.H"
00045 
00046 #include "Ice/RobotBrainObjects.ice.H"
00047 #include "Ice/RobotSimEvents.ice.H"
00048 #include <IceUtil/Thread.h>
00049 
00050 //#include "Demo/SaliencyMT.H"
00051 #include "Robots/Beobot2/LaneFollowing/RG_Lane/SaliencyMT.H"
00052 #include "Robots/Beobot2/BeoCommon.H"
00053 
00054 #include "Media/FrameSeries.H"
00055 
00056 #include "Image/Image.H"
00057 #include "Image/Point3D.H"
00058 #include "Raster/Raster.H"
00059 
00060 #include <vector>
00061 #include "Transport/FrameInfo.H"
00062 #include "Gist/SuperPixel.H"
00063 #include "GUI/ImageDisplayStream.H"
00064 #include "GUI/XWinManaged.H"//for keyboard & mouse click
00065 
00066 
00067 typedef struct _RegionInformation
00068 {
00069   uint start;
00070   uint end;
00071 
00072   float minDistance;
00073   uint minIndex;
00074 }
00075 RegionInformation;
00076 
00077 //! for loading simulation images
00078 class imageDB 
00079 {
00080   std::string path;//Full path such as "../data/corner/HNB/2010_12_34/image00000"
00081   int start;
00082   int end;
00083   int current;  
00084   int fps;
00085   int sign;
00086 public:
00087   imageDB(){}
00088   
00089   imageDB(std::string p,int s,int e)
00090   {
00091     start = s;
00092     end = e;
00093     path = p;
00094     current = s;
00095     fps = 10;
00096     sign = 1;
00097   }
00098   void forwardFaster(){
00099         fps+=5;
00100   }
00101   void forwardSlower(){
00102         fps-=5;
00103         if(fps < 0){
00104                 fps = 1;
00105         }
00106   }
00107   void forwardNormal(){
00108         fps = 1;
00109   }
00110   void flipDirection(){
00111         sign *= -1;
00112   }
00113 
00114   Image<PixRGB<byte> > nextImg()
00115   {
00116    current+=(fps*sign);
00117    if(current > end)
00118       current = start;                  
00119     else if(current < start)
00120       current = end;
00121     std::string imgpath = sformat("%s%05d.ppm",path.c_str(),current);
00122     LDEBUG("Image path is %s",imgpath.c_str());
00123     return Raster::ReadRGB(imgpath.c_str());
00124   }
00125 int getCurrentImageID(){
00126         return current;
00127 }
00128 int getFps(){
00129         return fps*sign;
00130 }
00131 //Time of frame , 0.0~1.0
00132 float getPercentage(){
00133 
00134         float c = current - start;
00135         float d = end - start;  
00136         float rate = c / d;
00137         LINFO("========= Play %f",rate);
00138         if(rate < 0.01){
00139                 rate = 0.01;
00140         }
00141         return rate;
00142 }
00143 float getCurrentTime(){
00144 
00145         float c = current - start;
00146         float time = c / 30.0;
00147         return time;
00148 }
00149 float getTotalTime(){
00150         float t = end - start;
00151         float time = t / 30.0;
00152         return time;
00153 
00154 
00155 }
00156   ~imageDB(){}
00157 };
00158 
00159 
00160 
00161 class kalman_filter
00162 {
00163   float est_var,sensor_var;
00164   float k;
00165   float est_x;
00166 public:
00167   kalman_filter(float ini_ev,float ini_sv)
00168   {
00169     est_var = ini_ev;
00170     sensor_var = ini_sv;
00171     k = est_var/(est_var + sensor_var);
00172     est_x = 0;
00173   }
00174   float update(float measure)
00175   {
00176     if(est_x == 0)
00177       {
00178         est_x = measure;
00179         return est_x;
00180       }
00181     est_x = est_x + 10*k*(measure - est_x);//update estimate x
00182     est_var = est_var*sensor_var/(est_var+sensor_var);//update p
00183     k = est_var/(est_var+sensor_var);//update k
00184     return est_x;
00185   }
00186 
00187 };
00188 
00189 class RG_Lane: public RobotBrainComponent
00190 {
00191 public:
00192 
00193   RG_Lane(OptionManager& mgr,
00194                 const std::string& descrName = "RG_Lane",
00195                 const std::string& tagName = "RG_Lane");
00196 
00197   ~RG_Lane();
00198 
00199   virtual void evolve();
00200 
00201   //! Get a message
00202   virtual void updateMessage (const RobotSimEvents::EventMessagePtr& eMsg, const Ice::Current&);
00203 
00204   virtual void registerTopics();
00205 
00206   void start1();
00207 
00208 private:
00209 
00210   void openDB(const std::string& path);
00211   void loadFrame();
00212   
00213   void loadDB(const std::string& path);
00214   void drawState();
00215   void fitLine(std::vector<Point2D<int> > points, Point2D<int>&p1,Point2D<int>&p2);
00216   Image<PixRGB<byte> > getSuperPixel(Image<PixRGB<byte> > img);
00217   Image<PixRGB<byte> > getCannyEdge(Image<PixRGB<byte> > img, Image<PixRGB<byte> > &rawCannyImg);
00218   Image<PixRGB<byte> > getSaliency(Image<PixRGB<byte> > img);
00219   void groundTruthMode(const rutz::shared_ptr<XWinManaged> uiwin);
00220   
00221   Point2D<int> itsEstMiddlePoint;
00222   Image<PixRGB<byte> > itsCurrImg;
00223   Image<PixRGB<byte> > itsProcImg;
00224   Image<PixRGB<byte> > itsRawSuperPixelImg;
00225   Image<PixRGB<byte> > itsGroundTruthImg;
00226   PixRGB<byte>  itsRoadColor;
00227   float itsRoadColorDiff;
00228   PixRGB<byte> itsRoadColorDiffSub;//Color diff for each color channel
00229   int itsMiddlePoint[5];
00230   bool itsUseFloatWindow;
00231   std::vector<Image<float> >itsMapChannels;//for superpixel multi channel test
00232   Image<float> itsSalMap;//for superpixel  channel test
00233   Image<float> itsIntMap;//for superpixel  channel test
00234   Image<float> itsRgMap;//for superpixel  channel test
00235   Image<float> itsByMap;//for superpixel  channel test
00236   nub::soft_ref<SaliencyMT> itsSmt;
00237   nub::soft_ref<OutputFrameSeries> itsOfs;
00238   imageDB itsImageDB;
00239   kalman_filter itsKf;
00240   int itsKfx;
00241   //! Overall Display Image
00242   Image<PixRGB<byte> > itsDispImg;
00243 
00244   void updateMotor(double tran, double rot);
00245 
00246   IceUtil::Mutex its_Curr_Img_mutex;        //!< locking log filename
00247   IceUtil::Mutex its_Curr_Mtr_mutex;        //!< locking log filename
00248   int itsCurrImgID;
00249   double itsRcTransSpeed;
00250   double itsRcRotSpeed;
00251   int itsRemoteMode;
00252   int itsPrevProcImgID;
00253   bool itsWaitScreen;
00254   bool itsGroundTruthMode;
00255   std::string itsConfFilename;
00256 
00257 
00258 
00259 // ######################################################################
00260 void writeToConfFile(std::string line)
00261 {
00262   FILE *rFile = fopen(itsConfFilename.c_str(), "at");
00263   if (rFile != NULL)
00264     {
00265       fputs(line.c_str(), rFile);
00266       fclose (rFile);
00267     }
00268   else LFATAL("can't append to file: %s", itsConfFilename.c_str());
00269 
00270 }
00271 
00272   float colorDiff(PixRGB<byte> c1,PixRGB<byte> c2){
00273         int r = c1.red() - c2.red();
00274         int g = c1.green() - c2.green();
00275         int b = c1.blue() - c2.blue();
00276         return sqrt(r*r+g*g+b*b);
00277  } 
00278   PixRGB<byte> colorDiffSub(PixRGB<byte> c1,PixRGB<byte> c2){
00279         int r = abs(c1.red() - c2.red());
00280         int g = abs(c1.green() - c2.green());
00281         int b = abs(c1.blue() - c2.blue());
00282         return PixRGB<byte>(r,g,b);
00283  } 
00284 //Avg two color
00285   PixRGB<byte> colorAvg(PixRGB<byte> c1,PixRGB<byte> c2){
00286 
00287         int ratio1 = 8;
00288         int ratio2 = 10 - ratio1;
00289 
00290         int r = (c1.red()*ratio1 + c2.red()*ratio2)/10;
00291         int g = (c1.green()*ratio1 + c2.green()*ratio2)/10;
00292         int b = (c1.blue()*ratio1 + c2.blue()*ratio2)/10;
00293         return PixRGB<byte>(r,g,b);
00294  } 
00295 Point2D<int> vanishPoint(Point2D<int> p1, Point2D<int> p2,Point2D<int> p3,Point2D<int> p4){
00296  //Find intersection point Algorithm can be find here :
00297  //http://paulbourke.net/geometry/lineline2d/
00298 
00299 
00300    double mua,mub;
00301    double denom,numera,numerb;
00302    double x,y;
00303    double EPS = 0.0001;//Epsilon : a small number to enough to be insignificant
00304         
00305 
00306    denom  = (p4.j-p3.j) * (p2.i-p1.i) - (p4.i-p3.i) * (p2.j-p1.j);
00307    numera = (p4.i-p3.i) * (p1.j-p3.j) - (p4.j-p3.j) * (p1.i-p3.i);
00308    numerb = (p2.i-p1.i) * (p1.j-p3.j) - (p2.j-p1.j) * (p1.i-p3.i);
00309 
00310    /* Are the line coincident? */
00311    if (abs(numera) < EPS && abs(numerb) < EPS && abs(denom) < EPS) {
00312       x = (p1.i + p2.i) / 2;
00313       y = (p1.j + p2.j) / 2;
00314         return Point2D<int>(x,y);
00315    }
00316 
00317    /* Are the line parallel */
00318    if (abs(denom) < EPS) {
00319       x = 0;
00320       y = 0;
00321         return Point2D<int>(x,y);
00322    }
00323 
00324    /* Is the intersection along the the segments */
00325    mua = numera / denom;
00326    mub = numerb / denom;
00327    if (mua < 0 || mua > 1 || mub < 0 || mub > 1) {
00328       x = 0;
00329       y = 0;
00330     
00331    }
00332    x = p1.i + mua * (p2.i - p1.i);
00333    y = p1.j + mua * (p2.j - p1.j);
00334 
00335         LINFO("Intersection Point is (%f,%f)",x,y);
00336         return Point2D<int>(x,y);
00337 }
00338   Timer itsTimer;
00339         /*
00340         float myLinearity(CvSeq *seq)
00341         {
00342                 int i;
00343                 CvPoint2D32f *p;
00344                 float *x = new float[seq->total];
00345                 float *y = new float[seq->total];
00346                 float x_bar=0.0, y_bar=0.0;
00347                 float u11=0.0, u20=0.0, u02=0.0;
00348                 float linearity=0.0;
00349                 for (i=0; i < seq->total; i++){
00350                         p=(CvPoint2D32f*)cvGetSeqElem(seq,i);
00351                         x=p->x;
00352                         y=p->y;
00353                 }
00354                 //x_bar, y_bar
00355                 for (i=0; i < seq->total; i++){
00356                         x_bar+=x;
00357                         y_bar+=y;
00358                 }
00359                 x_bar/=seq->total;
00360                 y_bar/=seq->total;
00361                 for (i=0; i < seq->total; i++){
00362                         u11+=((x-x_bar)*(y-y_bar));
00363                         u20+=pow(x-x_bar,2.0f);
00364                         u02+=pow(y-y_bar,2.0f);
00365                 }
00366                 u11/=seq->total;
00367                 u20/=seq->total;
00368                 u02/=seq->total;
00369                 linearity = sqrt(4*pow(u11,2.0f)+pow(u20-u02,2.0f))/(u20+u02);
00370                 return linearity;
00371         }*/
00372 };
00373 #endif
00374 
00375 // ######################################################################
00376 /* So things look consistent in everyone's emacs... */
00377 /* Local Variables: */
00378 /* indent-tabs-mode: nil */
00379 /* End: */
Generated on Sun May 8 08:41:17 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3