/***************************************************************************
 * $Id: extCalibration.cc 78 2008-09-22 12:00:13Z kaori $
 *
 *  Copyright Kaori Hagihara
 *  Email: kaori.hagihara@uclouvain.be
 ****************************************************************************/

/*
 * extCalibration
 * Estimate camera extrinsic parameters
 *
 * usage: ./extCalibration candidatePnts_file_path intrinsic_matrix projection_image output_directory_path
 */

#include <cv.h>
#include <highgui.h>
#include <iostream>
#include <fstream>
#include <vector>

using namespace std;

void load_candPnts( const char *fname, vector<CvPoint2D32f> *v_origPnts, vector<CvPoint3D32f> *v_objPnts);
void plot_onImage( CvMat *points, IplImage *inImg, IplImage *outImg);
void loadIntrinsicPar( const char fname[], CvMat *intrinsicMat, CvMat *distCoeffs);
void saveExtrinsicPar( const char fname[], CvMat *rotationVec, CvMat *transVec);

int main(int argc,char **argv)
{
  if(argc < 5){
    cerr << "Usage: candidate points file name" << endl
	 << "intrinsic matrix file name" << endl
	 << "projection image" << endl
	 << "output directory" << endl;
    return -1;
  }

  //
  // Data Definition
  //
  CvMat *origPnts, *objPnts, *projPnts;
  CvMat *intrinsicMat, *distCoeffs;
  CvMat *rotationVec, *transVec;
  vector<CvPoint2D32f> v_origPnts;
  vector<CvPoint3D32f> v_objPnts;
  int num_of_points;

  // Output
  string outPath;
  string fname;
  IplImage *inImg, *outImg;
  char windowName[] = "exCalibration Result";
  
  //
  // load data
  //
  load_candPnts( argv[1], &v_origPnts, &v_objPnts);
  
  num_of_points = v_origPnts.size(); // == v_objPnts.size()
  origPnts = cvCreateMat( num_of_points, 2, CV_64FC1);
  objPnts  = cvCreateMat( num_of_points, 3, CV_64FC1);

  for( int i=0; i<num_of_points; i++){
    cvSetReal2D( origPnts, i, 0, v_origPnts[i].x);
    cvSetReal2D( origPnts, i, 1, v_origPnts[i].y);
    cvSetReal2D( objPnts, i, 0, v_objPnts[i].x);
    cvSetReal2D( objPnts, i, 1, v_objPnts[i].y);
    cvSetReal2D( objPnts, i, 2, v_objPnts[i].z);
  }

  intrinsicMat = cvCreateMat( 3, 3, CV_32FC1);
  distCoeffs = cvCreateMat( 4, 1, CV_32FC1);

  loadIntrinsicPar( argv[2], intrinsicMat, distCoeffs);

  //
  // Calibration
  //
  rotationVec = cvCreateMat( 3, 1, CV_32FC1);
  transVec = cvCreateMat( 3, 1, CV_32FC1);

  cvFindExtrinsicCameraParams2( objPnts, origPnts, intrinsicMat, distCoeffs /*NULL*/, rotationVec, transVec);

  outPath = argv[4];
  fname = outPath + "extrinsicPars.ini";
  
  saveExtrinsicPar( fname.c_str(), rotationVec, transVec);

  //
  // Show the output
  //
  projPnts = cvCreateMat( num_of_points, 2, CV_64FC1);
  cvProjectPoints2( objPnts, rotationVec, transVec, intrinsicMat, distCoeffs, projPnts);

  inImg = cvLoadImage( argv[3], CV_LOAD_IMAGE_GRAYSCALE);
  outImg = cvCreateImage( cvGetSize( inImg), IPL_DEPTH_8U, 3);

  plot_onImage( projPnts, inImg, outImg);
  
  cvNamedWindow( windowName);
  cvShowImage( windowName, outImg);
  cvWaitKey(0);
  cvDestroyWindow( windowName ); 
    
  cvReleaseMat( &origPnts);
  cvReleaseMat( &objPnts);
  cvReleaseMat( &intrinsicMat);
  cvReleaseMat( &distCoeffs);
  cvReleaseMat( &rotationVec);
  cvReleaseMat( &transVec);

  cvReleaseImage( &inImg);
  cvReleaseImage( &outImg);
}

void load_candPnts( const char *fname, vector<CvPoint2D32f> *v_origPnts, vector<CvPoint3D32f> *v_objPnts)
{
  float x, y, X, Y, Z;

  cout << "open file " << fname << endl;
  ifstream inf( fname);
  if( !inf){
    cerr << "Failed to open " << fname << endl;
    exit( -1);
  }

  do{
    inf >> x >> y >> X >> Y >> Z;
    v_origPnts->push_back( cvPoint2D32f( x, y));
    v_objPnts->push_back( cvPoint3D32f( X, Y, Z));
  }while(!inf.eof());
  
  inf.close(); 
}

void plot_onImage( CvMat *points, IplImage *inImg, IplImage *outImg)
{
  int x, y;

  for( int coi=1; coi<=3; coi++){
    cvSetImageCOI( outImg, coi );
    cvCopy( inImg, outImg);
  }
  cvSetImageCOI( outImg, 0);
  
  for(int i=0; i<points->rows; i++){
    x = (int)cvGetReal2D( points, i, 0);
    y = (int)cvGetReal2D( points, i, 1);
    cvCircle ( outImg, cvPoint(x,y), 2, cvScalar(0,0,255), -1);
  }
}

void loadIntrinsicPar( const char fname[], CvMat *intrinsicMat, CvMat *distCoeffs)
{
  cout << "open file " << fname << endl;
  ifstream inf( fname);
  if(!inf){
    cerr << "Failed to open " << fname << endl;
    return;
  }
  
  float value;
  
  for( int i=0; i<3; i++)
    for( int j=0; j<3; j++){
      inf >> value;
      cvSetReal2D( intrinsicMat, i, j, value );
    }

  for( int i=0; i<4; i++)
    cvSetReal2D( distCoeffs, i, 0, 0.0);

  inf.close();
}

void saveExtrinsicPar( const char fname[], CvMat *rotationVec, CvMat *transVec)
{
  cout << "saving Extrinsic Parameters to " << fname << endl;

  ofstream of( fname);
  if(!of){
    cerr << "Failed to open " << fname << endl;
    exit (-1);
  }
  
  of << "Translation Vector" << endl
     << "Rotation Vector" << endl << endl;

  for (int i=0; i < 3; i++)
    of << cvGetReal2D( transVec, i, 0) << '\t';
  of << endl;
  for (int i=0; i < 3; i++)
    of << cvGetReal2D( rotationVec, i, 0) << '\t';
  of << endl << endl;

  of.close();  
}
