
//=============================================================================
//
// gsurf_match.cpp
// This a simple program that performs image matching		
// between two different images
//
// The program uses the FastHessian detector and the user	
// can choose between the different descriptors that are	
// included in the OpenGSURF library						
//
// The correspondences between two images can be found by an estimation of 
// the fundamental matrix. In this case a RANSAC procedure is run.
//
// Correspondences can be found also if you provide a .txt file with the 
// homography between the two images
// 
// The OpenGSURF library is based on the original OpenSURF library created
// by C. Evans			 
//
// Author: Pablo F. Alcantarilla
// Date: 11/11/2011
// Email: pablofdezalc@gmail.com
//=============================================================================

#include "gsurf_match.h"

//**************************************************************************************
//**************************************************************************************

int main(int argc, char *argv[])
{
  // Some variables for measuring computation times
  struct timeval tod1;
  double t2 = 0.0, t1 = 0.0, tsurf = 0.0, tmatch = 0.0, thomo = 0.0;
  
  // Some variables for input arguments
  char file1[NMAX_CHARACTERS], fileN[NMAX_CHARACTERS], fileH[NMAX_CHARACTERS], fileGT[NMAX_CHARACTERS];
  char fileHessian[NMAX_CHARACTERS], fileMode[NMAX_CHARACTERS], fileExtended[NMAX_CHARACTERS], fileUpright[NMAX_CHARACTERS];
  
  // Other variables
  unsigned int img1WIDTH = 0, img1HEIGHT = 0, imgNWIDTH = 0, imgNHEIGHT = 0, imgWIDTHR = 0, imgHEIGHTR = 0;
  unsigned int nkeypoints1 = 0, nkeypointsN = 0, nmatches = 0, ninliers = 0, noutliers = 0, iteration = 1;
  double H[3][3], HG[3][3]; // Homography matrices
  double ehomo = 0.0, ratio = 0.0; 
  double rfactor = .90; // Resize factor for the composite image
  double hessiant = 1.5; // Hessian threshold for feature detection
  unsigned int extended = 0, upright = 0, ground_truth = 0;
  unsigned int mode = 0; // Descriptor mode, by default 0
  vector<int> ptpairs, inliers;
	
  // Vectors of keypoints
  IpVec ipts1, iptsN;
  
  // OpenCV Images
  IplImage *img1,*img1_bw,*imgN,*imgN_bw,*img_com, *img_r; 

  // Fill the homography matrices to zero
  for( int i = 0; i < 3; i++ )
  for( int j = 0; j < 3; j++ )
  {
	   H[i][j] = HG[i][j] = 0.0;
  }

  // Check the input arguments
  if( argc < 9  )
  {
	  cout << "Error introducing video files" << endl;
      cout << "Number of Input Parameters: " << argc << endl;
	  cout << "Format: ./gsurf_match /img1 /imgN /H ground_truth Hthreshold mode extended upright" << endl;
	  cout << "/img1: Path of the first image" << endl;
	  cout << "/imgN: Path of the N image" << endl;
	  cout << "/H: Ground truth homography file" << endl;
	  cout << "ground_truth: Set to 1 in case we want to use the ground truth homography. Otherwise compute the fundamental matrix" << endl;
	  cout << "Hthreshold: Hessian threshold for feature detection (FastHessian)" << endl;
	  cout << "mode: Descriptors mode (See README file for more details)" << endl;
	  cout << "extended: Set to 1 in case we want to use the extended version of the descriptors" << endl;
	  cout << "upright: Set to 1 in case we want to use the upright version of the descriptors (not invariant to rotation)" << endl;
	
	  return -1;
  }

  // Copy the input parameters
  strcpy(file1,argv[1]);
  strcpy(fileN,argv[2]);
  strcpy(fileH,argv[3]);
  strcpy(fileGT,argv[4]);
  strcpy(fileHessian,argv[5]);
  strcpy(fileMode,argv[6]);
  strcpy(fileExtended,argv[7]);
  strcpy(fileUpright,argv[8]);

  mode = atoi(fileMode);
  hessiant = atof(fileHessian);
  ground_truth = atoi(fileGT);
  extended = atoi(fileExtended);
  upright = atoi(fileUpright);
  hessiant = 0.0001*hessiant; // Multiply the Hessian threshold value by a constant factor

//**************************************************************************************
//**************************************************************************************

  // OpenCV Visualization
  cvNamedWindow("SURF Image", 1 );

  // Read images
  img1  = cvLoadImage(file1);

  img1WIDTH = img1->width;
  img1HEIGHT = img1->height;

  img1_bw = cvCreateImage(cvSize(img1WIDTH,img1HEIGHT),IPL_DEPTH_8U,1);
  cvCvtColor(img1,img1_bw,CV_BGR2GRAY);

  imgN = cvLoadImage(fileN);

  imgNWIDTH = imgN->width;
  imgNHEIGHT = imgN->height;

  imgWIDTHR = (unsigned int)(rfactor*2.0*imgN->width);
  imgHEIGHTR = (unsigned int)(rfactor*imgN->height);

  imgN_bw = cvCreateImage(cvSize(imgNWIDTH,imgNHEIGHT),IPL_DEPTH_8U,1);
  cvCvtColor(imgN,imgN_bw,CV_BGR2GRAY);

  img_com = cvCreateImage(cvSize(2*img1WIDTH,img1HEIGHT),IPL_DEPTH_8U,3);
  img_r = cvCreateImage(cvSize(imgWIDTHR,imgHEIGHTR),IPL_DEPTH_8U,3);

  // SURF Features for images 1 and N
  gettimeofday(&tod1,NULL);
  t1 = tod1.tv_sec + tod1.tv_usec / 1000000.0;

  surfDetDes(img1_bw,ipts1,mode,(bool)upright,extended,4,4,2,hessiant);
  surfDetDes(imgN_bw,iptsN,mode,(bool)upright,extended,4,4,2,hessiant);

  gettimeofday(&tod1,NULL);
  t2 = tod1.tv_sec + tod1.tv_usec / 1000000.0;
  tsurf = 1000.0*(t2 - t1);

  nkeypoints1 = ipts1.size();
  nkeypointsN = iptsN.size();

  // Read Ground Truth Homography!!
  Read_Homography_Parameters(fileH,HG);

  // Matching Descriptors!!
  gettimeofday(&tod1,NULL);
  t1 = tod1.tv_sec + tod1.tv_usec / 1000000.0;

  nmatches = Matching_Descriptor(ipts1,iptsN,ptpairs);

  gettimeofday(&tod1,NULL);
  t2 = tod1.tv_sec + tod1.tv_usec / 1000000.0;

  tmatch = 1000.0*(t2 - t1);

  // Compute Inliers!!
  gettimeofday(&tod1,NULL);
  t1 = tod1.tv_sec + tod1.tv_usec / 1000000.0;

  if( ground_truth == 1 )
  {
	 Compute_Inliers_GT_Homography(ipts1,iptsN,ptpairs,inliers,HG);	  
  }
  else
  {
	 Compute_Inliers_Homography_RANSAC(ipts1,iptsN,ptpairs,inliers,H);
  }

  gettimeofday(&tod1,NULL);
  t2 = tod1.tv_sec + tod1.tv_usec / 1000000.0;

  thomo = 1000.0*(t2 - t1);

  ninliers = inliers.size()/2;
  noutliers = nmatches - ninliers;

  ratio = 100.0*((double) ninliers / (double) nmatches);

  // Compute Homography Error
  ehomo = Compute_Homography_Error(H,HG);

  // Draw SURF Points
  drawIpoints(img1,ipts1);
  drawIpoints(imgN,iptsN);

  // Draw the composite Image
  Composite_Image_with_Line(img1,imgN,img_com,ipts1,iptsN,inliers);
  cvResize(img_com,img_r,CV_INTER_LINEAR);

  // Display some information
  if( VERBOSE )
  {
	 cout << "Time SURF Detector+Descriptor Images 1 and N (ms): " << tsurf << endl;
	 cout << "Matching Descriptors Time (ms): " << tmatch << endl;
	 cout << "Homography Computation Time (ms): " << thomo << endl;
	 cout << "Hessian Threshold: " << hessiant << endl;
     cout << "Descriptor Mode: " << mode << endl;
     cout << "Is Extended?: " << extended << endl;
     cout << "Is Upright?: " << upright << endl;
	 cout << "Number of Keypoints Image 1: " << nkeypoints1 << endl;
	 cout << "Number of Keypoints Image N: " << nkeypointsN << endl;
	 cout << "Number of Matches: " << nmatches << endl;
	 cout << "Number of Inliers: " << ninliers << endl;
	 cout << "Number of Outliers: " << noutliers << endl;
	 cout << "Inliers Ratio: " << ratio << endl;
	 cout << "Homography Estimation Error: " << ehomo << endl << endl;
  }
  
  // Image visualization
  while( iteration == 1 )
  {
     cvShowImage("SURF Image",img_r);
     cvMoveWindow("SURF Image",50,50);

     if( cvWaitKey(10) >= 0 )
      {
		if( iteration == 1)
		  iteration = 2;
		else
		  iteration = 1;
      }
  }

  // Release the images and destroy the OpenCV window
  cvReleaseImage(&img1);
  cvReleaseImage(&img1_bw);
  cvReleaseImage(&imgN);
  cvReleaseImage(&imgN_bw);
  cvReleaseImage(&img_com);
  cvReleaseImage(&img_r);

  cvDestroyWindow("SURF Image");
}

//**************************************************************************************
//**************************************************************************************

// Match the descriptors of the first frame with respect to the others
unsigned int Matching_Descriptor(IpVec &ipts_orig, IpVec &ipts, vector<int> &indexes )
{
   double dist = 0.0, mind = 0.0, last_mind = 0.0;
   int mindex = -1, last_mindex = -1;
   unsigned int correct_matches = 0;
   bool first = false;

   indexes.clear();

   for( unsigned int i = 0; i < ipts_orig.size(); i++ )
   {
		mind = 10000.0;
		last_mind = 10000.0;
		mindex = -1;
		last_mindex = -1;
		first = false;

		for( unsigned int j = 0; j < ipts.size(); j++ )
		{
			dist = Compute_SURF_Descriptor_Distance(ipts_orig[i],ipts[j],1000.0);

			if( dist < mind )
			{
				if( first == false )
				{	mind = dist;
					mindex = j;
					first = true;
				}
				else
				{
					last_mind = mind;
					mind = dist;
					last_mindex = mindex;
					mindex = j;
				}
			}
			else if( dist < last_mind )
			{
				 last_mind = dist;
				 last_mindex = j;
			}
		}

	if( mind < DRATIO*last_mind )
	{
	    indexes.push_back(i);
	    indexes.push_back(mindex);
	    correct_matches++;
	}
  }

  return correct_matches;
}

//**************************************************************************************
//**************************************************************************************

// This function computes the Euclidean distance between two descriptors				
double Compute_SURF_Descriptor_Distance(Ipoint &p1, Ipoint &p2, double best)
{
    double dist = 0.0;
    int desc_size = p1.size;
		
    for(int i = 0; i < desc_size; i++ )
    {
		dist += pow(p1.descriptor[i] - p2.descriptor[i],2);
	
		if( dist > best )
		break;
    }

 return dist;

} 

//**************************************************************************************
//**************************************************************************************

// This function computes the inliers according to a ground truth homography
void Compute_Inliers_GT_Homography(IpVec &ipts1, IpVec &iptsN, vector<int> &ptpairs, vector<int> &inliers, double H[3][3])
{
   double s = 0.0, x2 = 0.0, y2 = 0.0, x1 = 0.0, y1 = 0.0, dist = 0.0, x2m = 0.0, y2m = 0.0;

   inliers.clear();

   for( unsigned int i = 0; i < ptpairs.size(); i+= 2 )
   {
		x1 = ipts1[ptpairs[i]].x;
		y1 = ipts1[ptpairs[i]].y;

		x2 = iptsN[ptpairs[i+1]].x;
		y2 = iptsN[ptpairs[i+1]].y;

		s = H[2][0]*x1 + H[2][1]*y1 + H[2][2];
		x2m = (H[0][0]*x1 + H[0][1]*y1 + H[0][2]) / s;
		y2m = (H[1][0]*x1 + H[1][1]*y1 + H[1][2]) / s;

		dist = sqrt( pow(x2m-x2,2) + pow(y2m-y2,2));

		if( dist <= MIN_H_ERROR )
		{
			inliers.push_back(ptpairs[i]);
			inliers.push_back(ptpairs[i+1]);
		}
   }
}

//**************************************************************************************
//**************************************************************************************

// This function computes the inliers according to a homography that is obtained	
// in a RANSAC process checking the fundamental matrix
void Compute_Inliers_Homography_RANSAC(IpVec &ipts1, IpVec &iptsN, vector<int> &ptpairs, 
							   vector<int> &inliers, double H[3][3])
{
   unsigned int npoints = 0, count = 0;

   inliers.clear();

   for(unsigned int i = 0; i < 3; i++ )
	for( unsigned int j = 0; j < 3; j++ )
		H[i][j] = 0.0;

   npoints = ptpairs.size()/2;

   double Src_points[2][npoints];
   double Dst_points[2][npoints];
   unsigned char nerrors[npoints];

   for( unsigned int i = 0; i < ptpairs.size(); i+=2 )
   {
		Src_points[0][count] = ipts1[ptpairs[i]].x;
		Src_points[1][count] = ipts1[ptpairs[i]].y;

		Dst_points[0][count] = iptsN[ptpairs[i+1]].x;
		Dst_points[1][count] = iptsN[ptpairs[i+1]].y;

		count++;
   }

   CvMat homography = cvMat(3,3,CV_64FC1,H);
   CvMat src_points = cvMat(2,npoints,CV_64FC1,Src_points);
   CvMat dst_points = cvMat(2,npoints,CV_64FC1,Dst_points);
   CvMat errors_points = cvMat(1,npoints,CV_8S,nerrors); 
   
   cvFindFundamentalMat(&src_points,&dst_points,&homography,CV_FM_RANSAC,MIN_H_ERROR,0.99,&errors_points );

   unsigned int icount = 0;
   for( unsigned int i = 0; i < ptpairs.size(); i+= 2 )
   {
		bool flag = nerrors[icount];

		if( flag == true )
		{
			inliers.push_back(ptpairs[i]);
			inliers.push_back(ptpairs[i+1]);
		}

		icount++;
   }
}

//**************************************************************************************
//**************************************************************************************

// Function to read the homography parameters from a .txt file
void Read_Homography_Parameters(char *calib_file, double HG[3][3])
{
   double h11 = 0.0, h12 = 0.0, h13 = 0.0;
   double h21 = 0.0, h22 = 0.0, h23 = 0.0; 
   double h31 = 0.0, h32 = 0.0, h33 = 0.0;
   int  tmp_buf_size = NMAX_CHARACTERS;
   char tmp_buf[tmp_buf_size];
   
   setlocale(LC_ALL,"C");

   string filename(calib_file);

   ifstream infile;
   infile.exceptions ( ifstream::eofbit | ifstream::failbit | ifstream::badbit );
   infile.open(filename.c_str(), ifstream::in);

   string tmp_string;

   infile.getline(tmp_buf,tmp_buf_size);
   sscanf(tmp_buf,"%lf %lf %lf",&h11,&h12,&h13);

   infile.getline(tmp_buf,tmp_buf_size);
   sscanf(tmp_buf,"%lf %lf %lf",&h21,&h22,&h23);

   infile.getline(tmp_buf,tmp_buf_size);
   sscanf(tmp_buf,"%lf %lf %lf",&h31,&h32,&h33);

   infile.close();

   HG[0][0] = h11 / h33;
   HG[0][1] = h12 / h33;
   HG[0][2] = h13 / h33;

   HG[1][0] = h21 / h33;
   HG[1][1] = h22 / h33;
   HG[1][2] = h23 / h33;

   HG[2][0] = h31 / h33;
   HG[2][1] = h32 / h33;
   HG[2][2] = h33 / h33;

}

//**************************************************************************************
//**************************************************************************************

// Compute the homography error
double Compute_Homography_Error( double H[3][3], double HG[3][3] )
{
  double error = 0.0;

  for( unsigned int i = 0; i < 3; i++ )
  for( unsigned int j = 0; j < 3; j++ )
  {
	   error += pow(H[i][j] - HG[i][j],2);
  }

  error = sqrt(error);

  return error;
}

//**************************************************************************************
//**************************************************************************************

// Function to fuse two images and depict the matched points with a line
void Composite_Image_with_Line(IplImage *img1,IplImage *imgN,IplImage *img_composite,IpVec &ipts1, IpVec &iptsN, vector<int> ptpairs)
{
   CvPoint c1, cN;

   for( int i = 0; i < img1->height; i++ )
	for( int j = 0; j < img1->width*2; j++ )
	{
	     if( j < img1->width )
	     {
			cvSet2D(img_composite,i,j,cvGet2D(img1,i,j));
	     }
	     else
	     {
			cvSet2D(img_composite,i,j,cvGet2D(imgN,i,j-imgN->width));
	     }
	}

   for( int i = 0; i < (int) ptpairs.size(); i+= 2)
   {
		c1.x = (int) ipts1[ptpairs[i]].x;
		c1.y = (int) ipts1[ptpairs[i]].y;
		
		cN.x = (int) iptsN[ptpairs[i+1]].x + img1->width;
		cN.y = (int) iptsN[ptpairs[i+1]].y;
		
		cvLine(img_composite,c1,cN,CV_RGB(255,0,0),2,8,0);
   }
}

//**************************************************************************************
//**************************************************************************************

// Function for saving matching results information
void Save_Matching_Results(double ratio, double ninliers, double ehomo, IplImage *img_com)
{
  ofstream ratio_out, inliers_out, ehomo_out;

  ratio_out.open("./results/ratio.txt",ios::app);
  inliers_out.open("./results/ninliers.txt",ios::app);
  ehomo_out.open("./results/ehomo.txt",ios::app);

  ratio_out << ratio << endl;
  inliers_out << ninliers << endl;
  ehomo_out << ehomo << endl;

  cvSaveImage("./results/img_match.bmp",img_com);

  ratio_out.close();
  inliers_out.close();
  ehomo_out.close();

}

//**************************************************************************************
//**************************************************************************************

void Save_TPR_FPR_Curves(double tpr, double fpr)
{
  ofstream tpr_out, fpr_out;

  tpr_out.open("./results/tpr_ratio.txt",ios::app);
  fpr_out.open("./results/fpr_ratio.txt",ios::app);

  tpr_out << tpr << endl;
  fpr_out << fpr << endl;

  tpr_out.close();
  fpr_out.close();

}

//**************************************************************************************
//**************************************************************************************

// Save the interest points to a regular ASCII file
void SaveIpoints(char *sFileName, const IpVec &ipts, bool bVerbose, bool bLaplacian, unsigned int mode, int extended) 
{
  int VLength = 0;
  double sc;
  unsigned count = ipts.size();
  
  if( extended == 0 )
  {
	 VLength = 64; 
  }
  else
  {
	 VLength = 128;
  }
	
  ofstream ipfile(sFileName);

  if( !ipfile )
  {
    cerr << "ERROR in loadIpoints(): "
         << "Couldn't open file '" << sFileName << "'!" << endl;
    return;
  }

  // Write the file header
  if(bLaplacian)
  {
	 ipfile << VLength + 1 << endl << count << endl;
  }
  else
  {
	 ipfile << VLength << endl << count << endl;
  }
    
  // In order to just save the interest points without descriptor, comment 
  // the above and uncomment the following command.
  // ipfile << 1.0 << endl << count << endl;
  // Save interest point with descriptor in the format of Krystian Mikolajczyk
  // for reasons of comparison with other descriptors. As our interest points 
  // are circular in any case, we use the second component of the ellipse to 
  // provide some information about the strength of the interest point. This is 
  // important for 3D reconstruction as only the strongest interest points are 
  // considered. Replace the strength with 0.0 in order to perform Krystian's 
  // comparisons.
  for (unsigned int n=0; n<ipts.size(); n++)
  {
    // circular regions with diameter 5 x scale
    sc = 2.5 * ipts[n].scale; sc*=sc;
    ipfile  << ipts[n].x /* x-location of the interest point */
            << " " << ipts[n].y /* y-location of the interest point */
            << " " << 1.0/sc /* 1/r^2 */
            << " " << 0.0     //(*ipts)[n]->strength /* 0.0 */
            << " " << 1.0/sc; /* 1/r^2 */

    if( bLaplacian)
    {  
		ipfile << " " << ipts[n].laplacian;
	}

    // Here comes the descriptor
    for( int i = 0; i < VLength; i++)
	{
		ipfile << " " << ipts[n].descriptor[i];
	}
	
    ipfile << endl;
  }

  ipfile.close();

  // Write message to terminal.
  if( bVerbose )
  {
	  cout << count << " interest points found" << endl;  
  }
}

//**************************************************************************************
//**************************************************************************************

// Save the interest points to a regular ASCII file
void SaveIpoints_for_SIFT(char *sFileName, const IpVec &ipts) 
{
  ofstream ipfile(sFileName);

  if( !ipfile )
  {
    cerr << "ERROR in loadIpoints(): "
         << "Couldn't open file '" << sFileName << "'!" << endl;
    return;
  }

  for (unsigned int n=0; n<ipts.size(); n++)
  {
    ipfile  << ipts[n].x /* x-location of the interest point */
            << " " << ipts[n].y /* y-location of the interest point */
            << " " << ipts[n].scale /* scale */
	    << " " << ipts[n].orientation << endl;
  }

  ipfile.close();

}

//**************************************************************************************
//**************************************************************************************

// Load the interest points from a regular ASCII file
void LoadIpoints(char *sFileName,IpVec & ipts, bool bVerbose) 
{
  unsigned int length = 0, nkeypoints = 0;
  int  tmp_buf_size = 256, laplacian = 0;
  char tmp_buf[tmp_buf_size];
  float dummy = 0.0, x = 0.0, y = 0.0, a = 0.0, b = 0.0, c = 0.0;
  float det = 0.0, e1 = 0.0, e2 = 0.0, l1 = 0.0, l2 = 0.0, sc = 0.0;

  string filename(sFileName);

  ifstream infile;
  infile.exceptions ( ifstream::eofbit | ifstream::failbit | ifstream::badbit );
  infile.open(filename.c_str(), ifstream::in);

  infile.getline(tmp_buf,tmp_buf_size);
  sscanf(tmp_buf,"%d",&length);

  infile.getline(tmp_buf,tmp_buf_size);
  sscanf(tmp_buf,"%d",&nkeypoints);

  if( bVerbose )
  {
	 cout << "length: " << length << endl;
	 cout << "nkey: " << nkeypoints << endl;
  }

  //Create a new interest point vector
  ipts.clear();
  ipts.resize(nkeypoints);

  // Load the interest points in Mikolajczyk's format
  for(unsigned int n = 0; n < nkeypoints; n++)
  {
    infile >> x >> y >> a >> b >> c >> laplacian;

    for( unsigned int k = 0; k < length-1; k++ )
	infile >> dummy;

    det = sqrt((a-c)*(a-c) + 4.0*b*b);
    e1 = 0.5*(a+c + det);
    e2 = 0.5*(a+c - det);
    l1 = (1.0/sqrt(e1));
    l2 = (1.0/sqrt(e2));
    sc = sqrt( l1*l2 );

    ipts[n].x     = x;
    ipts[n].y     = y;
    ipts[n].scale = sc/2.5;
    ipts[n].laplacian = laplacian;
  }

  // close the interest point file again
  infile.close();

  // Write message to terminal.
  if( bVerbose )
  {
	  cout << "read in " << nkeypoints << " interest points." << endl;
  }
    
}

