RG_Lane.C

00001 /*!@file Robots2/Beobot2/LaneFollowing/RG_Lane/RG_Lane.C */
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/RG_Lane.C
00034 // $ $Id: RG_Lane.C 13084 2010-03-30 02:42:00Z kai $
00035 //
00036 //////////////////////////////////////////////////////////////////////////
00037 //#include "Image/OpenCVUtil.H"
00038 #include "Robots/Beobot2/LaneFollowing/RG_Lane/RG_Lane.H"
00039 
00040 #include "Raster/Raster.H"
00041 #include "Util/sformat.H"
00042 #include "Image/Image.H"
00043 #include "Image/DrawOps.H"              // for 
00044 #include "Image/ColorOps.H"   // for luminance(),colorize()
00045 #include "Image/ShapeOps.H"   // for rescale()
00046 #include "Image/MathOps.H"    // for stdev()
00047 #include "Image/MatrixOps.H"  // for matrixMult()
00048 #include "Image/CutPaste.H"   // for inplacePaste()
00049 #include "Image/Transforms.H" // for segmentObject()
00050 #include "Image/Normalize.H"    // for normalizeFloat()
00051 #include "Util/Timer.H"
00052 #include "Ice/BeobotEvents.ice.H"
00053 
00054 
00055 #include "SIFT/Keypoint.H"
00056 #include "SIFT/VisualObject.H"
00057 #include "SIFT/VisualObjectMatch.H"
00058 #include "Transport/FrameInfo.H"
00059 
00060 #include "Ice/IceImageUtils.H"
00061 
00062 #include "Component/ModelParam.H"
00063 #include "Component/ModelOptionDef.H"
00064 
00065 #include "GUI/DebugWin.H" // for SHOWIMG()
00066 
00067 #include <map>
00068 #define FOLDER  "../data/logs"
00069 //#define DBNAME        "Lane"
00070 //#define DBNAME        "outdoor"
00071 //#define DBNAME        "indoor"
00072 #define DBNAME  "Equad"
00073 #define IMAGE_PREFIX    "image_0000000000"
00074 
00075 
00076 //float window does not doing better
00077 #define USE_FLOAT_WINDOW 0
00078 // ######################################################################
00079 RG_Lane::RG_Lane(OptionManager& mgr,
00080                 const std::string& descrName, const std::string& tagName) :
00081         RobotBrainComponent(mgr, descrName, tagName),
00082         itsSmt(new SaliencyMT(mgr,descrName,tagName)),
00083         itsOfs(new OutputFrameSeries(mgr)),
00084         itsKf(80.0,30.0),
00085         itsKfx(0),
00086         itsTimer(1000000)
00087 {
00088         setModelParamVal("InputFameDims",Dims(320,240),MC_RECURSE|MC_IGNORE_MISSING);
00089         addSubComponent(itsSmt);
00090         itsRoadColor = PixRGB<byte>(0,0,0); 
00091         itsMiddlePoint[0] = 0;
00092         itsMiddlePoint[1] = 0;
00093         itsMiddlePoint[2] = 0;
00094         itsMiddlePoint[3] = 0;
00095         itsMiddlePoint[4] = 0;
00096         itsRoadColorDiff = 0.0;
00097         itsEstMiddlePoint = Point2D<int>(0,0);
00098         itsRoadColorDiffSub = PixRGB<byte>(0,0,0);
00099         itsOfs->addFrameDest("display");//Add default --out=display
00100         addSubComponent(itsOfs);
00101         itsWaitScreen = false;
00102         itsGroundTruthMode = false;
00103         itsUseFloatWindow = false;
00104 
00105         // load image db
00106         LINFO("load db");
00107         itsConfFilename = std::string(sformat("%s/%s/%s.conf",FOLDER,DBNAME,DBNAME));
00108         openDB(itsConfFilename.c_str());  
00109 }
00110 
00111 // ######################################################################
00112 void RG_Lane::openDB(const std::string& path)
00113 {
00114         FILE *dbconf = fopen(path.c_str(),"r");
00115         if(dbconf == NULL){
00116                 LFATAL("Can not open file: %s",path.c_str());
00117         }else{
00118                 char line[512];
00119                 while(fgets(line,sizeof line,dbconf)!= NULL)
00120                 {
00121                         LINFO("Got line[%s]",line);
00122                         //Skip the line start at#
00123                         if(line[0] != '#')
00124                         {
00125                                 char subdb[256];
00126                                 int start,end;
00127                                 int ret = sscanf(line,"%s %d %d",subdb,&start,&end);
00128                                 if(ret == 3){
00129                                         LINFO("Got [%s],start[%d],end[%d]",subdb,start,end);
00130                                         std::string path = sformat("%s/%s/%s/%s",FOLDER,DBNAME,subdb,IMAGE_PREFIX);
00131                                         imageDB tmpDB(path,start,end);
00132                                         itsImageDB = tmpDB;
00133                                 }
00134                         }                       
00135                 }
00136         }
00137 }
00138 
00139 // ######################################################################
00140 RG_Lane::~RG_Lane()
00141 { }
00142 
00143 // ######################################################################
00144 void RG_Lane::start1()
00145 { }
00146 
00147 // ######################################################################
00148 void RG_Lane::registerTopics()
00149 {
00150         // subscribe to all sensor data
00151         this->registerSubscription("CameraMessageTopic");
00152         this->registerSubscription("MotorMessageTopic");
00153         this->registerPublisher("MotorRequestTopic");
00154 }
00155 
00156 // ######################################################################
00157 void RG_Lane::evolve()
00158 {
00159         // if simulation
00160 
00161 
00162         if(!itsWaitScreen){
00163                 loadFrame();//Load fram from file instead of camera
00164                 drawState();
00165                 itsOfs->writeRGB(itsDispImg, "display", FrameInfo("RG_nav",SRC_POS));
00166         }       
00167     //handle clicks
00168     const nub::soft_ref<ImageDisplayStream> ids =
00169       itsOfs->findFrameDestType<ImageDisplayStream>();
00170 
00171     const rutz::shared_ptr<XWinManaged> uiwin =
00172       ids.is_valid()
00173       ? ids->getWindow("display")
00174       : rutz::shared_ptr<XWinManaged>();
00175 
00176 
00177     if (uiwin.is_valid())
00178     {
00179 
00180       int key = uiwin->getLastKeyPress();
00181 
00182         switch(key)
00183         {
00184         case -1:
00185                 break;
00186         case 42: //g label ground
00187                 itsWaitScreen = true;
00188                 itsGroundTruthMode = true;
00189                 groundTruthMode(uiwin);         
00190                 break;
00191         case 41: //f forward faster
00192                 itsImageDB.forwardFaster();
00193                 break;
00194         case 40: //d forward slower     
00195                 itsImageDB.forwardSlower();
00196                 break;
00197         case 39: //s flip play direction 
00198                 itsImageDB.flipDirection();
00199                 break;
00200         case 38: //a forward normal
00201                 itsImageDB.forwardNormal();
00202                 break;
00203         default:                
00204                 LINFO("key %i", key);
00205                 itsWaitScreen =! itsWaitScreen;
00206                 break;
00207 
00208         }
00209 
00210       Point2D<int> pos = uiwin->getLastMouseClick();
00211       if (pos.isValid())
00212       {
00213         PixRGB<byte> pixColor = itsDispImg.getVal(pos);
00214         LINFO("Mouse Click (%d %d) Color (%d,%d,%d)", pos.i,pos.j,pixColor.red(),pixColor.green(),pixColor.blue());
00215       }  
00216 
00217 
00218 
00219 
00220     }
00221 
00222 
00223 
00224 
00225         //itsOfs->writeRGB(itsDispImg, "RG_nav", FrameInfo("RG_nav",SRC_POS));
00226 }
00227 
00228 // ######################################################################
00229 void RG_Lane::loadFrame()
00230 {
00231         // load teach image data
00232         itsTimer.reset();
00233         itsCurrImg =  itsImageDB.nextImg();
00234         if(itsCurrImg.getDims() != Dims(320,240)){
00235                 itsCurrImg = rescale(itsCurrImg,Dims(320,240)) ;
00236         }
00237                 
00238 
00239         LDEBUG("Time for Load %f", itsTimer.get()/1000.0);
00240 }
00241 
00242 void RG_Lane::groundTruthMode(const rutz::shared_ptr<XWinManaged> uiwin){
00243 
00244         int clickCount = 0;
00245         int w = itsCurrImg.getWidth();
00246         int h = itsCurrImg.getHeight();
00247         Point2D<int> l1,l2,r1,r2,r3,vp,mp,bp,lbp,rbp;
00248 
00249 
00250         
00251         Point2D<int> bl1 = Point2D<int>(0,2*h);//Image bottom line left
00252         Point2D<int> bl2 = Point2D<int>(w,2*h);//Image bottom line right
00253         Point2D<int> offset_2x = Point2D<int>(w,0);
00254         Image<PixRGB<byte> > tempImage;
00255 
00256         tempImage.resize(itsDispImg.getDims(), ZEROS);
00257         tempImage =  itsDispImg;
00258         char buffer[200];
00259         bool save = false;
00260         sprintf(buffer,"Ground Truth Mode");
00261         writeText(tempImage,Point2D<int>(0,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(20));
00262         inplacePaste(tempImage, rescale(itsCurrImg,itsCurrImg.getDims()*2), offset_2x);
00263 
00264         
00265         sprintf(buffer,"Please Click Left     Top Edge");
00266         writeText(tempImage,Point2D<int>(w,10),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,255),SimpleFont::FIXED(20));
00267         itsOfs->writeRGB(tempImage, "display", FrameInfo("RG_nav",SRC_POS));
00268 
00269 
00270         while(clickCount < 5){
00271                 if (uiwin.is_valid())
00272                 {
00273 
00274                         Point2D<int> pos = uiwin->getLastMouseClick();
00275                         if (pos.isValid())
00276                         {
00277                                 PixRGB<byte> pixColor = itsDispImg.getVal(pos);
00278                                 LINFO("Mouse Click (%d %d) Color (%d,%d,%d) Click %d", pos.i,pos.j,pixColor.red(),pixColor.green(),pixColor.blue(),clickCount);
00279                                 switch(clickCount){
00280                                         case 0:
00281                                                 l1 = pos;
00282                                                 sprintf(buffer,"Please Click Left  Bottom Edge");
00283                                                 writeText(tempImage,Point2D<int>(w*1,10),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,255),SimpleFont::FIXED(20));
00284                                                 break;
00285                                         case 1:
00286                                                 l2 = pos;
00287                                                 drawLine(tempImage,l1,l2,PixRGB<byte>(0,255,0),2);//screen center -Green line 
00288                                                 sprintf(buffer,"Please Click Right    Top Edge");
00289                                                 writeText(tempImage,Point2D<int>(w*1,10),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,255),SimpleFont::FIXED(20));
00290                                                 break;
00291                                         case 2: 
00292                                                 sprintf(buffer,"Please Click Right Bottom Edge");
00293                                                 writeText(tempImage,Point2D<int>(w*1,10),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,255),SimpleFont::FIXED(20));
00294                                                 r1 = pos;
00295                                                 break;
00296                                         case 3: 
00297                                                 r2 = pos;
00298                                                 drawLine(tempImage,r1,r2,PixRGB<byte>(0,255,0),2);//screen center -Green line 
00299                                                 vp = vanishPoint(l1,l2,r1,r2);
00300                                                 drawCircle(tempImage,vp,5,PixRGB<byte>(20,50,255),10);
00301                                                 
00302                                                 r3 = vanishPoint(l2,Point2D<int>(l2.i+1,l2.j),r1,r2);//Find r3 point with same y of left line
00303                                                 mp = (l2+r3)/2;//bottom of middle point 
00304                                                 //drawCircle(tempImage,mp,10,PixRGB<byte>(20,50,255));
00305                         
00306                                                 //middle point of image bottom edge
00307                                                 bp = vanishPoint(bl1,bl2,vp,mp);
00308                                                 //drawCircle(tempImage,bp,10,PixRGB<byte>(20,50,255));
00309 
00310                                                 lbp = vanishPoint(l1,l2,bl1,bl2);
00311                                                 rbp = vanishPoint(r1,r2,bl1,bl2);
00312 
00313 
00314                                                 drawLine(tempImage,vp,bp,PixRGB<byte>(255,255,0),2);//groudtruth middle line 
00315                                                 drawLine(tempImage,vp,lbp,PixRGB<byte>(0,255,0),2);//screen center -Green line 
00316                                                 drawLine(tempImage,vp,rbp,PixRGB<byte>(0,255,0),2);//screen center -Green line 
00317                                                 
00318 
00319                                                 //Draw Buttons
00320                                                 sprintf(buffer,"                                ");
00321                                                 writeText(tempImage,Point2D<int>(w*1,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(20));
00322                                                 writeText(tempImage,Point2D<int>(w*1,10),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(20));
00323                                                 sprintf(buffer,"Save");
00324                                                 writeText(tempImage,Point2D<int>(w*1.7,10),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,255,0),SimpleFont::FIXED(20));
00325                                                 sprintf(buffer,"Redo");
00326                                                 writeText(tempImage,Point2D<int>(w*2.0,10),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,255,0),SimpleFont::FIXED(20));
00327                                                 sprintf(buffer,"Cancel");
00328                                                 writeText(tempImage,Point2D<int>(w*2.3,10),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,255,0),SimpleFont::FIXED(20));
00329                                                 
00330                                                 break;
00331                                         case 4:
00332                 
00333                                                 //redo
00334                                         if(pos.j < 55){
00335 
00336                                                 //handle button click
00337                                                 if(pos.i >= (w*1.7-10) && pos.i < (w*2-10)){
00338                                                         LINFO("Save");
00339                                                         clickCount = 6;
00340                                                         save = true;
00341                                                 }else if(pos.i >= (w*2-10) && pos.i < (w*2.3-10)){
00342                                                         LINFO("Redo");
00343                                                                 
00344                                                         clickCount = -1;
00345                                                         tempImage =  itsDispImg;
00346                                                         inplacePaste(tempImage, rescale(itsCurrImg,itsCurrImg.getDims()*2), offset_2x);
00347                                                         sprintf(buffer,"Ground Truth Mode");
00348                                                         writeText(tempImage,Point2D<int>(0,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(20));
00349                                                         sprintf(buffer,"Please Click Left     Top Edge");
00350                                                         writeText(tempImage,Point2D<int>(w,10),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,255),SimpleFont::FIXED(20));
00351                                                 }else if(pos.i >= (w*2.3-10) && pos.i < (w*2.6-10)){
00352                                                         clickCount = 6;//exit click
00353                                                         tempImage =  itsDispImg;
00354                                                         LINFO("Cancel");
00355                                                 }else{
00356 
00357                                                         clickCount = 3;//do notthing
00358                                                 }
00359 
00360                                         }else{
00361                                                 clickCount = 3;//do notthing
00362                                         }               
00363 
00364 
00365 
00366 
00367                                                 break;
00368                                         default:
00369                                                 break;
00370                                 }
00371                                 clickCount++;
00372                                 
00373                         }  
00374                 }
00375                 itsOfs->writeRGB(tempImage, "display", FrameInfo("RG_nav",SRC_POS));
00376         }
00377 
00378 
00379         if(save){
00380                 //Convert coords to 1x size
00381                 Point2D<int> nvp,nbp,nlbp,nrbp,offset_1x,np;
00382                 nvp = (vp - offset_2x)/2;
00383                 nbp = (bp - offset_2x)/2;
00384                 nlbp = (lbp - offset_2x)/2;
00385                 nrbp = (rbp - offset_2x)/2;
00386 
00387                 //Conpute navigation point, 8 pixel above bottom
00388                 np = vanishPoint(Point2D<int>(0,h-8),Point2D<int>(w,h-8),nvp,nbp);
00389 
00390                 std::string trainGT = std::string(sformat("GT:ID %d,VP %d,%d,BP %d,%d,LBP %d,%d,RBP %d,%d,npx %d\n",itsImageDB.getCurrentImageID(),nvp.i,nvp.j,nbp.i,nbp.j,nlbp.i,nlbp.j,nrbp.i,nrbp.j,np.i));  
00391                 LINFO(trainGT.c_str());
00392                 writeToConfFile(trainGT);
00393                 LINFO("GT - EST %d",np.i - itsEstMiddlePoint.i);        
00394 
00395                 tempImage = itsDispImg;
00396                 offset_1x = Point2D<int>(2*w,h);
00397                 //Draw line on orginal image
00398 
00399                 drawCircle(itsGroundTruthImg,nvp ,5,PixRGB<byte>(20,50,255),10);
00400                 drawLine(itsGroundTruthImg,nvp ,nbp  ,PixRGB<byte>(255,255,0),2);//groudtruth middle line 
00401                 drawLine(itsGroundTruthImg,nvp ,nlbp ,PixRGB<byte>(0,255,0),2);//screen center -Green line 
00402                 drawLine(itsGroundTruthImg,nvp ,nrbp ,PixRGB<byte>(0,255,0),2);//screen center -Green line 
00403 
00404                 inplacePaste(tempImage, itsGroundTruthImg, Point2D<int>(2*w, h));
00405         }
00406 
00407         //Return back to normal mode with labeled data
00408         itsOfs->writeRGB(tempImage, "display", FrameInfo("RG_nav",SRC_POS));
00409 
00410 }
00411 // ######################################################################
00412 Image<PixRGB<byte> > RG_Lane::getSuperPixel(Image<PixRGB<byte> > img)
00413 {
00414         if(!img.initialized())
00415                 return Image<PixRGB<byte> >(320,240,ZEROS);
00416         // default parameters for the Superpixel segmentation
00417         float sigma = .5; uint k = 500; uint minSize = 20;
00418         int num_ccs;
00419         std::vector<std::vector<Point2D<int> > > groups;
00420         Image<int> groupImage = SuperPixelSegment(img,sigma, k, minSize, num_ccs, &groups);
00421         Image<PixRGB<byte> > sp_img = SuperPixelDebugImage(groups,img);
00422         Image<int> sp_size_img = SuperPixelRegionSizeImage(groups,groupImage);
00423 
00424         int w = sp_img.getWidth();
00425         int h = sp_img.getHeight();
00426 
00427         //Look for road color, let's assume it always show up in the middle bottom
00428         //(h-5,w/2 +- 10)
00429 
00430         std::vector<PixRGB<byte> >      color_map;
00431         std::vector<int>        color_size_map;
00432         std::vector<int>        color_map_count;
00433 
00434         //Pick all road pixel candidates 
00435         for(int i = -10; i<= 10;i++)
00436         {
00437                 //We are search bottom area as most likely road pixel candidates
00438                 for(int k = 1; k <=5;k++){
00439 
00440                         //set our grow window float to the middle of road
00441                         int middlePoint;
00442                         if(itsMiddlePoint[k-1]!=0 && itsUseFloatWindow){
00443                                 middlePoint = itsMiddlePoint[k-1]/4;//1/4 size image
00444                                 LINFO("=========Float Window %d midpoint %d",middlePoint,k-1);
00445                         }else{
00446 
00447                                 middlePoint = w/2;
00448                                 LINFO("=========Fixed Window %d midpoint",middlePoint);
00449                         }
00450                         if(sp_img.coordsOk(middlePoint+i,h-k)){
00451                                 PixRGB<byte> tmp_color = sp_img.getVal(middlePoint + i,h-k);
00452                                 int regionSize = sp_size_img.getVal(middlePoint+i,h-k);
00453 
00454                                 bool notfound = true;
00455 
00456                                 // Search color
00457                                 for(int j = 0; j < (int)color_map.size() && notfound ; j++)
00458                                 {
00459                                         if(color_map[j] == tmp_color)
00460                                         {
00461                                                 notfound = false;
00462                                                 color_map_count[j]++;
00463                                         }
00464                                 }
00465                                 if(notfound)
00466                                 {
00467                                         //This color is not in the map
00468                                         color_map.push_back(tmp_color);         
00469                                         color_map_count.push_back(0);
00470                                         color_size_map.push_back(regionSize);
00471                                 }
00472                         }
00473                 }       
00474         }
00475 
00476         if(color_map.size() != 1)
00477         {//if we found more than one color
00478                 //Some Option Here:
00479                 //1.Choose max count color
00480                 //2.Pick min color difference from previous avg road color pixel
00481 
00482                 //if road color is not available, we pick max pixel color
00483                 if(itsRoadColor == PixRGB<byte>(0,0,0)){        
00484                         int max = color_map_count[0];
00485                         int max_index = 0;
00486                         for(int i = 1; i < (int)color_map_count.size() ; i++)
00487                         {
00488                                 if(max < color_map_count[i])
00489                                 {
00490                                         max = color_map_count[i];
00491                                         max_index = i;                  
00492                                 }
00493                         }
00494                         itsRoadColor = color_map[max_index];
00495                         LINFO("Max count color have count %d",max);
00496                 }else{
00497                         //Pick min color difference color
00498                         int min_index = 0;
00499                         float min = colorDiff(itsRoadColor,color_map[0]);
00500                         for(int i = 1; i < (int)color_map_count.size() ; i++)
00501                         {
00502                                 float cd = colorDiff(itsRoadColor,color_map[i]);
00503                                 int rs = color_size_map[i];//region size
00504                                 LINFO("======Road Region Size %d",rs);
00505                                 if(cd < min && rs > 100)
00506                                 {
00507                                         min = cd;
00508                                         min_index = i;                  
00509                                 }
00510                         }
00511                         itsRoadColorDiff = colorDiff(itsRoadColor,color_map[min_index]);
00512 
00513                         //to prevent jump too much
00514                         if(itsRoadColorDiff < 50.0){
00515                                 itsRoadColor = color_map[min_index];
00516                         }else{
00517                                 //keep avg color so it will help to solve kid-napping problem   
00518                                 PixRGB<byte> avgColor = colorAvg(itsRoadColor,color_map[0]);
00519                                 itsRoadColor = avgColor; 
00520                                 LINFO("COLOR DIFF1 %f",itsRoadColorDiff);
00521                         }
00522                 }
00523         }
00524         else
00525         {
00526                 //if only one region
00527                 itsRoadColorDiff = colorDiff(itsRoadColor,color_map[0]);
00528                 itsRoadColorDiffSub = colorDiffSub(itsRoadColor,color_map[0]);
00529 
00530                 if(itsRoadColorDiff < 50.0 && color_map[0].green() > 150){
00531                         itsRoadColor = color_map[0];
00532                         itsUseFloatWindow = false;
00533                 }else{
00534                         PixRGB<byte> avgColor = colorAvg(itsRoadColor,color_map[0]);
00535                         itsRoadColor = avgColor; 
00536                         itsUseFloatWindow = true;
00537                         LINFO("COLOR DIFF2 %f,USE float window",itsRoadColorDiff);
00538                 }
00539                 LINFO("Only one color (%d,%d,%d)",itsRoadColor.red(),itsRoadColor.green(),itsRoadColor.blue());
00540         }
00541 
00542         // use iterator!!!!!!
00543         Image<PixRGB<byte> > output(w,h,ZEROS);  
00544         Image<PixRGB<byte> > hybrid(w,h,ZEROS);  
00545         //      inplacePaste(output, sp_img, Point2D<int>(w, 0));
00546         for(int y = 0 ; y < h ; y ++)
00547         {
00548         int dot = 0;
00549                 for(int x = 0 ; x < w ; x ++)
00550                 {
00551                         PixRGB<byte> c = sp_img.getVal(x,y);
00552                         //LINFO("Pixel color(%d,%d,%d)road color(%d,%d,%d)",c.red(),c.green(),c.blue(),
00553                         //              itsRoadColor.red(),itsRoadColor.green(),itsRoadColor.blue());
00554                         if(c == itsRoadColor)   {
00555                                 //Set road color to red
00556                                 output.setVal(x,y,PixRGB<byte>(255,0,0));
00557                                 //LINFO("Found road pixel============");
00558                                 if(dot % 3 == 0){
00559                                         hybrid.setVal(x,y,PixRGB<byte>(255,0,0));
00560                                 }else{
00561                                         hybrid.setVal(x,y,c);
00562                                 }
00563                                 dot ++;
00564                         }
00565                         else{
00566                                 //set to it's color
00567                                 output.setVal(x,y,c);
00568                                 hybrid.setVal(x,y,c);
00569                                 //                                      output.setVal(x,y,c);
00570                         }
00571                 }
00572         }
00573         if(!output.initialized())
00574                 return img;
00575 
00576         LINFO("Finish Road Finding");
00577         itsRawSuperPixelImg = hybrid;
00578         return output;
00579 }
00580 
00581 // ######################################################################
00582 //Image<PixRGB<byte> > RG_Lane::getCannyEdgeDraw(Image<PixRGB<byte> > img){}
00583 // ######################################################################
00584 
00585 //Doing Canny Edge 
00586 Image<PixRGB<byte> > RG_Lane::getCannyEdge(Image<PixRGB<byte> > img,Image<PixRGB<byte> > &rawCannyImg){
00587 
00588         if(!itsCurrImg.initialized())
00589                 return img;
00590         //Doing Canny Edge 
00591         IplImage *cvImg = cvCreateImage(cvGetSize(img2ipl(itsCurrImg)),8,1);
00592         CvMemStorage *s = cvCreateMemStorage(0);
00593         CvSeq *lines = 0;
00594 
00595         cvCanny(img2ipl(luminance(itsCurrImg)),cvImg,100,150,3);
00596         Image<PixRGB<byte> > edgeImg = toRGB(ipl2gray(cvImg)); 
00597         rawCannyImg = edgeImg;
00598 
00599         lines = cvHoughLines2(cvImg,s,CV_HOUGH_PROBABILISTIC,1,CV_PI/180,80,30,10);
00600 
00601         int lx1 = 0,lx2 = 0,ly1 = 0,ly2 = 0,rx1 = 0,rx2 =0 ,ry1 = 0,ry2 = 0;
00602         for(int i = 0;i<MIN(lines->total,100);i++)
00603         {
00604                 CvPoint* line = (CvPoint*)cvGetSeqElem(lines,i);
00605                 Point2D<int> pt1 = Point2D<int>(line[0].x,line[0].y);
00606                 Point2D<int> pt2 = Point2D<int>(line[1].x,line[1].y);
00607                 int dx = pt2.i - pt1.i;
00608                 int dy = pt2.j - pt1.j;
00609                 int midx = (pt1.i+pt2.i)/2;
00610                 int midy = (pt1.j+pt2.j)/2;
00611                 float slope = 0;
00612                 if(dx*dy != 0)
00613                         slope = dy/dx;  
00614                 PixRGB<byte > line_color;
00615                 if(slope == 0.0)
00616                         line_color = PixRGB<byte>(200,0,0);//Dark Red
00617                 else if(slope < 0.0 && slope > -10.0)   
00618                 {
00619                         line_color = PixRGB<byte>(128,128,200);//Light blue
00620                         LDEBUG("Left edge Road!!!! %f",slope);
00621                         lx1 = pt2.i - (pt2.j*slope);
00622                         ly1 = 0;
00623                         lx2 = pt2.i -(pt2.j-240)*slope;
00624                         ly2 = 240;
00625                 }
00626                 else if(slope <= -10.0 )        
00627                         line_color = PixRGB<byte>(0,128,0);//Green
00628                 else if(slope > 0.0 && slope < 10.0)    
00629                 {
00630                         line_color = PixRGB<byte>(0,255,0);//light green
00631                         rx1 = pt2.i - pt2.j*slope;
00632                         ry1 = 0;
00633                         rx2 = pt2.i -(pt2.j-240)*slope;
00634                         ry2 = 240;
00635                 }
00636                 else {
00637                         line_color = PixRGB<byte>(0,0,128);//blue
00638                 }
00639                 if(slope != 0.0)
00640                 {
00641                         drawLine(edgeImg,pt1,pt2,line_color,2);
00642                         char buffer[128];
00643                         sprintf(buffer,"%1.2f",slope);
00644                         writeText(edgeImg,Point2D<int>(midx,midy),buffer,line_color,PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00645                 }//end slope != 0.0
00646 
00647                 //find center line, using Kalman filter
00648                 //int midx1 = (lx1+rx1)/2;
00649                 //int midx2 = (lx2+rx2)/2;
00650                 //int mid_dx = midx1 -midx2;
00651                 /*
00652                    if(midx1 != 0){
00653                    float fkfx = itsKf.update(midx1*1.0);
00654                 //LINFO("update kalman filter old_kfx[%d],measure[%d],new_kfx[%f]",itsKfx,midx1,fkfx);
00655 
00656                 itsKfx = (int)fkfx;
00657                 }
00658                 //LINFO("Slope %f",slope);
00659                 drawLine(edgeImg,Point2D<int>(itsKfx,0),Point2D<int>(itsKfx,240),PixRGB<byte>(255,255,0),2);//Yellow line
00660                  */
00661         }//end for loop
00662         return edgeImg; 
00663 }
00664 // ######################################################################
00665 Image<PixRGB<byte> > RG_Lane::getSaliency(Image<PixRGB<byte> > img)
00666 {
00667         //uint w = 320, h = 240;
00668         uint w = img.getWidth(), h = img.getHeight();
00669         Image<PixRGB<byte> > saliencyImage;
00670         saliencyImage.resize(w, h, ZEROS);
00671         //Do Saliency
00672         //descard the currently processed saliency map
00673         //build a new unbised saliency map, and find the highest local value within it
00674         while(!itsSmt->outputReady()){ //wait for any processing to finish
00675                 usleep(100);
00676         }
00677         //itsSmt->setBiasSM(false);//let the system know we dont want a biased smap
00678         //itsSmt->setSaliencyMapLevel(0); //set the saliency map level to 0
00679         itsSmt->newInput(itsCurrImg,true);//set the image
00680         while(!itsSmt->outputReady()){ //wait for any processing to finish
00681                 usleep(100);
00682         }
00683         Image<float> salMap = itsSmt->getOutput();
00684         Image<PixRGB<byte> > smap = normalizeFloat(salMap,FLOAT_NORM_0_255);
00685         Image<float> rgMap = itsSmt->getRGOutput();
00686         Image<PixRGB<byte> > rg_map = normalizeFloat(rgMap,FLOAT_NORM_0_255);
00687         Image<float> byMap = itsSmt->getBYOutput();
00688         Image<PixRGB<byte> > by_map = normalizeFloat(byMap,FLOAT_NORM_0_255);
00689         Image<float> colorMap = rgMap+byMap;
00690         Image<PixRGB<byte> > color_map = normalizeFloat(colorMap,FLOAT_NORM_0_255);
00691         Image<float> intensityMap = itsSmt->getIntensityOutput();               
00692         Image<PixRGB<byte> > intensity_map = normalizeFloat(intensityMap,FLOAT_NORM_0_255);
00693 
00694         //save channels for superpixel
00695         itsMapChannels.push_back(salMap);
00696         itsMapChannels.push_back(rgMap);
00697         itsMapChannels.push_back(byMap);
00698         itsMapChannels.push_back(intensityMap);
00699         itsSalMap = salMap;
00700         itsRgMap = rgMap;
00701         itsByMap = byMap;
00702         itsIntMap = intensityMap;
00703 
00704 
00705         //draw circle on saliency poin
00706         float minval;
00707         Point2D<int> minPoint;
00708         findMax(salMap,minPoint,minval);
00709         drawCircle(smap,minPoint,10,PixRGB<byte>(20,50,255));
00710         findMax(rgMap,minPoint,minval);
00711         drawCircle(rg_map,minPoint,10,PixRGB<byte>(20,50,255));
00712         findMax(byMap,minPoint,minval);
00713         drawCircle(by_map,minPoint,10,PixRGB<byte>(20,50,255));
00714         findMax(colorMap,minPoint,minval);
00715         //drawCircle(color_map,minPoint,10,PixRGB<byte>(20,50,255));
00716         findMax(intensityMap,minPoint,minval);
00717         drawCircle(intensity_map,minPoint,10,PixRGB<byte>(20,50,255));
00718 
00719 
00720         //write text on each channel
00721         char buffer[200];
00722 
00723 /*
00724         sprintf(buffer,"RG + BY");
00725         writeText(color_map,Point2D<int>(0,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00726         inplacePaste(saliencyImage, rescale(color_map,itsCurrImg.getDims()), Point2D<int>(w, 0));
00727 */
00728 
00729         //sprintf(buffer,"Color");
00730         //writeText(saliencyImage,Point2D<int>(w,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00731 
00732         /*
00733            inplacePaste(saliencyImage, rescale(rg_map,itsCurrImg.getDims()/2), Point2D<int>(w*1.5, 0));
00734            sprintf(buffer,"Red-Green");
00735            writeText(saliencyImage,Point2D<int>(w*1.5,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00736 
00737            inplacePaste(saliencyImage, rescale(by_map,itsCurrImg.getDims()/2), Point2D<int>(w*1.5, h*0.5));
00738            sprintf(buffer,"Blue-Yellow");
00739            writeText(saliencyImage,Point2D<int>(w*1.5,h*0.5),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00740 
00741            inplacePaste(saliencyImage, rescale(intensity_map,itsCurrImg.getDims()/2), Point2D<int>(w, h*0.5));
00742            sprintf(buffer,"Intensity");
00743            writeText(saliencyImage,Point2D<int>(w,h*0.5),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00744            LINFO("hi");
00745          */
00746         //inplacePaste(saliencyImage, rescale(smap,itsCurrImg.getDims()), Point2D<int>(0, 0));
00747         saliencyImage = rescale(smap,itsCurrImg.getDims());
00748         sprintf(buffer,"Saliency");
00749         writeText(saliencyImage,Point2D<int>(0,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00750         return saliencyImage;
00751 }
00752 
00753 // ######################################################################
00754 void RG_Lane::drawState()
00755 {
00756         int w = 320, h = 240;
00757         if(itsCurrImg.initialized())
00758         {
00759                 w = itsCurrImg.getWidth();
00760                 h = itsCurrImg.getHeight();
00761                 itsDispImg.resize(w*5, 2*h, ZEROS);
00762 
00763                 char buffer[256];
00764 
00765 
00766                 // Do canny edge 
00767                 Image<PixRGB<byte> > rawCannyImg;
00768                 Image<PixRGB<byte> > cannyEdgeMap = getCannyEdge(itsCurrImg,rawCannyImg);
00769 
00770                 sprintf(buffer,"Canny Edge");
00771                 writeText(cannyEdgeMap,Point2D<int>(0,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00772                 inplacePaste(itsDispImg,cannyEdgeMap, Point2D<int>(2*w, 0));
00773 
00774                 // Segment the regions in the image 
00775                 // using Superpixel algorithm (Felzenszwalb & Huttenlocher 2003) 
00776                 LINFO("current image size w %d h %d",
00777                                 itsCurrImg.getWidth(),itsCurrImg.getHeight());
00778 
00779                 //get Saliency map, it will also compute each cmap which required by superpixel
00780                 inplacePaste(itsDispImg,getSaliency(itsCurrImg), Point2D<int>(w, 0));
00781 
00782 
00783 
00784                 /*
00785                 //RGB superpixel
00786                 float sigma = .5; uint k = 500; uint minSize = 20;
00787                 int num_ccs;
00788                 std::vector<std::vector<Point2D<int> > > groups;
00789                 SuperPixelSegment(rescale(itsCurrImg,itsCurrImg.getDims()),sigma, k, minSize, num_ccs, &groups);
00790                 Image<PixRGB<byte> > superPixelMap= rescale(SuperPixelDebugImage(groups,itsCurrImg),itsCurrImg.getDims());
00791                 sprintf(buffer,"RGB");
00792                 writeText(superPixelMap,Point2D<int>(0,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00793                 inplacePaste(itsDispImg,superPixelMap, Point2D<int>(3*w, 0));
00794                  */
00795                 //Saliency map for superPixel
00796                 /*
00797                    std::vector<Image<float> > salchannels;
00798                    LINFO("itsSalMap size1 w %d h %d",itsSalMap.getWidth(),itsSalMap.getHeight());
00799 
00800                    salchannels.push_back(rescale(itsSalMap,itsCurrImg.getDims()));//saliency map is only 1/4 of image!
00801                    std::vector<std::vector<Point2D<int> > > salgroups;
00802                    int num_ccs2;
00803                    SuperPixelSegment(itsCurrImg,sigma, k, minSize, num_ccs2, &salgroups,salchannels);
00804                    Image<PixRGB<byte> > salsp_img = SuperPixelDebugImage(salgroups,itsCurrImg);
00805                    LINFO("sal superpixel size1 w %d h %d",salsp_img.getWidth(),salsp_img.getHeight());
00806                    sprintf(buffer,"Saliency");
00807                    writeText(salsp_img,Point2D<int>(0,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00808                    inplacePaste(itsDispImg,salsp_img, Point2D<int>(w, 0));
00809 
00810 
00811                    std::vector<Image<float> > intchannels;
00812                    intchannels.push_back(rescale(itsIntMap,itsCurrImg.getDims()));//saliency map is only 1/4 of image!
00813                    std::vector<std::vector<Point2D<int> > > intgroups;
00814                    int num_ccs3;
00815                    SuperPixelSegment(itsCurrImg,sigma, k, minSize, num_ccs3, &intgroups,intchannels);
00816                    Image<PixRGB<byte> > intsp_img = SuperPixelDebugImage(salgroups,itsCurrImg);
00817                    LINFO("int superpixel size1 w %d h %d",intsp_img.getWidth(),intsp_img.getHeight());
00818                    sprintf(buffer,"Intensity");
00819                    writeText(intsp_img,Point2D<int>(0,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00820                    inplacePaste(itsDispImg,intsp_img, Point2D<int>(w*4, 0));
00821                  */
00822 
00823 
00824                 //Compute Superpixel and find road region. Resize to original for display only
00825                 Image<PixRGB<byte> > superPixelMap = rescale(getSuperPixel(rescale(itsCurrImg,itsCurrImg.getDims()/4)),itsCurrImg.getWidth(),itsCurrImg.getHeight());
00826                 
00827                 LINFO("superpixel size1 w %d h %d",superPixelMap.getWidth(),superPixelMap.getHeight());
00828 
00829 
00830                 //Estimate Middle of Road
00831                 if(superPixelMap.initialized())
00832                 {
00833                         itsRawSuperPixelImg = rescale(itsRawSuperPixelImg,itsCurrImg.getDims());
00834                         sprintf(buffer,"Raw SuperPixel");
00835                         writeText(itsRawSuperPixelImg,Point2D<int>(0,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00836                         inplacePaste(itsDispImg, itsRawSuperPixelImg  , Point2D<int>(w, h));
00837 
00838                         //Combine Superpixel & Canny edge
00839                         Image<PixRGB<byte> > superCannyMap;
00840                         std::vector<Point2D<int> > points;
00841                         superCannyMap.resize(w, h, ZEROS);
00842 
00843                         LINFO("current image size2 w %d h %d",itsCurrImg.getWidth(),itsCurrImg.getHeight());
00844                         LINFO("superpixel size w %d h %d",superPixelMap.getWidth(),superPixelMap.getHeight());
00845 
00846                         sprintf(buffer,"Ground truth");
00847                         itsGroundTruthImg = superPixelMap;
00848                         writeText(itsGroundTruthImg,Point2D<int>(0,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00849 
00850                         inplacePaste(itsDispImg, itsGroundTruthImg, Point2D<int>(2*w, h));
00851 
00852 
00853                         for(int y = 0 ; y < superPixelMap.getHeight() ; y ++)
00854                         {
00855                                 int middle = 0, pixCount = 0;
00856                                 for(int x = 0 ; x < superPixelMap.getWidth() ; x ++)
00857                                 {
00858                                         //PixRGB<byte> c = rawCannyImg.getVal(x,y);
00859                                         //LINFO("get val");
00860                                         PixRGB<byte> s = superPixelMap.getVal(x,y);
00861                                         //LINFO("got val");
00862                                         //if(c.red()==0 && c.green()==0 && c.blue()==0){
00863                                         //Find ave of middle pixel
00864                                         if(s.red()==255 && s.green()==0 && s.blue()==0)
00865                                         {
00866                                                 middle+=x;
00867                                                 pixCount++;
00868                                         }
00869                                         superCannyMap.setVal(x,y,s);//copy pixel from superPixelMap
00870                                 }
00871                                 if(pixCount!=0)
00872                                 {
00873                                         int midx = (int)middle/pixCount;
00874                                         superCannyMap.setVal(midx,y,PixRGB<byte>(255,255,0));//draw yellow point in the middle line
00875                                         //Only use bottom 20 pixel for middle line,which is just right front the robot
00876 
00877                                         if(y > h-21 && y < h-5)
00878                                                 points.push_back(Point2D<int>(midx,y));
00879 
00880                                         if(y > h-5){
00881                                                 itsMiddlePoint[h-y] = midx;
00882                                         }
00883                                 }
00884                         }
00885 
00886                         LINFO("Do Middle Line finder");
00887                         if(points.size() > 1)
00888                         {
00889                                 Point2D<int> p1,p2;
00890                                 fitLine(points,p1,p2);
00891                                 drawLine(superCannyMap,Point2D<int>(w/2,0),Point2D<int>(w/2,h),PixRGB<byte>(0,255,0),1);//Green line
00892                                 drawLine(superCannyMap,p1,p2,PixRGB<byte>(255,255,0),3);//Yellow line
00893 
00894                                 itsEstMiddlePoint = Point2D<int>((p1.i+p2.i)/2,h-8);
00895                                 double error = (w - (p1.i+p2.i))/2;
00896                                 drawLine(superCannyMap,Point2D<int>(w/2,h/2),Point2D<int>(w/2+error,h/2),PixRGB<byte>(0,255,0),2);//screen center -Green line 
00897                                 char buffer[20];
00898                                 sprintf(buffer,"%1.2f",error);
00899                                 writeText(superCannyMap,Point2D<int>(w/2,h/2),buffer,PixRGB<byte>(0,255,0),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00900 
00901                                 sprintf(buffer,"DF %1.2f (%d,%d,%d)",itsRoadColorDiff,itsRoadColorDiffSub.red(),itsRoadColorDiffSub.green(),itsRoadColorDiffSub.blue());
00902                                 writeText(superCannyMap,Point2D<int>(0,h-15),buffer,PixRGB<byte>(255,255,0),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00903 
00904                                 sprintf(buffer,"Road Map");
00905                                 writeText(superCannyMap,Point2D<int>(0,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00906                 
00907                                 inplacePaste(itsDispImg,superCannyMap, Point2D<int>(0, h));
00908 
00909 
00910                                 //error < 0 turn right
00911                                 //error > 0 turn left
00912                                 updateMotor(0.5,1*(error/100));
00913                         }else
00914                         {
00915                                 updateMotor(0.0,0.0);
00916                         }
00917                 }//do supercanny
00918 
00919                 //draw a play bar
00920                 Image<PixRGB<byte> > meterImg;
00921                 meterImg.resize(w, 23, ZEROS);
00922                 drawFilledRect(meterImg, Rectangle(Point2D<int>(0,0), Dims(w, 23)), PixRGB<byte>(128,128,128));
00923                 drawFilledRect(meterImg, Rectangle(Point2D<int>(0,0), Dims(w*itsImageDB.getPercentage(), 23)), PixRGB<byte>(0,255,128));
00924                 inplacePaste(itsCurrImg, meterImg, Point2D<int>(0, 0));
00925 
00926                 int ptg = itsImageDB.getPercentage()*100;
00927                 sprintf(buffer,"ID:%6d Fps:%3d Time:%3.2fs %d%%",itsImageDB.getCurrentImageID(),itsImageDB.getFps(),itsImageDB.getCurrentTime(),ptg);
00928                 writeText(itsCurrImg,Point2D<int>(0,0),buffer,PixRGB<byte>(255,255,255),PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
00929 //
00930                 inplacePaste(itsDispImg, itsCurrImg, Point2D<int>(0, 0));
00931         }//initialized image
00932 }
00933 
00934 void RG_Lane:: fitLine(std::vector<Point2D<int> > points, Point2D<int>&p1,Point2D<int>&p2){
00935         float *line = new float[4];
00936         float linearity = 0.0f;
00937         CvMemStorage* storage = cvCreateMemStorage(0);
00938         //      CvSeq* point_seq = cvCreateSeq( CV_32FC2, sizeof(CvSeq), sizeof(CvPoint2D32f), storage );
00939         CvPoint* cvPoints = (CvPoint*)malloc(points.size()*sizeof(Point2D<int>));
00940         for(uint i = 0; i < points.size(); i++){
00941                 int x = points.at(i).i;
00942                 int y = points.at(i).j;
00943                 //cvSeqPush(point_seq,cvPoint2D32f(x,y));
00944                 cvPoints[i].x = x;
00945                 cvPoints[i].y = y;
00946         }
00947         //      linearity = myLinearity(point_seq);
00948         CvMat point_mat = cvMat(1,points.size(), CV_32SC2, cvPoints);
00949         cvFitLine(&point_mat, CV_DIST_L2,0, 0.01,0.01,line);
00950         LINFO("v=(%f,%f),vy/vx=%f,(x,y)=(%f,%f), Linearity=%f\n",line[0],line[1],line[1]/line[0],line[2],line[3],linearity);
00951         cvReleaseMemStorage(&storage);
00952 
00953         double a, b, c, d, e, f,x0,y0,x1,y1;              // y = a + b*x, b is slop
00954         b = line[1]/ line[0];                  // y = c + d*x
00955         a = line[3]- b*line[2];                // y = e + f*x
00956         d = -1/b;
00957         c = points[0].j - d*points[0].i;
00958         f = d;
00959         e = points[points.size()-1].j - f*points[points.size()-1].i;
00960 
00961         x0 = (a-c)/(d-b);                  // x = (a-c)/(d-b)
00962         y0 = c+d*x0;                   // y = a + b*x
00963         x1 = (a-e)/(f-b);
00964         y1 = e+f*x1;
00965 
00966         p1.i = (int)x0;              
00967         p1.j = (int)y0;           
00968         p2.i = (int)x1;
00969         p2.j = (int)y1;
00970 
00971 }
00972 
00973 // ######################################################################
00974         void RG_Lane:: updateMessage 
00975 (const RobotSimEvents::EventMessagePtr& eMsg, const Ice::Current&)
00976 {
00977         // camera message
00978         if(eMsg->ice_isA("::BeobotEvents::CameraMessage"))
00979         {
00980                 // store the image
00981                 BeobotEvents::CameraMessagePtr cameraMsg =
00982                         BeobotEvents::CameraMessagePtr::dynamicCast(eMsg);
00983 
00984                 int currRequestID = cameraMsg->RequestID;
00985                 Image<PixRGB<byte> > img = Ice2Image<PixRGB<byte> >(cameraMsg->image);
00986 
00987                 LDEBUG("Got a CameraMessage with Request ID = %d", currRequestID);
00988 
00989                 its_Curr_Img_mutex.lock();
00990                 itsCurrImg = img;
00991                 itsCurrImgID = cameraMsg->RequestID;
00992                 its_Curr_Img_mutex.unlock();
00993         }
00994 
00995         // motor message
00996         else if(eMsg->ice_isA("::BeobotEvents::MotorMessage"))
00997         {
00998                 BeobotEvents::MotorMessagePtr mtrMsg =
00999                         BeobotEvents::MotorMessagePtr::dynamicCast(eMsg);
01000                 LDEBUG("Got a MotorMessage with Request ID = %d: RC Trans %f, Rot %f",
01001                                 mtrMsg->RequestID, itsRcTransSpeed, itsRcRotSpeed);
01002                 its_Curr_Mtr_mutex.lock();
01003                 itsRemoteMode = mtrMsg->rcMode;
01004                 itsRcTransSpeed = mtrMsg->rcTransVel;
01005                 itsRcRotSpeed = mtrMsg->rcRotVel;
01006                 its_Curr_Mtr_mutex.unlock();
01007         }
01008         //        LINFO("updateMessage");
01009 
01010 
01011 }
01012 // ######################################################################
01013 void RG_Lane::updateMotor(double tran, double rot)
01014 {
01015         BeobotEvents::MotorRequestPtr msg = new BeobotEvents::MotorRequest;
01016         msg->transVel = tran;
01017         msg->rotVel   = rot;
01018         this->publish("MotorRequestTopic", msg);
01019         LINFO("[%d] Publish motor request Trans %f Rotation %f",
01020                         itsPrevProcImgID,tran,rot);
01021 }
01022 
01023 
01024 // ######################################################################
01025 /* So things look consistent in everyone's emacs... */
01026 /* Local Variables: */
01027 /* indent-tabs-mode: nil */
01028 /* End: */
Generated on Sun May 8 08:41:17 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3