#include "cv.h"
#include "highgui.h"

#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <iostream.h>
#include <fstream.h>

#include "calibCam.h"

int CalibCam::totalNumImages = 10;
int CalibCam::errorImageNum = 0;

CalibCam::CalibCam()
{
  sourceI = 0;
  errorI = 0;
  
  inMat = cvMat(3, 3, CV_64FC1);
  exRot = cvMat(3, 3, CV_64FC1);
  exTrs = cvMat(3, 1, CV_64FC1);
  m2InMat = cvMat(3, 3, CV_64FC1);
  m2ExRot = cvMat(3, 3, CV_64FC1);
  m2ExTrs = cvMat(3, 1, CV_64FC1);
  
  tempM1 = cvMat(3, 1, CV_64FC1);
  tempM2 = cvMat(3, 1, CV_64FC1);
  

  cvmAlloc(&inMat);
  cvmAlloc(&exRot);
  cvmAlloc(&exTrs);
  cvmAlloc(&m2InMat);
  cvmAlloc(&m2ExRot);
  cvmAlloc(&m2ExTrs);
  cvmAlloc(&tempM1);
  cvmAlloc(&tempM2);
  
  
}

//************************************************************************
CalibCam::~CalibCam()
{
  cvmFree(&inMat);
  cvmFree(&exRot);
  cvmFree(&exTrs);
  cvmFree(&m2InMat);
  cvmFree(&m2ExRot);
  cvmFree(&m2ExTrs);
  cvmFree(&tempM1);
  cvmFree(&tempM2);
  cvmFree(&AMat);
  cvmFree(&projMat);
  
}
  

//***********************************************
//*   initialize CalibCam with
//*            numI---number of images
//*            numPPI---number of points per image
//*            fileN---file-number: for example
//*                   pixel05.txt pixel12.txt
//*              fileN is: 5, 12 or 05, 12
void CalibCam::initialize(int numI, int numPPI, int *fileN)
{
  int i, j, k, m;
  
  fstream fileP, fileW;
  char fileNameP[80], fileNameW[80], tempC[10];

  numImages = numI;
  numPoints = new int[numI];
  imageNums = new int[numI];
  distortion =   new double[4];
  cameraMatrix = new double[9];
  transVects =   new double[numImages*3];
  rotMatrs =     new double[numImages*9];
  
  for(i = 0;i < 9; i++)
    cameraMatrix[i] = 0.0;
  for(i = 0;i < 4; i++)
    distortion[i] = 0.0;

  useIntrinsicGuess = 0;

  for (i = 0; i < numI; i++)
    numPoints[i] = numPPI;

  imagePoints = new CvPoint2D64d [numI * numPPI];
  objectPoints = new CvPoint3D64d [numI * numPPI];
  totalImagePts = new CvPoint2D64d [totalNumImages * numPPI];
  totalObjectPts = new CvPoint3D64d [totalNumImages * numPPI];
  computedImgPts = new CvPoint2D64d [numPPI];

  for (i = 1; i < totalNumImages + 1; i++) {
    sprintf(fileNameP, "pixel%02d.txt", i);
    sprintf(fileNameW, "world%02d.txt", i);
    
    fileP.open(fileNameP);
    if( ! fileP ) {
      cout << "Error in open fileP. "<<endl;
      exit(1);
    }
    fileP.setf(ios::scientific);
    fileW.open(fileNameW);
    if ( ! fileW ) {
      cout << "Error in open fileW. "<<endl;
    }
    fileP.setf(ios::scientific);

    for (j = 0; j < numPPI; j++) {
      k = (i - 1) * numPPI + j;
      fileP >> totalImagePts[k].x;
      fileP >> totalImagePts[k].y;
      fileW >> totalObjectPts[k].x;
      fileW >> totalObjectPts[k].y;
      totalObjectPts[k].z = 0.0;
      
    }
    
    fileP.close();
    fileW.close();
  }

  for (i = 0; i < numI; i++) {
    imageNums[i] = fileN[i];
    for (j = 0; j < numPPI; j++) {
      k = i * numPPI + j;
      m = (fileN[i] - 1) * numPPI + j;
      imagePoints[k].x = totalImagePts[m].x;
      imagePoints[k].y = totalImagePts[m].y;
      objectPoints[k].x = totalObjectPts[m].x;
      objectPoints[k].y = totalObjectPts[m].y;
      objectPoints[k].z = totalObjectPts[m].z;
    }
  }
 
  //initialize the AMat22222222222222222222222Method
  projMatInit();

}


//***********************************************************
//*         set image size
//
void CalibCam::getImageSize(int *fn)
{
  
  char fileName[80];
  char tempC[10];
  IplImage *img = 0;

  strcpy(fileName, "cap");
  sprintf(tempC, "%02d", fn[0]);
  strcat(fileName, tempC);
  strcat(fileName, ".bmp");

  if ( (img = cvLoadImage(fileName)) == 0 ) {
    cout<<" Cannot load image. "<<endl;
    exit(0);
  }
  
  imageSize = cvGetSize(img);
  cvReleaseImage(&img);
  //xxx cout<<"---imageSi "<<imageSize.width<<endl;;

}

void CalibCam::directCalibration()
{
  
  int i, j, k;

  cvCalibrateCamera_64d(numImages,
		       numPoints,
		       imageSize,
		       imagePoints,
		       objectPoints,
		       distortion,
		       cameraMatrix,
		       transVects,
		       rotMatrs,
		       useIntrinsicGuess);

  k = 0;
  for (i = 0; i < 3; i++)
    for (j = 0; j < 3; j++) {
      cvmSet(&inMat, i, j, cameraMatrix[k]);
      k++;
    }

}


void CalibCam::getExtrinsicParams(int whichImage)
{
  int i, j, k;

  CvVect64d t = transVects + 3 * whichImage;
  CvMatr64d r = rotMatrs + 9 * whichImage;
  
  k = 0;
  for (i = 0; i < 3; i++) {
    cvmSet(&exTrs, i, 0, t[i]);
    for (j = 0; j < 3; j++) {
      cvmSet(&exRot, i, j, r[k]);
      k++;
    }
  }

}
	     

void CalibCam::printCameraParams()
{
  int i, j;

  cout<<"Extrinsic Parameters: rotation matrix: "<<endl;
  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++)
      cout <<cvmGet(&exRot, i, j)<<"  ";
    cout<<endl;
  }
  cout<<"Extrinsic Parameters: translation vector: "<<endl;
  for (i = 0; i < 3; i++) {
    cout <<cvmGet(&exTrs, i, 0)<<"  ";
    cout<<endl;
  }
  cout<<"Intrinsic Parameters:    "<<endl;
  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++)
      cout <<cvmGet(&inMat, i, j)<<"  ";
    cout<<endl;
  }
  
}



CvPoint2D64d CalibCam::world2pixel(const CvPoint3D64d &wp)
{
 
  cvmSet(&tempM1, 0, 0, wp.x);
  cvmSet(&tempM1, 1, 0, wp.y);
  cvmSet(&tempM1, 2, 0, wp.z);
  
  cvmMul(&exRot, &tempM1, &tempM2);
  cvmAdd(&tempM2, &exTrs, &tempM2);
  cvmMul(&inMat, &tempM2, &tempM2);

  double test = cvmGet(&tempM2, 2, 0);
  if (test < 0) 
    test *= -1.0;
  if (test < 1.0e-8) {
    cout<<" Error in world2pixel.  "<<endl;
    exit(1);
  }

  CvPoint2D64d px = {cvmGet(&tempM2, 0, 0),
		     cvmGet(&tempM2, 1, 0)};
  px.x /= cvmGet(&tempM2, 2, 0);
  px.y /= cvmGet(&tempM2, 2, 0);

  return px;

}


//calculate all the image points corresponding to the world object
//   3D points:
//     input:  whichOne----image number 01, 02....
void CalibCam::world2pixelOneImage(int whichOne)
{
  int i, j, k;

  int imgPtr = whichOne - 1;
  
  //suppose all the images have SAME number of points
  imgPtr *= numPoints[0]; 

  for (i = imgPtr; i < imgPtr + numPoints[0]; i++)
    computedImgPts[i - imgPtr] = 
      world2pixel(totalObjectPts[i]);

  /*//xxx
  for (i = 0; i < numPoints[0]; i++) {
    cout<<"orig: point "<<i<<" pixel at: "<<totalImagePts[i+imgPtr].x<<"  " <<totalImagePts[i+imgPtr].y<<endl;
    cout<<"computed: point "<<i<<" pixel at: "<<computedImgPts[i].x<<"  " <<computedImgPts[i].y<<endl<<"------------"<<endl;
    }*/
  
    
}


void CalibCam::projectionE(int whichOne)
{
  int i, j, k;
  double dex, dey, error = 0.0;

  //find the extrinsic camera parameters
  CvPoint2D64d *iPoints; iPoints= totalImagePts;
  CvPoint3D64d *oPoints = totalObjectPts;
  iPoints += (whichOne - 1 ) * numPoints[0]; 
  oPoints += (whichOne -1) * numPoints[0];
  CvVect64d focalLength = new double[2];
  CvPoint2D64d principalPoint;//[1]; = new CvPoint2D64d [1];
  focalLength[0] = cvmGet(&inMat, 0, 0);
  focalLength[1] = cvmGet(&inMat, 1, 1);
  principalPoint.x = cvmGet(&inMat, 0, 2);
  principalPoint.y = cvmGet(&inMat, 1, 2);
  CvVect64d rotVect = new double [3];

  cvFindExtrinsicCameraParams_64d(numPoints[0],
				  imageSize,
				  iPoints,
				  oPoints,
				  focalLength,
				  principalPoint,
				  distortion,
				  rotVect,
				  transVects);
  getExtrinsicParams(0);
  
  CvMat jocMat,rotMatt;
  rotMatt = cvMat(3, 1, CV_64FC1);
  jocMat = cvMat(3, 9, CV_64FC1);
  cvmAlloc(&rotMatt); cvmAlloc(&jocMat);
  for (i = 0; i < 3; i++){
    for (j = 0; j < 9; j++)
    cvmSet(&jocMat, i, j, 0.0);
    cvmSet(&rotMatt, i, 0, rotVect[i]);
  }
  cvRodrigues(&exRot, &rotMatt, &jocMat,
	      CV_RODRIGUES_V2M);

  k =  (whichOne - 1) * numPoints[0];
  world2pixelOneImage(whichOne);
  for (i = 0; i < numPoints[0]; i++) {
    dex = (computedImgPts[i].x - totalImagePts[k + i].x);
    dey = (computedImgPts[i].y - totalImagePts[k + i].y);
    error += sqrt(dex * dex + dey * dey);
  }
    
  cout<<"projection error :  "<<error<<endl;
    
  
}


void CalibCam::accuracyE(int whichOne)
{
  int i, j, k;
  double dex, dey, error = 0.0;
  int innerN;

  for (i = 0; i < numImages; i++) 
    if (whichOne == imageNums[i]) {
      innerN = i;
      break;
    }
  if (i == numImages) {
    cout << " Error in accuracy ...(inner image number). "<<endl;
    exit(0);
  }
  
  getExtrinsicParams(innerN);
  world2pixelOneImage(whichOne);

  k = innerN * numPoints[0];
  for (i = 0; i < numPoints[0]; i++) {
    dex = (computedImgPts[i].x - imagePoints[k + i].x) * 
      (computedImgPts[i].x - imagePoints[k + i].x);
    dey = (computedImgPts[i].y - imagePoints[k + i].y) * 
      (computedImgPts[i].y - imagePoints[k + i].y);
    error += sqrt(dex + dey);
  }
  cout<<"accuracy error:  "<<error<<endl;

}


void CalibCam::displayErrorInImage(int whichOne)
{
  int i, j, k;
  char fileName[80];
  int dx = 3, dy = 3;
  double ix, iy, ex, ey;
  int ip,rgbi, rgbe;

  sprintf(fileName, "cap%02d.bmp", whichOne);
  if (sourceI != NULL) cvReleaseImage(&sourceI);
  if (errorI != NULL) cvReleaseImage(&errorI);
  if( (sourceI = cvLoadImage(fileName)) == 0 ) {
    cout << " Can not load image. " << endl;
    exit(1);
  }
  errorI = cvCloneImage(sourceI);

  cvNamedWindow("ErrorInImage", CV_WINDOW_AUTOSIZE);
  cvShowImage("ErrorInImage", errorI);
  
  ip = whichOne - 1;
  ip = ip * numPoints[0];
  rgbi = CV_RGB( 0, 255, 0 );
  rgbe = CV_RGB( 255, 0, 0 );
  for (i = 0; i < numPoints[0]; i++) {
    ix = totalImagePts[ip + i].x;
    iy = totalImagePts[ip + i].y;
    ex = computedImgPts[i].x;
    ey = computedImgPts[i].y;
    cvLine(errorI,
	   cvPoint(cvRound(ix-dx),cvRound(iy-dx)),
	   cvPoint(cvRound(ix+dx),cvRound(iy+dx)),
	   rgbi);
    cvLine(errorI,
	   cvPoint(cvRound(ix-dx),cvRound(iy+dx)),
	   cvPoint(cvRound(ix+dx),cvRound(iy-dx)),
	   rgbi);
    cvLine(errorI,
	   cvPoint(cvRound(ex-dx),cvRound(ey-dx)),
	   cvPoint(cvRound(ex+dx),cvRound(ey+dx)),
	   rgbe);
    cvLine(errorI,
	   cvPoint(cvRound(ex-dx),cvRound(ey+dx)),
	   cvPoint(cvRound(ex+dx),cvRound(ey-dx)),
	   rgbe);
    
  }
  cvShowImage("ErrorInImage", errorI);

  //save the marked image
  sprintf(fileName, "errorMarkedImg%02d_%02d.jpg", 
	  whichOne, errorImageNum);
  cvSaveImage(fileName, errorI);
  errorImageNum++;
}
  
//2222222222222222222222222222222222222222222222222222
//
void CalibCam::projMatInit()
{
  int i, j, k;
  double xw, yw, zw, xim, yim;

  int rows = numImages * numPoints[0] * 2;
  int cols = 12;
  AMat = cvMat(rows, cols, CV_64FC1);
  projMat = cvMat(3, 4, CV_64FC1);
  cvmAlloc(&AMat);
  cvmAlloc(&projMat);

  //xxx
  ofstream file; file.open("AMat.dat");int kk=0;

  for(i = 0; i < rows / 2; i++) {
    j = 2 * i;
    xw = objectPoints[i].x;
    yw = objectPoints[i].y;
    zw = objectPoints[i].z;
    xim = imagePoints[i].x;
    yim = imagePoints[i].y;
    /*if(i%96 == 0) {
      file<<endl<<"---- image:  "<<kk<<endl;
      kk++;}
      file<<"--- point :-----  "<<j<<endl;*/
    cvmSet(&AMat, j, 0, xw);file<<" "<<xw;
    cvmSet(&AMat, j, 1, yw);file<<" "<<yw;
    cvmSet(&AMat, j, 2, zw); file<<" "<<zw;
    cvmSet(&AMat, j, 3, 1.0);file<<" "<<1.0;
    cvmSet(&AMat, j, 4, 0.0);file<<" "<<0.0;
    cvmSet(&AMat, j, 5, 0.0);file<<" "<<0.0;
    cvmSet(&AMat, j, 6, 0.0);file<<" "<<0.0;
    cvmSet(&AMat, j, 7, 0.0);file<<" "<<0.0;
    cvmSet(&AMat, j, 8, 0.0-(xim * xw));file<<" "<<-(xim*xw);
    cvmSet(&AMat, j, 9, 0.0-(xim * yw));file<<" "<<-(xim*yw);
    cvmSet(&AMat, j, 10, 0.0-(xim * zw));file<<" "<<-(xim*zw);
    cvmSet(&AMat, j, 11, 0.0-xim);file<<" "<<-xim<<" "<<endl;

    j++;
    //file<<"--- point :-----  "<<j<<endl;
    cvmSet(&AMat, j, 0, 0.0);file<<" "<<0.0;
    cvmSet(&AMat, j, 1, 0.0);file<<" "<<0.0;
    cvmSet(&AMat, j, 2, 0.0);file<<" "<<0.0;
    cvmSet(&AMat, j, 3, 0.0);file<<" "<<0.0;
    cvmSet(&AMat, j, 4, xw);file<<" "<<xw;
    cvmSet(&AMat, j, 5, yw);file<<" "<<yw;
    cvmSet(&AMat, j, 6, zw);file<<" "<<zw;
    cvmSet(&AMat, j, 7, 1.0);file<<" "<<1.0;
    cvmSet(&AMat, j, 8, 0.0-(yim * xw));file<<" "<<-(yim*xw);
    cvmSet(&AMat, j, 9, 0.0-(yim * yw));file<<" "<<-(yim*yw);
    cvmSet(&AMat, j, 10,0.0 -(yim * zw));file<<" "<<-(yim*zw);
    cvmSet(&AMat, j, 11, 0.0-yim);file<<" "<<-yim<<endl;
  }
  file.close();
  for (i = 0; i < 3; i++)
    for (j = 0; j < 4; j++)
      cvmSet(&projMat, i, j, 0.0);

}


void CalibCam::projMatCalibration()
{
  int i, j, k;

  int rows = numImages * numPoints[0] * 2;
  int cols = 12;
  CvMat U, W, V;

  U = cvMat(rows, rows, CV_64FC1);
  W = cvMat(rows, 12, CV_64FC1);
  V = cvMat(12, 12, CV_64FC1);
  cvmAlloc(&U);
  cvmAlloc(&W);
  cvmAlloc(&V);

  
  cvSVD(&AMat, &W, &U, &V, 0);

  cvmSet(&projMat, 0, 0, cvmGet(&V, 0, 9));
  cvmSet(&projMat, 0, 1, cvmGet(&V, 1, 9));
  cvmSet(&projMat, 0, 2, cvmGet(&V, 2, 9));
  cvmSet(&projMat, 0, 3, cvmGet(&V, 3, 9));
  cvmSet(&projMat, 1, 0, cvmGet(&V, 4, 9));
  cvmSet(&projMat, 1, 1, cvmGet(&V, 5, 9));
  cvmSet(&projMat, 1, 2, cvmGet(&V, 6, 9));
  cvmSet(&projMat, 1, 3, cvmGet(&V, 7, 9));
  cvmSet(&projMat, 2, 0, cvmGet(&V, 8, 9));
  cvmSet(&projMat, 2, 1, cvmGet(&V, 9, 9));
  cvmSet(&projMat, 2, 2, cvmGet(&V, 10, 9));
  cvmSet(&projMat, 2, 3, cvmGet(&V, 11, 9));
  
  double gamma = 0.0;
  for (i = 0; i < 3; i++)
    gamma += cvmGet(&projMat, 2, i) * cvmGet(&projMat, 2, i);
  gamma = sqrt(gamma);
  for (i = 0; i < 3; i++) {
    cout<<" projM["<<i<<"]= ";
    for (j = 0; j < 4; j++) {
      cvmSet(&projMat, i, j, cvmGet(&projMat, i, j) / gamma);
      //xxx
      cout<<" "<<cvmGet(&projMat, i, j);
    }
    cout<<endl;
  }
  
  double ox = 0.0;
  ox += cvmGet(&projMat, 0, 0) * cvmGet(&projMat, 2, 0);
  ox += cvmGet(&projMat, 0, 1) * cvmGet(&projMat, 2, 1);
  ox += cvmGet(&projMat, 0, 2) * cvmGet(&projMat, 2, 2);
  cout <<" ox = "<<ox<<endl;
  double oy = 0.0;
  oy += cvmGet(&projMat, 1, 0) * cvmGet(&projMat, 2, 0);
  oy += cvmGet(&projMat, 1, 1) * cvmGet(&projMat, 2, 1);
  oy += cvmGet(&projMat, 1, 2) * cvmGet(&projMat, 2, 2);
  cout <<" oy = "<<oy<<endl;
  
  double fx = 0.0;
  fx += cvmGet(&projMat, 0, 0) * cvmGet(&projMat, 0, 0);
  fx += cvmGet(&projMat, 0, 1) * cvmGet(&projMat, 0, 1);
  fx += cvmGet(&projMat, 0, 2) * cvmGet(&projMat, 0, 2);
  fx -= ox * ox; fx = sqrt(fx);
  cout<<" fx = "<<fx<<endl;
  double fy = 0.0;
  fy += cvmGet(&projMat, 1, 0) * cvmGet(&projMat, 1, 0);
  fy += cvmGet(&projMat, 1, 1) * cvmGet(&projMat, 1, 1);
  fy += cvmGet(&projMat, 1, 2) * cvmGet(&projMat, 1, 2);
  fy -= oy * oy; fy = sqrt(fy);
  cout<<" fy = "<<fy<<endl;  
  
  cout<<" gamma=" <<gamma<<endl;
  
  for (i = 0; i < 12; i++) {
    cout<<"V["<<i<<"]= "<<endl;
    for (j = 0; j < 12; j++)
      cout<<" "<<cvmGet(&V, i, j);
    cout<<endl;
  }
  cout<<endl<<endl;
  for (i = 0; i < 12; i++) {
    cout<<"W["<<i<<"]= "<<endl;
    for (j = 0; j < 12; j++)
      cout<<" "<<cvmGet(&W, i, j);
    cout<<endl;
  }
}