RG_Lane.H

/*!@file Robots2/Beobot2/LaneFollowing/RG_Lane/RG_Lane.H Ice Module to Log data    */
// //////////////////////////////////////////////////////////////////// //
// The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the //
// University of Southern California (USC) and the iLab at USC.         //
// See http://iLab.usc.edu for information about this project.          //
// //////////////////////////////////////////////////////////////////// //
// Major portions of the iLab Neuromorphic Vision Toolkit are protected //
// under the U.S. patent ``Computation of Intrinsic Perceptual Saliency //
// in Visual Environments, and Applications'' by Christof Koch and      //
// Laurent Itti, California Institute of Technology, 2001 (patent       //
// pending; application number 09/912,225 filed July 23, 2001; see      //
// http://pair.uspto.gov/cgi-bin/final/home.pl for current status).     //
// //////////////////////////////////////////////////////////////////// //
// This file is part of the iLab Neuromorphic Vision C++ Toolkit.       //
//                                                                      //
// The iLab Neuromorphic Vision C++ Toolkit is free software; you can   //
// redistribute it and/or modify it under the terms of the GNU General  //
// Public License as published by the Free Software Foundation; either  //
// version 2 of the License, or (at your option) any later version.     //
//                                                                      //
// The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope  //
// that it will be useful, but WITHOUT ANY WARRANTY; without even the   //
// implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR      //
// PURPOSE.  See the GNU General Public License for more details.       //
//                                                                      //
// You should have received a copy of the GNU General Public License    //
// along with the iLab Neuromorphic Vision C++ Toolkit; if not, write   //
// to the Free Software Foundation, Inc., 59 Temple Place, Suite 330,   //
// Boston, MA 02111-1307 USA.                                           //
// //////////////////////////////////////////////////////////////////// //
//
// Primary maintainer for this file: Chin-Kai Chang <chinkaic@usc.edu>
// $HeadURL: svn://ilab.usc.edu/trunk/saliency/src/Robots/Beobot2/LaneFollowing/RG_Lane/RG_Lane.H
// $Id: RG_Lane.H 12962 2010-03-06 02:13:53Z irock $
//

#ifndef RG_LANE
#define RG_LANE
#include "Image/OpenCVUtil.H"
#include "Component/ModelComponent.H"
#include "Component/ModelParam.H"

#include "Robots/RobotBrain/RobotBrainComponent.H"
#include "Util/Timer.H"

#include "Ice/RobotBrainObjects.ice.H"
#include "Ice/RobotSimEvents.ice.H"
#include <IceUtil/Thread.h>

//#include "Demo/SaliencyMT.H"
#include "Robots/Beobot2/LaneFollowing/RG_Lane/SaliencyMT.H"
#include "Robots/Beobot2/BeoCommon.H"

#include "Media/FrameSeries.H"

#include "Image/Image.H"
#include "Image/Point3D.H"
#include "Raster/Raster.H"

#include <vector>
#include "Transport/FrameInfo.H"
#include "Gist/SuperPixel.H"
#include "GUI/ImageDisplayStream.H"
#include "GUI/XWinManaged.H"//for keyboard & mouse click


typedef struct _RegionInformation
{
  uint start;
  uint end;

  float minDistance;
  uint minIndex;
}
RegionInformation;

//! for loading simulation images
class imageDB 
{
  std::string path;//Full path such as "../data/corner/HNB/2010_12_34/image00000"
  int start;
  int end;
  int current;  
  int fps;
  int sign;
public:
  imageDB(){}
  
  imageDB(std::string p,int s,int e)
  {
    start = s;
    end = e;
    path = p;
    current = s;
    fps = 10;
    sign = 1;
  }
  void forwardFaster(){
        fps+=5;
  }
  void forwardSlower(){
        fps-=5;
        if(fps < 0){
                fps = 1;
        }
  }
  void forwardNormal(){
        fps = 1;
  }
  void flipDirection(){
        sign *= -1;
  }

  Image<PixRGB<byte> > nextImg()
  {
   current+=(fps*sign);
   if(current > end)
      current = start;                  
    else if(current < start)
      current = end;
    std::string imgpath = sformat("%s%05d.ppm",path.c_str(),current);
    LDEBUG("Image path is %s",imgpath.c_str());
    return Raster::ReadRGB(imgpath.c_str());
  }
int getCurrentImageID(){
        return current;
}
int getFps(){
        return fps*sign;
}
//Time of frame , 0.0~1.0
float getPercentage(){

        float c = current - start;
        float d = end - start;  
        float rate = c / d;
        LINFO("========= Play %f",rate);
        if(rate < 0.01){
                rate = 0.01;
        }
        return rate;
}
float getCurrentTime(){

        float c = current - start;
        float time = c / 30.0;
        return time;
}
float getTotalTime(){
        float t = end - start;
        float time = t / 30.0;
        return time;


}
  ~imageDB(){}
};



class kalman_filter
{
  float est_var,sensor_var;
  float k;
  float est_x;
public:
  kalman_filter(float ini_ev,float ini_sv)
  {
    est_var = ini_ev;
    sensor_var = ini_sv;
    k = est_var/(est_var + sensor_var);
    est_x = 0;
  }
  float update(float measure)
  {
    if(est_x == 0)
      {
        est_x = measure;
        return est_x;
      }
    est_x = est_x + 10*k*(measure - est_x);//update estimate x
    est_var = est_var*sensor_var/(est_var+sensor_var);//update p
    k = est_var/(est_var+sensor_var);//update k
    return est_x;
  }

};

class RG_Lane: public RobotBrainComponent
{
public:

  RG_Lane(OptionManager& mgr,
                const std::string& descrName = "RG_Lane",
                const std::string& tagName = "RG_Lane");

  ~RG_Lane();

  virtual void evolve();

  //! Get a message
  virtual void updateMessage (const RobotSimEvents::EventMessagePtr& eMsg, const Ice::Current&);

  virtual void registerTopics();

  void start1();

private:

  void openDB(const std::string& path);
  void loadFrame();
  
  void loadDB(const std::string& path);
  void drawState();
  void fitLine(std::vector<Point2D<int> > points, Point2D<int>&p1,Point2D<int>&p2);
  Image<PixRGB<byte> > getSuperPixel(Image<PixRGB<byte> > img);
  Image<PixRGB<byte> > getCannyEdge(Image<PixRGB<byte> > img, Image<PixRGB<byte> > &rawCannyImg);
  Image<PixRGB<byte> > getSaliency(Image<PixRGB<byte> > img);
  void groundTruthMode(const rutz::shared_ptr<XWinManaged> uiwin);
  
  Point2D<int> itsEstMiddlePoint;
  Image<PixRGB<byte> > itsCurrImg;
  Image<PixRGB<byte> > itsProcImg;
  Image<PixRGB<byte> > itsRawSuperPixelImg;
  Image<PixRGB<byte> > itsGroundTruthImg;
  PixRGB<byte>  itsRoadColor;
  float itsRoadColorDiff;
  PixRGB<byte> itsRoadColorDiffSub;//Color diff for each color channel
  int itsMiddlePoint[5];
  bool itsUseFloatWindow;
  std::vector<Image<float> >itsMapChannels;//for superpixel multi channel test
  Image<float> itsSalMap;//for superpixel  channel test
  Image<float> itsIntMap;//for superpixel  channel test
  Image<float> itsRgMap;//for superpixel  channel test
  Image<float> itsByMap;//for superpixel  channel test
  nub::soft_ref<SaliencyMT> itsSmt;
  nub::soft_ref<OutputFrameSeries> itsOfs;
  imageDB itsImageDB;
  kalman_filter itsKf;
  int itsKfx;
  //! Overall Display Image
  Image<PixRGB<byte> > itsDispImg;

  void updateMotor(double tran, double rot);

  IceUtil::Mutex its_Curr_Img_mutex;        //!< locking log filename
  IceUtil::Mutex its_Curr_Mtr_mutex;        //!< locking log filename
  int itsCurrImgID;
  double itsRcTransSpeed;
  double itsRcRotSpeed;
  int itsRemoteMode;
  int itsPrevProcImgID;
  bool itsWaitScreen;
  bool itsGroundTruthMode;
  std::string itsConfFilename;



// ######################################################################
void writeToConfFile(std::string line)
{
  FILE *rFile = fopen(itsConfFilename.c_str(), "at");
  if (rFile != NULL)
    {
      fputs(line.c_str(), rFile);
      fclose (rFile);
    }
  else LFATAL("can't append to file: %s", itsConfFilename.c_str());

}

  float colorDiff(PixRGB<byte> c1,PixRGB<byte> c2){
        int r = c1.red() - c2.red();
        int g = c1.green() - c2.green();
        int b = c1.blue() - c2.blue();
        return sqrt(r*r+g*g+b*b);
 } 
  PixRGB<byte> colorDiffSub(PixRGB<byte> c1,PixRGB<byte> c2){
        int r = abs(c1.red() - c2.red());
        int g = abs(c1.green() - c2.green());
        int b = abs(c1.blue() - c2.blue());
        return PixRGB<byte>(r,g,b);
 } 
//Avg two color
  PixRGB<byte> colorAvg(PixRGB<byte> c1,PixRGB<byte> c2){

        int ratio1 = 8;
        int ratio2 = 10 - ratio1;

        int r = (c1.red()*ratio1 + c2.red()*ratio2)/10;
        int g = (c1.green()*ratio1 + c2.green()*ratio2)/10;
        int b = (c1.blue()*ratio1 + c2.blue()*ratio2)/10;
        return PixRGB<byte>(r,g,b);
 } 
Point2D<int> vanishPoint(Point2D<int> p1, Point2D<int> p2,Point2D<int> p3,Point2D<int> p4){
 //Find intersection point Algorithm can be find here :
 //http://paulbourke.net/geometry/lineline2d/


   double mua,mub;
   double denom,numera,numerb;
   double x,y;
   double EPS = 0.0001;//Epsilon : a small number to enough to be insignificant
        

   denom  = (p4.j-p3.j) * (p2.i-p1.i) - (p4.i-p3.i) * (p2.j-p1.j);
   numera = (p4.i-p3.i) * (p1.j-p3.j) - (p4.j-p3.j) * (p1.i-p3.i);
   numerb = (p2.i-p1.i) * (p1.j-p3.j) - (p2.j-p1.j) * (p1.i-p3.i);

   /* Are the line coincident? */
   if (abs(numera) < EPS && abs(numerb) < EPS && abs(denom) < EPS) {
      x = (p1.i + p2.i) / 2;
      y = (p1.j + p2.j) / 2;
        return Point2D<int>(x,y);
   }

   /* Are the line parallel */
   if (abs(denom) < EPS) {
      x = 0;
      y = 0;
        return Point2D<int>(x,y);
   }

   /* Is the intersection along the the segments */
   mua = numera / denom;
   mub = numerb / denom;
   if (mua < 0 || mua > 1 || mub < 0 || mub > 1) {
      x = 0;
      y = 0;
    
   }
   x = p1.i + mua * (p2.i - p1.i);
   y = p1.j + mua * (p2.j - p1.j);

        LINFO("Intersection Point is (%f,%f)",x,y);
        return Point2D<int>(x,y);
}
  Timer itsTimer;
        /*
        float myLinearity(CvSeq *seq)
        {
                int i;
                CvPoint2D32f *p;
                float *x = new float[seq->total];
                float *y = new float[seq->total];
                float x_bar=0.0, y_bar=0.0;
                float u11=0.0, u20=0.0, u02=0.0;
                float linearity=0.0;
                for (i=0; i < seq->total; i++){
                        p=(CvPoint2D32f*)cvGetSeqElem(seq,i);
                        x=p->x;
                        y=p->y;
                }
                //x_bar, y_bar
                for (i=0; i < seq->total; i++){
                        x_bar+=x;
                        y_bar+=y;
                }
                x_bar/=seq->total;
                y_bar/=seq->total;
                for (i=0; i < seq->total; i++){
                        u11+=((x-x_bar)*(y-y_bar));
                        u20+=pow(x-x_bar,2.0f);
                        u02+=pow(y-y_bar,2.0f);
                }
                u11/=seq->total;
                u20/=seq->total;
                u02/=seq->total;
                linearity = sqrt(4*pow(u11,2.0f)+pow(u20-u02,2.0f))/(u20+u02);
                return linearity;
        }*/
};
#endif

// ######################################################################
/* So things look consistent in everyone's emacs... */
/* Local Variables: */
/* indent-tabs-mode: nil */
/* End: */