CornerNavigation.C

/*!@file Robots2/Beobot2/Navigation/GistSal_Navigation/CornerNavigation.C */
// //////////////////////////////////////////////////////////////////// //
// 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/CornerNavigation.C
// $ $Id: CornerNavigation.C 13084 2010-03-30 02:42:00Z kai $
//
//////////////////////////////////////////////////////////////////////////

#include "Robots/Beobot2/Navigation/GistSal_Navigation/CornerNavigation.H"
#include "Ice/BeobotEvents.ice.H"

#include "Raster/Raster.H"
#include "Util/sformat.H"
#include "Image/Image.H"
#include "Image/DrawOps.H"
#include "Image/ColorOps.H"   // for luminance()
#include "Image/ShapeOps.H"   // for rescale()
#include "Image/MathOps.H"    // for stdev()
#include "Image/MatrixOps.H"  // for matrixMult()
#include "Image/CutPaste.H"   // for inplacePaste()

#include "Util/Timer.H"

#include "SIFT/Keypoint.H"
#include "SIFT/VisualObject.H"
#include "SIFT/VisualObjectMatch.H"
#include "Transport/FrameInfo.H"

#include "Ice/IceImageUtils.H"

#include "Component/ModelParam.H"
#include "Component/ModelOptionDef.H"

#define  FOLDER                                                 "../data/corner"
#define  DBNAME                                                 "HNB"
#define  IMAGE_PREFIX                           "image_0000000000"
#define  IMAX                      1.0
#define  IMIN                      0.0

#define C_PROCEDURE_INACTIVE       0
#define C_PROCEDURE_ACTIVE         1
#define C_PROCEDURE_CONCLUDING     2

const ModelOptionCateg MOC_CornerNavigation =
  { MOC_SORTPRI_3, "QT Navigation Related Options" };
const ModelOptionDef OPT_RealCamera =
  { MODOPT_ARG(bool), "RealCamera", &MOC_CornerNavigation, OPTEXP_CORE,
    "Do we want use real camera from BeoCamera? Default will load from image files.",
    "camera", '\0', "true | false", "false" };

// ######################################################################
CornerNavigation::CornerNavigation(OptionManager& mgr,
                                   const std::string& descrName,
                                   const std::string& tagName) :
  RobotBrainComponent(mgr, descrName, tagName),
  itsFftComputer(new GistEstimatorFFT(mgr)),
  itsOfs(new OutputFrameSeries(mgr)),
  itsMapImg(320,240,ZEROS),
  itsReplayDBnum(0),
  itsFfn(new FeedForwardNetwork()),
  itsTimer(1000000),
  itsCurrImgID(-1),
  itsPrevProcImgID(-1),
  itsTeachImgID(0),
  itsReplayImgID(0),
  itsTransSpeed(0.0),
  itsRotSpeed(0.0),
  itsDirVector(0.0),
  itsDir(0),
  itsMilestoneErr(999.99),
  itsFalseErrRate(0.0),
  itsDirCount(0),
  itsPosition(0.0, 0.0, 0.0),
  itsError(0.0, 0.0, 0.0),
  itsRealCamera(&OPT_RealCamera, this, 0),
  itsEstop(true),
  itsIState(0.0),
  itsDState(0.0),
  itsPGain(1.0),
  itsIGain(0.01),
  itsDGain(0.0)
{
  addSubComponent(itsOfs);

  itsStatus = C_PROCEDURE_INACTIVE;
  // load the teach Visual Object Database
  openDB(sformat("%s/%s/%s.conf",FOLDER,DBNAME,DBNAME));
}

// ######################################################################
void CornerNavigation::openDB(const std::string& path)
{
        FILE *dbconf = fopen(path.c_str(),"r");
        if(dbconf == NULL){
                LFATAL("Can not open file: %s",path.c_str());
        }else{
                char line[512];
                        while(fgets(line,sizeof line,dbconf)!= NULL)
                        {
                                //Skip the line start at#
                                if(line[0] != '#')
                                {
                                        char subdb[256];
                                        char type;//Teach or Replay
                                        int start,end;
                                        int ret = sscanf(line,"%s %d %d %c",subdb,&start,&end,&type);
                                        if(ret == 4){
                                                LINFO("Got [%s],start[%d],end[%d],type[%c]",subdb,start,end,type);
                                                cornerDB tmpDB;
                                                tmpDB.path = sformat("%s/%s/%s/%s",FOLDER,DBNAME,subdb,IMAGE_PREFIX);
                                                tmpDB.start = start;
                                                tmpDB.end = end;
                                                tmpDB.type = type;
                                                loadDB(tmpDB);

                                        }
                                }
                        
                        }
        
        
        }
}

// ######################################################################
void CornerNavigation::initFFN()
{ }

// ######################################################################
CornerNavigation::~CornerNavigation()
{ }

// ######################################################################
void CornerNavigation::start1()
{
  //if it runing in simulation
  if(!itsRealCamera.getVal())
    itsDBnum = 0;
  else // corner location to turn: initially set none
    itsDBnum = -1;
  LINFO("itsDBNUM: %d", itsDBnum);
}

// ######################################################################
void CornerNavigation::registerTopics()
{
  // subscribe to all sensor data
  //this->registerSubscription("CameraMessageTopic");
  this->registerSubscription("GistSalMessageTopic");
  this->registerSubscription("MotorMessageTopic");
  this->registerSubscription("CornerLocationMessageTopic");

  this->registerPublisher("CornerMotorRequestTopic");
  //this->registerPublisher("MotorRequestTopic");
}

// ######################################################################
void CornerNavigation::evolve()
{
  // check if the current image is updated
  its_Curr_Img_mutex.lock();
  bool newImageFlag = (itsPrevProcImgID < itsCurrImgID);
  its_Curr_Img_mutex.unlock();

  // if simulation
  if(!itsRealCamera.getVal())
    {
      navigation();
    }

  // if real camera
  else
    {
      // if so, process
      if(newImageFlag)
        {
          // get the input image
          itsTimer.reset();
          its_Curr_Img_mutex.lock();
          itsProcImg = itsCurrImg;
          itsPrevProcImgID = itsCurrImgID;
          its_Curr_Img_mutex.unlock();

          // check if we are turning a corner
          its_Corner_Loc_mutex.lock();
          int dbNum = itsDBnum;
          int status =  itsStatus;
          its_Corner_Loc_mutex.unlock();

          // if so navigate
          if(dbNum != -1 && status != C_PROCEDURE_INACTIVE)
            {
              LINFO("size: %"ZU, itsVisualObjectDBs.size() );
              int cornerSize = itsVisualObjectDBs[dbNum].vdb->numObjects();

            if(itsTeachImgID >= cornerSize)
              {
                itsStatus = C_PROCEDURE_INACTIVE;
              }
            else if(itsTeachImgID > int(cornerSize*0.9))
              {
                itsStatus = C_PROCEDURE_CONCLUDING;
              }
            else
              {
                itsStatus = C_PROCEDURE_ACTIVE;
              }
            LINFO("5");
            navigation();
            LINFO("6");
          }
        }
    }

  if(!itsRealCamera.getVal() || newImageFlag)
    {
      drawState();
      itsOfs->writeRGB(itsDispImg, "CornerNav",
                       FrameInfo("CornerNav",SRC_POS));
    }

//   if(itsEstop == false)
//     {
//       Raster::waitForKey();
//       itsEstop = true;
//     }
}

// ######################################################################
void CornerNavigation::recovery()
{
  int trytimes = 0;
  do{
    loadFrame();
    std::vector< rutz::shared_ptr<VisualObjectMatch> > matches;

    its_Corner_Loc_mutex.lock();
    int dbNum = itsDBnum;
    its_Corner_Loc_mutex.unlock();

    int nmatches =
      itsVisualObjectDBs[dbNum].vdb->getObjectMatches
      (itsReplayVo,matches,VOMA_SIMPLE,
       10U,
       0.5F, //keypoint distance score default 0.5F
       0.5F, //affine distance score default 0.5F
       1.0F, //minscore  default 1.0F
       4U, //min # of keypoint match
       100U, //keypoint selection thershold
       false //sort by preattentive
       );
    std::string objName;
    int closestFrame = 999999999;
    int tmpFrameID = itsTeachImgID;
    LINFO("Searching For Nearest Key Frame in the Database");
    if (nmatches > 0 )
      {
        for(int i = 0; i < nmatches; i++)
          {
            LINFO("Found [%d] frames matches Frame[%d]", nmatches, i);

            // so that we will have a ref to the last matches obj
            rutz::shared_ptr<VisualObject> obj;
            rutz::shared_ptr<VisualObjectMatch> vom;
            vom = matches[i];
            obj = vom->getVoTest();
            //      score = vom->getScore();
            //      nkeyp = vom->size();
            //      avgScore = vom->getKeypointAvgDist();
            //      affineAvgDist = vom->getAffineAvgDist();
            objName = obj->getName();
            int tmpID = atoi(objName.c_str());
            int dx = tmpID - itsTeachImgID;

            LINFO("Dist. from current teach frame[%d]=======new ID[%d]",
                  dx,tmpID);
            // find closest frame that matches
            //FIXX: we don't consider the match rate yet.
            if(dx < closestFrame)
              {
                closestFrame = dx;
                tmpFrameID = tmpID;
              }
          }
        LINFO("Recovery Found:TID[%d]RID[%d] Match[%d]",
              itsTeachImgID,itsReplayImgID,tmpFrameID);
        itsTeachImgID = tmpFrameID;
        itsEstop = true;
        itsTransSpeed = 0.8;
        loadFrame();
        computeSIFT();
      }
    else
      {
        LINFO("No Match Point,Try get new frame and try again");
        loadFrame();
        trytimes++;
      }
  }
  while(!itsEstop && trytimes < 10);
}

// ######################################################################
// The main function of navigation
void CornerNavigation::navigation()
{
  // load key frame from database
  // compute SIFT keypoint and matching
  loadFrame();
  //computeSIFT();

  // apply QT algo for direction
  double error = computeDirection2(itsTeachImgID);

        //When the mean error and stderr have too much difference
        //Then ignore the SIFT bogus match
        if(error != 0.0 && itsMatchList->size()!=0 &&
                        (itsMatchList->checkSIFTaffine() || itsFalseErrRate > 1.0)
                ){
                updateMotor(itsTransSpeed, itsRotSpeed,itsStatus);
                updateKeyframe(error);
                itsEstop = true;

      // update keyframe when reaching milestone
      updateKeyframe(error);
      itsEstop = true;
    }

  // when the mean error and stderr have too much difference
  // then ignore the SIFT bogus match
  else
    {
      itsEstop = false;
      LINFO("lost matching; start recover mechanism");
      itsTransSpeed = 0.0;
      itsRotSpeed = 0.0;

                        if(itsRealCamera.getVal())
                        {
                                its_Corner_Loc_mutex.lock();
                                itsStatus = C_PROCEDURE_INACTIVE;
                                int status = itsStatus;
                                itsDBnum  = -1;
                                its_Corner_Loc_mutex.unlock();
                                updateMotor(itsTransSpeed, itsRotSpeed, status);
                        }else{
                                updateMotor(itsTransSpeed, itsRotSpeed, 0);
                                //recovery();
                                //itsEstop = true; 
                        }
    }
}

// ######################################################################
void CornerNavigation::loadDB(cornerDB db)
{

  //std::string path = sformat("%s/%s.sift",db.path.c_str(),IMAGE_PREFIX);

  // load the actual VisualObjectDatabase but don't load images
  db.vdb.reset(new VisualObjectDB());
  bool rt = db.vdb->loadFrom(sformat("%s.sift",db.path.c_str()),false);
  if(rt)
    {
      LINFO("Load SIFT Database");
      itsVisualObjectDBs.push_back(db);

      //Check if db is use for replay
      if(db.type =='R')
        itsReplayDBnum = int(itsVisualObjectDBs.size())-1;
    }
  else
    {
      LINFO("SIFT Database %s not exist, create new", db.path.c_str());
      saveDB(db.start,db.end,db.path.c_str());
    }
}
// ######################################################################
void CornerNavigation::saveDB(int start,int end,const std::string& path)
{
  cornerDB tmp;
  tmp.vdb.reset(new VisualObjectDB());
  for(int i = start; i < end; i++)
    {
      itsTimer.reset();
      std::string teachFileName(sformat("%s%05d.ppm",path.c_str(),i));
      Image< PixRGB<byte> > img = Raster::ReadRGB(teachFileName);
      rutz::shared_ptr<VisualObject> vo1
        (new VisualObject(sformat("%d",i),
                          sformat("%s%05d.png",path.c_str(),i),
                          rescale(img,img.getWidth()/2, img.getHeight()/2)));
      tmp.vdb->addObject(vo1);
      LINFO("Time[%f] Compute SIFT for Frame %d/%d",
            itsTimer.get()/1000.0,i-start+1,end-start+1);
    }
  std::string teachDB(sformat("%s.sift",path.c_str()));
  //        itsVisualObjectDB->setName();
  tmp.vdb->saveTo(teachDB);
}

// ######################################################################
void CornerNavigation::loadFrame()
{
  // load teach image data
  itsTimer.reset();

  its_Corner_Loc_mutex.lock();
  int dbNum = itsDBnum;
  its_Corner_Loc_mutex.unlock();

  //Check the size of subdb
  if(itsTeachImgID < (int)itsVisualObjectDBs[dbNum].vdb->numObjects())
    {
      itsTeachVo = itsVisualObjectDBs[dbNum].vdb->getObject(itsTeachImgID);
      itsTeachImg = itsTeachVo->getImage();
    }
  else
    {
      LINFO("=======Teach DB End=====");
      itsTeachImgID = 0;
      //itsReplayImgID = 0;
    }

  LINFO("Time for Computing SIFT on TeachImg %f",
        itsTimer.get()/1000.0);
  itsTimer.reset();

  // Load Current Replay image data

  // running from image files
  if(!itsRealCamera.getVal())
    {
      if(itsReplayImgID < (int)itsVisualObjectDBs[itsReplayDBnum].vdb->numObjects())
        {
                                        if(itsReplayImgID <(int)itsVisualObjectDBs[itsReplayDBnum].vdb->numObjects())
                                        {
                  itsReplayVo = itsVisualObjectDBs[itsReplayDBnum].vdb-> getObject(itsReplayImgID);     
                                                itsReplayImg = itsReplayVo->getImage(); 
                                                itsProcImg = itsReplayImg;
                                                itsReplayImgID++;
                                        }
        }
    }

  // running from real camera
  else
    {
      itsReplayImg   = itsProcImg;
      itsReplayImgID = itsPrevProcImgID;
      rutz::shared_ptr<VisualObject> vo2 (new VisualObject ("1", "", itsReplayImg));
      itsReplayVo = vo2;
    }
  LINFO("Time for Computing SIFT on ReplayImg %f",
        itsTimer.get()/1000.0);
}

// ######################################################################
bool CornerNavigation::computeSIFT()
{
  itsTimer.reset();
  VisualObjectMatchAlgo voma(VOMA_SIMPLE);
  itsMatchList.reset(new VisualObjectMatch(itsTeachVo, itsReplayVo, voma));
  LINFO("=========================================");
  //LINFO("Found %u matches between Teach and Replay", match.size());

  // let's prune the matches:
  //uint np =
  itsMatchList->prune();
  //LINFO("Pruned %u outlier matches.", np);
  LINFO("Match size now is %u ", itsMatchList->size());

  // show our final affine transform:
  //        std::cerr<<match.getSIFTaffine();

  //LINFO("getKeypointAvgDist = %f", match.getKeypointAvgDist());
  //LINFO("getAffineAvgDist = %f", match.getAffineAvgDist());
  //LINFO("getScore = %f", match.getScore());
  if (itsMatchList->checkSIFTaffine() == false)
    LINFO("### Affine is too weird -- BOGUS MATCH");

  // record the time
  LINFO("Time for Matching SIFT %f",itsTimer.get()/1000.0);

  // if the sift point is valid, return true
  return (itsMatchList->size()!=0 && itsMatchList->checkSIFTaffine());
}

// ######################################################################
double CornerNavigation::computeDirection2(int teachID)
{
  int dirF = 0,dirL = 0,dirR = 0;
  rutz::shared_ptr<VisualObjectMatch> bestMatch;
  int bestMatchSize = -1;
  int bestID = 0;
  LINFO("Direction2 Start");

  // boundary condition
  if((teachID-2) < 0) teachID = 2;

  // looking 10 frame ahead to find best match frame
  for(int id = teachID-2; id < teachID+10; id++)
    {

      its_Corner_Loc_mutex.lock();
      int dbNum = itsDBnum;
      its_Corner_Loc_mutex.unlock();

      //std::string teachFileName(sformat("%s%05d.ppm",TEACH_FOLDER,id));
      rutz::shared_ptr<VisualObject> vo1;
        if(id < (int)itsVisualObjectDBs[dbNum].vdb->numObjects()){
      vo1 = itsVisualObjectDBs[dbNum].vdb->getObject(id);
        }
        else{
                LINFO("DB END");
                        return 0.0;
        }
      VisualObjectMatchAlgo voma(VOMA_SIMPLE);
      rutz::shared_ptr<VisualObjectMatch>
        match(new VisualObjectMatch(vo1, itsReplayVo, voma));
      match->prune();

      // =========================
      // find the best Keyframe
      float theta = 2.0, sx = 2.0, sy = 2.0, str = 0;

      // check if the match is valid
      if (match->checkSIFTaffine())
        {
          match->getSIFTaffine().decompose(theta, sx, sy, str);
        }
      int msize = match->size();
      if((msize > bestMatchSize && sx < 1.0 && sy < 1.0) ||
         (bestMatchSize == -1 && id==teachID))
        {
          bestMatchSize = msize;
          bestMatch = match;
          bestID = id;
        }
      LDEBUG("Best Match Size %d,",bestMatchSize);
      itsMatchList = bestMatch;

      int dirGain = 1; if(id == bestID) dirGain = 2;

      // compute the direction and find mean of error
      for (int i = 0; i < msize; i ++)
        {
          rutz::shared_ptr<Keypoint> refk = match->getKeypointMatch(i).refkp;
          rutz::shared_ptr<Keypoint> tstk = match->getKeypointMatch(i).tstkp;

          double ut = tstk->getX() - itsReplayVo->getImage().getWidth()/2;
          double ud = refk->getX() -  itsTeachVo->getImage().getWidth()/2;

          if(ut > 0.0 && ud < 0.0)
            {
              dirR += dirGain;
            }
          else if(ut < 0.0 && ud > 0.0)
            {
              dirL += dirGain;
            }
          else if(ut > 0.0 && ut > ud)
            {
              dirR += dirGain;
            }
          else if(ut < 0.0 && ut < ud)
            {
              dirL += dirGain;
            }
          else
            {
              dirF +=dirGain;
            }
        }
    }//end for

  //=========================
  double stdx = 0.0,stdy = 0.0;
  double avgdx = 0.0,avgdy = 0.0;

  // compute the std error
  for (int i = 0; i < bestMatchSize; i ++)
    {
      // LINFO("Loading best match point %d",i);
      rutz::shared_ptr<Keypoint> refk = bestMatch->getKeypointMatch(i).refkp;
      rutz::shared_ptr<Keypoint> tstk = bestMatch->getKeypointMatch(i).tstkp;
      double dx = fabs(tstk->getX() - refk->getX())+ 0.000001;
      stdx += dx * dx;
      avgdx+= dx;

      double dy = fabs(tstk->getY() - refk->getY())+ 0.000001;
      stdy += dy * dy;
      avgdy+= dy;
      LDEBUG("Error! i=[%d] BestMatchSize[%d] "
            "avgdx[%f] avgdy[%f],dx[%f], dy[%f]",
            i,bestMatchSize,avgdx,avgdy,dx,dy);
    }
  if(bestMatchSize != 0)
    {
      avgdx/= bestMatchSize;
      avgdy/= bestMatchSize;
      stdx -= (avgdx*avgdx);
      stdy -= (avgdy*avgdy);
      stdx /= bestMatchSize;
      stdx  = sqrt(stdx);
      stdy /= bestMatchSize;
      stdy  = sqrt(stdy);
      LDEBUG("Error! BestMatchSize[%d]  avgdx[%f] avgdy[%f]",
            bestMatchSize,avgdx,avgdy);
    }
  else
    {
      avgdx = 0;
      avgdy = 0;
      stdx  = 0;
      stdy  = 0;
    }

  LDEBUG("stdx %f stdy %f",stdx,stdy);
  uint stdcount = 0;
  double newAvgX = 0.0,newAvgY = 0.0;

  // Pick one stddev error as the new error data
  for (int i = 0; i < bestMatchSize; i ++)
    {
      rutz::shared_ptr<Keypoint> refk = bestMatch->getKeypointMatch(i).refkp;
      rutz::shared_ptr<Keypoint> tstk = bestMatch->getKeypointMatch(i).tstkp;
      double dx = fabs(tstk->getX() - refk->getX())+ 0.000001;
      double dy = fabs(tstk->getY() - refk->getY())+ 0.000001 ;
      double ddx = fabs(dx - avgdx);
      double ddy = fabs(dy - avgdy);

      if(ddx < stdx && ddy < stdy)
        {
          newAvgX += dx;
          newAvgY += dy;
          stdcount++;
        }
    }

  if(stdcount != 0)
    {
      newAvgX/= stdcount;
      newAvgY/= stdcount;
    }
  double newAvgerr = sqrt(newAvgX*newAvgX + newAvgY*newAvgY);
  double avgerr = sqrt(avgdx*avgdx + avgdy*avgdy);
  double angle = (newAvgX*(16.0/33.0));
  itsError.x = newAvgX;
  itsError.y = newAvgY;
  itsError.z = newAvgerr;
  //FIXXX
  if(newAvgerr == 0.0)
    {
      //Raster::waitForKey();
      newAvgerr = avgerr;
      itsFalseErrRate = 0;
    }
  else
    {
      itsFalseErrRate = avgerr/newAvgerr;
    }
  LDEBUG("False Error Rate %1.3f",itsFalseErrRate);
  if(itsFalseErrRate == 0.0)
    {
      LDEBUG("Error!!!! stdcount[%d]  avgerr[%f]",
            stdcount,avgerr);
      LDEBUG("Error!!!! bestMatchSize[%d]  stdx[%f] stdy[%f]",
            bestMatchSize,stdx,stdy);
      //Raster::waitForKey();
    }

  // reset target keyframe if we found better match
  if(bestID > itsTeachImgID && newAvgerr < 10.0)
    itsTeachImgID = bestID;

  // compute the forward speed based on scale
  if (bestMatch->checkSIFTaffine())//Check if the match is valid
    {
      float theta = 0.0 , sx = 0.0, sy = 0.0, str = 0.0 ;
      bestMatch->getSIFTaffine().decompose(theta, sx, sy, str);
      //if the size difference is large(sx/sy << 1.0),we can speed up
      double rms = 1.0 -(sqrt(sx*sx+sy*sy)/sqrt(2));

      double gain = 1.0;
      if(rms > 0.0)
        itsTransSpeed = 0.8 + rms * gain;
      if(itsTransSpeed > 1.0)itsTransSpeed = 1.0;
      if(itsTransSpeed < 0.0)itsTransSpeed = 0.0;
    }
  else
    {
      itsTransSpeed = 0.8;
    }

  // compute turning speed
  if(dirF >= abs(dirR + dirL))
    {
      updatePosition(0);
      itsRotSpeed = 0.4;
      if(itsDir == 0)//same heading since last time
        {
          itsDirCount++;
        }
      else
        {
          itsDirCount = 0;//reset direction count
          itsDir = 0;//reset direction
        }
    }
  else if(dirR > dirL)
    {
      updatePosition(angle);
      //Average the speed
      itsRotSpeed -= newAvgX/100.0;
      itsRotSpeed /= 2.0;
      if(itsDir == 1)//same heading since last time
        {
          itsDirCount++;
        }
      else
        {
          itsDirCount = 0;//reset direction count
          itsDir = 1;//reset direction
        }
    }
  else
    {
      updatePosition(-1*angle);
      //Average the speed
      itsRotSpeed += newAvgX/100.0+1.0;
      itsRotSpeed /= 2.0;
      if(itsDir == -1)//same heading since last time
        {
          itsDirCount++;
        }else{
        itsDirCount = 0;//reset direction count
        itsDir = -1;//reset direction
      }
    }
  return newAvgerr;
}

// ######################################################################
double CornerNavigation::computeDirection()
{
  itsTimer.reset();
  int dirF = 0,dirL = 0,dirR = 0;
  double avgdx = 0.0,avgdy = 0.0;
  uint msize = itsMatchList->size();

  // compute the direction and find mean of error
  for (uint i = 0; i < msize; i ++)
    {
      rutz::shared_ptr<Keypoint> refk =
        itsMatchList->getKeypointMatch(i).refkp;
      rutz::shared_ptr<Keypoint> tstk =
        itsMatchList->getKeypointMatch(i).tstkp;

      double ut = tstk->getX() - itsReplayVo->getImage().getWidth()/2;
      double ud = refk->getX() - itsTeachVo->getImage().getWidth()/2;
      double dx = fabs(ut - ud);
      double dy = fabs(tstk->getY() - refk->getY());
      avgdx += dx;
      avgdy += dy;
      std::string dir;

      if(ut > 0.0 && ud < 0.0)
        {
          dir = std::string("RIGHT");
          dirR ++;
        }
      else if(ut < 0.0 && ud > 0.0)
        {
          dir = std::string("LEFT");
          dirL ++;
        }
      else if(ut > 0.0 && ut > ud  )
        {
          dir = std::string("RIGHT");
          dirR ++;
        }
      else if(ut < 0.0 && ut < ud)
        {
          dir = std::string("LEFT");
          dirL ++;
        }
      else
        {
          dir = std::string("Forward");
          dirF ++;
        }
      LDEBUG("MatchPoint[%d] Teach(%f,%f) Replay(%f,%f), "
             "ud:[%f],ut:[%f],dx:[%f],dy:[%f] Turn %s",
             i,
             (refk->getX()),
             (refk->getY()),
             (tstk->getX()),
             (tstk->getY()),
             ud,ut,dx,dy,dir.c_str()
             );
    }
  avgdx/= msize;
  avgdy/= msize;
  double stdx = 0.0,stdy = 0.0;

  // compute the std error
  for (uint i = 0; i < msize; i ++)
    {
      rutz::shared_ptr<Keypoint> refk =
        itsMatchList->getKeypointMatch(i).refkp;
      rutz::shared_ptr<Keypoint> tstk =
        itsMatchList->getKeypointMatch(i).tstkp;
      double ddx = fabs(tstk->getX() - refk->getX()) - avgdx;
      stdx += ddx * ddx;
      double ddy = fabs(tstk->getY() - refk->getY()) - avgdy;
      stdy += ddy * ddy;
    }
  stdx /= msize;
  //stdx = sqrt(stdx);
  stdy /= msize;
  //stdy = sqrt(stdy);
  uint stdcount = 0;
  double newAvgX = 0.0,newAvgY = 0.0;

  // pick one std error as the new error data
  for (uint i = 0; i < msize; i ++)
    {
      rutz::shared_ptr<Keypoint> refk =
        itsMatchList->getKeypointMatch(i).refkp;
      rutz::shared_ptr<Keypoint> tstk =
        itsMatchList->getKeypointMatch(i).tstkp;
      double dx = fabs(tstk->getX() - refk->getX()) ;
      double dy = fabs(tstk->getY() - refk->getY()) ;
      double ddx = fabs(dx - avgdx);
      double ddy = fabs(dy - avgdy);

      if(ddx < sqrt(stdx) && ddy < sqrt(stdy))
        {
          newAvgX += dx;
          newAvgY += dy;
          stdcount++;
        }
    }
  if(stdcount != 0)
    {
      newAvgX/=stdcount;
      newAvgY/=stdcount;
    }

  double stdeverr = sqrt(stdx+stdy);
  double avgerr = sqrt(avgdx*avgdx + avgdy*avgdy);
  double newAvgerr = sqrt(newAvgX*newAvgX + newAvgY*newAvgY);
  double df = (itsTeachImgID - itsReplayImgID );
  double angle = (newAvgX*(56.0/33.0))/df;
  //double angle = (newAvgX*(16.0/33.0));
  LDEBUG("StdError[%f],StdErrX[%f],StdErrY[%f],Frame[%1.0f]",
        stdeverr,sqrt(stdx),sqrt(stdy),df);
  LDEBUG("AvgErr[%f],AvgX[%f],AvgY[%f]",avgerr,avgdx,avgdy);
  LDEBUG("StdAvgErr[%f],StdAvgX[%f],StdAvgY[%f]",newAvgerr,newAvgX,newAvgY);
  LDEBUG("Forward[%d], Right[%d], Left[%d] Angle[%f]",dirF,dirR,dirL,angle);
  itsError.x = newAvgX;
  itsError.y = newAvgY;
  itsError.z = newAvgerr;
  //FIXXX
  if(newAvgerr == 0.0)
    {
      //Raster::waitForKey();
      newAvgerr = avgerr;
    }

  itsFalseErrRate = avgerr/newAvgerr;

  LINFO("Time for QT Navigation %f",itsTimer.get()/1000.0);
  return newAvgerr;
}

// ######################################################################
void CornerNavigation::updateKeyframe(double error)
{
  LINFO("itsMerror %f,new error %f", itsMilestoneErr, error);

  // check if the match is valid
  if (itsMatchList->checkSIFTaffine())
    {
      float theta, sx, sy, str;
      itsMatchList->getSIFTaffine().decompose(theta, sx, sy, str);
      LDEBUG("SIFT SCALE SX[%f] SY[%f] Theta[%f] Str[%f]",sx,sy,theta,str);

      //Check the frame reach the milestone image or not
      //if(itsMilestoneErr > error && (sx <1.0&&sy <1.0))
      //        itsMilestoneErr = error;
      //else
      //{
      //FIXXX: Hack,Single threshold will not work in all environment
      if(((sx >=0.9 && sy >=0.9) && error <50.0 ) && itsMatchList->size()>2){
        itsMilestoneErr = 999.99;
        itsTeachImgID += 20;//FIXXX when turning sharp,slow down the forward rate
        LINFO("*********=========Switch Keyframe=========**********");
      }
      //}
    }
}

//Given Current image frame,it will try to find proper keyframe for navation
//
// ######################################################################
int CornerNavigation::computeEntropy()
{
  return 1;
}

// ######################################################################
void CornerNavigation::drawState()
{
  its_Corner_Loc_mutex.lock();
  int dbNum = itsDBnum;
  its_Corner_Loc_mutex.unlock();

  uint w = itsProcImg.getWidth()*2, h = itsProcImg.getHeight()*2;
  //uint w = 320, h = 240;
  LINFO("TeachImg w=%d,h=%d, dbNum: %d", w, h, dbNum);
  itsDispImg.resize(w*2, 3*h, NO_INIT);
  char buffer[128];

  // original image
  inplacePaste(itsDispImg, itsProcImg, Point2D<int>(0, 0));

  if(dbNum != -1)
    {
      // replay image
      Image< PixRGB<byte> > replayImg = rescale(itsReplayImg,w,h);
      sprintf(buffer,"img%5d",itsReplayImgID);
      writeText(replayImg, Point2D<int>(5,5),
                buffer, PixRGB<byte>(255,255,255),
                PixRGB<byte>(0,0,0),SimpleFont::FIXED(8));
      inplacePaste(itsDispImg, replayImg, Point2D<int>(0, 2*h));

      // teach image
      Image< PixRGB<byte> > teachImg = rescale(itsTeachImg,w,h);
      sprintf(buffer,"img%5d",itsTeachImgID);
      writeText(teachImg, Point2D<int>(5,5),
                buffer, PixRGB<byte>(255,255,255),
                PixRGB<byte>(0,0,0), SimpleFont::FIXED(8));
      inplacePaste(itsDispImg, teachImg, Point2D<int>(0, h));

      // teach keypoint image
      //inplacePaste(itsDispImg, itsTeachVo->getKeypointImage(1.0) ,
      //             Point2D<int>(w, h));

      // replay keypoint image
      //inplacePaste(itsDispImg, itsReplayVo->getKeypointImage(1.0),
      //             Point2D<int>(w, h*1.5));

      // match image
      if(itsEstop)
        {
          Image< PixRGB<byte> > mimg = itsMatchList->getMatchImage(1.0F);
          LDEBUG("itsdispImg[%d,%d] mimg[%d,%d] pt[%d,%d]",
                 itsDispImg.getWidth(), itsDispImg.getHeight(),
                 mimg.getWidth(), mimg.getHeight(), int(w*1.5), h);

          inplacePaste(itsDispImg, mimg, Point2D<int>(w*1.5, h));
        }

      // visual odometry
      double mapScale = 0.20;
      Point2D<int> drawPos
        (int(itsPosition.x*mapScale + itsMapImg.getWidth()/2),
         int(itsPosition.y*mapScale + itsMapImg.getHeight()/2));
      if(itsMapImg.coordsOk(drawPos))
        itsMapImg.setVal(drawPos, PixRGB<byte>(0,255,0));
      inplacePaste(itsDispImg, itsMapImg, Point2D<int>(w, h*2));
    }

  // information
  inplacePaste(itsDispImg, drawInfoImg(), Point2D<int>(w, 0));
}

// ######################################################################
// Only for display current heading
Image<PixRGB<byte> > CornerNavigation::drawInfoImg()
{
  char buffer[128];
  Image<PixRGB<byte> > dirImg(320,240,ZEROS);

  its_Corner_Loc_mutex.lock();
  int dbNum = itsDBnum;
  int status = itsStatus;
  its_Corner_Loc_mutex.unlock();

  // return if currently not turning
  sprintf(buffer, "DB# :[%d] Status:[%d]", dbNum, status);
  writeText(dirImg, Point2D<int>(10,70),
            buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),
            SimpleFont::FIXED(8));
  if (dbNum == -1) return dirImg;

  // heading direction
  if(itsDir == 0)    { sprintf(buffer, "||| "); }
  else if(itsDir > 0){ sprintf(buffer, "--> "); }
  else               { sprintf(buffer, "<-- "); }
  writeText(dirImg, Point2D<int>(dirImg.getWidth()/2-20,
                                 dirImg.getHeight()/2-20),
            buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),
            SimpleFont::FIXED(20));

  // vote count
  sprintf(buffer, "[%d]",itsDirCount);
  writeText(dirImg, Point2D<int>(dirImg.getWidth()/2-10,
                                 dirImg.getHeight()/2+40),
            buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),
            SimpleFont::FIXED(20));

  // error rate
  sprintf(buffer, "False Error Rate:[%1.3f]",itsFalseErrRate);
  writeText(dirImg, Point2D<int>(10,10),
            buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),
            SimpleFont::FIXED(8));
  if(itsEstop)
    {
      sprintf(buffer, "Match:[%d]",itsMatchList->size());
      writeText(dirImg, Point2D<int>(210,10),
                buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),
                SimpleFont::FIXED(8));
    }
  sprintf(buffer, "Error:[%1.3f] X:[%1.3f] Y:[%1.3f]",
          itsError.z,itsError.x,itsError.y);
  writeText(dirImg, Point2D<int>(10,25),
            buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),
            SimpleFont::FIXED(8));

  // check if the match is valid
  if (itsEstop && itsMatchList->checkSIFTaffine())
    {
      float theta, sx, sy, str;
      itsMatchList->getSIFTaffine().decompose(theta, sx, sy, str);
      sprintf(buffer, "Affine T:[%1.3f] X:[%1.3f] Y:[%1.3f]",theta,sx,sy);
      writeText(dirImg, Point2D<int>(10,40),
                buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),
                SimpleFont::FIXED(8));
    }
  sprintf(buffer, "Motor Speed :[%1.3f] Motor Rot:[%1.3f]",
          itsTransSpeed, itsRotSpeed);
  writeText(dirImg, Point2D<int>(10,55),
            buffer, PixRGB<byte>(255,255,255), PixRGB<byte>(0,0,0),
            SimpleFont::FIXED(8));
  return dirImg;
}

// ######################################################################
void CornerNavigation::updatePosition(double turn)
{
  itsDirVector += turn;
  if(itsDirVector > 360.0) itsDirVector = 0.0;
  if(itsDirVector < 0.0) itsDirVector = 360.0;

  itsPosition.x -= sin(itsDirVector*(M_PI/180.0));
  itsPosition.y -= cos(itsDirVector*(M_PI/180.0));
}

// ######################################################################
void CornerNavigation::updateMessage
(const RobotSimEvents::EventMessagePtr& eMsg, const Ice::Current&)
{
//   // camera message
//   if(eMsg->ice_isA("::BeobotEvents::CameraMessage"))
//     {
//       // store the image
//       BeobotEvents::CameraMessagePtr cameraMsg =
//         BeobotEvents::CameraMessagePtr::dynamicCast(eMsg);

//       int currRequestID = cameraMsg->RequestID;
//       Image<PixRGB<byte> > img = Ice2Image<PixRGB<byte> >(cameraMsg->image);

//       LDEBUG("Got a CameraMessage with Request ID = %d", currRequestID);

//       its_Curr_Img_mutex.lock();
//       itsCurrImg = img;
//       itsCurrImgID = cameraMsg->RequestID;
//       its_Curr_Img_mutex.unlock();
//     }

  // Get a gist-sal message
  if(eMsg->ice_isA("::BeobotEvents::GistSalMessage"))
  {
    BeobotEvents::GistSalMessagePtr gistSalMsg =
      BeobotEvents::GistSalMessagePtr::dynamicCast(eMsg);

    // Get the current request ID
    int currRequestID = gistSalMsg->RequestID;
    Image<PixRGB<byte> > img = Ice2Image<PixRGB<byte> >(gistSalMsg->currIma);

    its_Curr_Img_mutex.lock();
    itsCurrImg = img;
    itsCurrImgID = gistSalMsg->RequestID;
    its_Curr_Img_mutex.unlock();

    LINFO("Got a GSMessage with Request ID = %d", currRequestID);
  }

  // motor message
  else if(eMsg->ice_isA("::BeobotEvents::MotorMessage"))
    {
      BeobotEvents::MotorMessagePtr mtrMsg =
        BeobotEvents::MotorMessagePtr::dynamicCast(eMsg);
      LDEBUG("Got a MotorMessage with Request ID = %d: RC Trans %f, Rot %f",
             mtrMsg->RequestID, itsRcTransSpeed, itsRcRotSpeed);
      its_Curr_Mtr_mutex.lock();
      itsRemoteMode = mtrMsg->rcMode;
      itsRcTransSpeed = mtrMsg->rcTransVel;
      itsRcRotSpeed = mtrMsg->rcRotVel;
      its_Curr_Mtr_mutex.unlock();
    }

  // corner location message
  else if(eMsg->ice_isA("::BeobotEvents::CornerLocationMessage"))
    {
      BeobotEvents::CornerLocationMessagePtr clMsg =
        BeobotEvents::CornerLocationMessagePtr::dynamicCast(eMsg);
      LINFO("Turn on on corner %d", clMsg->cornerLocation);

      // check if the corner location exceed the maximum index
      int index = clMsg->cornerLocation;
      LINFO("Got Corner location Message Corner:[%d]",index);

      //only take new command if corner system is not running
      its_Corner_Loc_mutex.lock();
      int status = itsStatus;
      its_Corner_Loc_mutex.unlock();

      if(status == C_PROCEDURE_INACTIVE)
        {
          if(index >= (int)itsVisualObjectDBs.size()) index = -1;
          its_Corner_Loc_mutex.lock();
          itsStatus = C_PROCEDURE_ACTIVE;
          itsTeachImgID = 0;
          itsDBnum = index;
          its_Corner_Loc_mutex.unlock();
        }
    }
}

// ######################################################################
void CornerNavigation::updateMotorPID(double tran, double rot,double error)
{
  error/=100.0;
  if(itsDir == 1)
    error *= -1.0;
  double pTerm,iTerm,dTerm;
  pTerm = itsPGain * error;
  itsIState += error;
  if(itsIState > IMAX)
    itsIState = IMAX;
  else if(itsIState < IMIN)
    itsIState = IMIN;
  iTerm = itsIGain * itsIState;
  dTerm = itsDGain * (rot - itsDState);
  itsDState = rot;
  double pid = pTerm + iTerm - dTerm;

  LINFO("P[%1.2f] I[%1.2f] D[%1.2f], Istate[%1.2f] DState[%1.2f]",
        pTerm,iTerm,dTerm,itsIState,itsDState);
  LINFO("pid[%1.2f],rot[%1.2f]",pid,rot);
  updateMotor(tran,pid,itsStatus);
}

// // ######################################################################
// void CornerNavigation::updateMotor(double tran, double rot)
// {
//   BeobotEvents::MotorRequestPtr msg = new BeobotEvents::MotorRequest;
//   msg->transVel = tran;
//   msg->rotVel   = rot;
//   this->publish("MotorRequestTopic", msg);
//   LINFO("[%d] Publish motor request Trans %f Rotation %f",
//         itsPrevProcImgID,tran,rot);
// }

// ######################################################################
void CornerNavigation::updateMotor(double tran, double rot, int status)
{
  BeobotEvents::CornerMotorRequestPtr msg =
    new BeobotEvents::CornerMotorRequest;
  msg->transVel = tran;
  msg->rotVel   = rot;
  msg->status   = status;
  this->publish("CornerMotorRequestTopic", msg);
  LINFO("[%d] Publish corner motor request Trans %f Rotation %f",
        itsPrevProcImgID, tran, rot);
}

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