/* posecalib.c - exterior orientation calibration.  Translation + rotation.
 *  Aassumed that initial estimate of camera position is available (by
 *  using GPS for example).  Several points around the camera are surveyed 
 *  using GPS, and the pan tilt values that bring those points into the
 *  center of the image are also measured.  
 *  For a given translation, the optimal rotation is computed using
 *  Horn's closed form solution (see rotation.c).  The optimal translation
 *  is then searched for using an iterative method
 *
 * Note on coordinate systems:
 *  LVCS:  X points east, Y points North, Z points Up
 *  PTU: X points left, Y points Up, Z points into scene
 *
 * Modifications:
 * Wed Aug 25 23:10:36 EDT 1999 - Bob Collins
 *    removed iterative solution for translation using Powell optimization
 *    from numerical recipes in C, and substituted with my own iterative
 *    solution using gradient descent.  My approach gives nearly the same
 *    results (with .01 meters on test cases), is much faster, relies on
 *    less external code (from NR in C), and converges in more situations
 *    than the old code using Powell's method.  A big win all around. -BobC
 *
 */


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "nr.h"
#include "nrutil.h"

#ifndef M_PI
#define M_PI        3.14159265358979323846
#endif

/* converting degrees to radians and vice versa */
#define DEG2RAD(DEG) ((double)(DEG)*M_PI/180.0)
#define RAD2DEG(RAD) ((double)(RAD)*180.0/M_PI)


#define MAX_LANDMARKS 100
#define MAXCHARSPERLINE 128

double xland[MAX_LANDMARKS], yland[MAX_LANDMARKS], zland[MAX_LANDMARKS];

int read_landmarks(char *filename)
{ 
   int i=0;
   char line[MAXCHARSPERLINE+1];
   double lat,lon,elv,height,xloc,yloc,zloc,dx,dy,dz;
   int num,epoch;
   FILE *file; 
   printf("Reading landmarks from %s\n",filename);
   file = fopen(filename,"r");
   while (fgets(line,MAXCHARSPERLINE,file) != NULL) 
      if (*line != ';') {
        if (i >= MAX_LANDMARKS) { 
	  fprintf(stderr,"Error, too many landmarks, stopping at %d\n",i); 
          return(i); 
        }
        else {
          sscanf(line,"%d %lf %lf %lf %lf %d %lf %lf %lf %lf %lf %lf",
		 &num,&lat,&lon,&elv,&height,&epoch,
		 &xloc,&yloc,&zloc,&dx, &dy, &dz);
	  xland[i] = xloc; 
	  yland[i] = yloc; 
	  zland[i] = zloc;
	  i++;	  
        }
      }
   fclose(file);
   return(i);
}


int read_measurement_data(char *filename, 
			  double *x, double *y, double *z,
			  double *pan, double *tilt)
{ 
   int i=0;
   char line[MAXCHARSPERLINE+1];
   double tpan, ttilt;
   int num;
   FILE *file; 
   printf("Reading pan-tilt measurements from %s\n",filename);
   file = fopen(filename,"r");
   while (fgets(line,MAXCHARSPERLINE,file) != NULL) 
      if (*line != ';') {
        if (i >= MAX_LANDMARKS) { 
	  fprintf(stderr,"Error, too many measurements, stopping at %d\n",i); 
          return(i); 
        }
        else {
          sscanf(line,"%d %lf %lf", &num,&tpan,&ttilt);
	  num--;
	  x[i] = xland[num]; 
	  y[i] = yland[num]; 
	  z[i] = zland[num]; 
	  pan[i] = tpan; 
	  tilt[i] = ttilt;
	  i++;	  
        }
      }
   fclose(file);
   return(i);
}




void uvec(double x, double y, double z, double *ux, double *uy, double *uz)
{
  double tmp;
  tmp = sqrt(x*x + y*y + z*z);
  *ux = x / tmp;
  *uy = y / tmp;
  *uz = z / tmp;
  return;
}


void pan_tilt_to_unit_vector(double pan, double tilt, 
			     double *x, double *y, double *z)
{
  double rpan, rtilt, tmp;
  rpan = DEG2RAD(pan);
  rtilt = DEG2RAD(tilt);
  tmp = cos(rtilt);
  *x = sin(rpan) * tmp;
  *y = sin(rtilt);
  *z = cos(rpan) * tmp;
  return;
}

void unit_vector_to_pan_tilt(double x, double y, double z,
			     double *pan, double *tilt)
{
  double rpan, rtilt, costilt;
  rpan = atan2(x,z);
  /* even though sqrt looses the sign, atan computes tilt between 
    -90 and 90, where cos is nonnegative, so we are OK */
  rtilt = atan(y / sqrt(x*x + z*z));
  *pan = RAD2DEG(rpan);
  *tilt = RAD2DEG(rtilt);
  return;
}


/* Solve a set of linear equations using LU decomposition */
void solve_linear(double *mat, double *vec, int n, double *solution)
{
  float **Amat, *bvec, d;
  int *indx, i, j, k;
  
  /*
    printf("C Matrix:\n");
    printf("   %6.8f  %6.8f  %6.8f\n",mat[0],mat[1],mat[2]);
    printf("   %6.8f  %6.8f  %6.8f\n",mat[3],mat[4],mat[5]);
    printf("   %6.8f  %6.8f  %6.8f\n",mat[6],mat[7],mat[8]);
    printf("B Vector:\n");
    printf("   %6.3f  %6.3f  %6.3f\n",vec[0],vec[1],vec[2]);
    */

  Amat = matrix(1,n,1,n);
  bvec = vector(1,n);
  indx = ivector(1,n);
  for (i=1, k=0; i <= n; i++) {
    for (j=1; j <= n; j++)
      Amat[i][j] = (float)mat[k++];
  }
  
  for (i=1; i <= n; i++) {
    bvec[i] = (float)vec[i-1];
  }
  ludcmp(Amat, n, indx, &d);
  lubksb(Amat, n, indx, bvec);
  for (i=1; i <= n; i++) {
		      solution[i-1] = (double)bvec[i];
  }
  
  return;
}


/***********************************************************************
 * entering the HORN zone
 ***********************************************************************/
#define Sxx sumvec[0]
#define Sxy sumvec[1]
#define Sxz sumvec[2]
#define Syx sumvec[3]
#define Syy sumvec[4]
#define Syz sumvec[5]
#define Szx sumvec[6]
#define Szy sumvec[7]
#define Szz sumvec[8]

/* matrix of sums of products, ala Horn */
void sum_unit_products(double *sumvec, double x1, double y1, double z1,
		       double x2, double y2, double z2)
{
  Sxx += x1 * x2;
  Sxy += x1 * y2;
  Sxz += x1 * z2;
  Syx += y1 * x2;
  Syy += y1 * y2;
  Syz += y1 * z2;
  Szx += z1 * x2;
  Szy += z1 * y2;
  Szz += z1 * z2;
  return;
}

/* find best rotation as the max eigenvector of a 4x4 matrix N, from Horn. */
/* returns max eigenvalue - which is the error metric that was minimized */
double best_rotation(double *best_quaternion, double *sumvec)
{
  float **Nmatrix, **eigmatrix, *eigenvec, maxeig;
  int nrot, maxind, i;
  Nmatrix = matrix(1,4,1,4);
  eigmatrix = matrix(1,4,1,4);
  eigenvec = vector(1,4);
  Nmatrix[1][1] = (float)(Sxx + Syy + Szz);
  Nmatrix[1][2] = (float)(Syz - Szy);
  Nmatrix[1][3] = (float)(Szx - Sxz);
  Nmatrix[1][4] = (float)(Sxy - Syx);
  Nmatrix[2][1] = (float)(Syz - Szy);
  Nmatrix[2][2] = (float)(Sxx - Syy - Szz);
  Nmatrix[2][3] = (float)(Sxy + Syx);
  Nmatrix[2][4] = (float)(Szx + Sxz);
  Nmatrix[3][1] = (float)(Szx - Sxz);
  Nmatrix[3][2] = (float)(Sxy + Syx);
  Nmatrix[3][3] = (float)(Syy - Sxx - Szz);
  Nmatrix[3][4] = (float)(Syz + Szy);
  Nmatrix[4][1] = (float)(Sxy - Syx);
  Nmatrix[4][2] = (float)(Szx + Sxz);
  Nmatrix[4][3] = (float)(Syz + Szy);
  Nmatrix[4][4] = (float)(Szz - Sxx - Syy);
  /* eigendecomposition of symmetric matrix using Jacobi method */
  jacobi(Nmatrix, 4, eigenvec, eigmatrix, &nrot);
  /* find largest eigenvalue */
  maxeig = eigenvec[1];
  maxind = 1;
  for (i=2; i <= 4; i++) {
    if (eigenvec[i] > maxeig) {
      maxeig = eigenvec[i];
      maxind = i;
    }
  }
  /* best quaternion rotation is corresponding eigenvector */
  best_quaternion[0] = (double)eigmatrix[1][maxind];
  best_quaternion[1] = (double)eigmatrix[2][maxind];
  best_quaternion[2] = (double)eigmatrix[3][maxind];
  best_quaternion[3] = (double)eigmatrix[4][maxind];
 
  free_matrix(Nmatrix,1,4,1,4);
  free_matrix(eigmatrix,1,4,1,4);
  free_vector(eigenvec,1,4);
  return(maxeig);
}


#define q0 q[0]
#define qx q[1]
#define qy q[2]
#define qz q[3]
/* convert a quaternion into the 9 elements of a rotation matrix */
void quaternion_to_rotvec (double *q, double *rot)
{
  rot[0] = q0*q0 + qx*qx - qy*qy - qz*qz;
  rot[1] = 2*(qx*qy - q0*qz);
  rot[2] = 2*(qx*qz + q0*qy);
  rot[3] = 2*(qx*qy + q0*qz);
  rot[4] = q0*q0 - qx*qx + qy*qy - qz*qz;
  rot[5] = 2*(qy*qz - q0*qx);
  rot[6] = 2*(qx*qz - q0*qy);
  rot[7] = 2*(qy*qz + q0*qx);
  rot[8] = q0*q0 - qx*qx - qy*qy + qz*qz;
  return;
}


/***********************************************************************
 * leaving the HORN zone
 ***********************************************************************/

void rotate_point(double *mat, double *x, double *y, double *z)
{
  double tmp[3];
  tmp[0] = mat[0] * (*x) + mat[1] * (*y) + mat[2] * (*z);
  tmp[1] = mat[3] * (*x) + mat[4] * (*y) + mat[5] * (*z);
  tmp[2] = mat[6] * (*x) + mat[7] * (*y) + mat[8] * (*z);
  *x = tmp[0]; *y = tmp[1]; *z = tmp[2];
  return;
}

void transpose_rotmat(double *rotmat)
{
  double tmp;
  tmp = rotmat[1]; rotmat[1] = rotmat[3]; rotmat[3] = tmp;
  tmp = rotmat[2]; rotmat[2] = rotmat[6]; rotmat[6] = tmp;
  tmp = rotmat[5]; rotmat[5] = rotmat[7]; rotmat[7] = tmp;
  return;
}

/* assumes two unit vectors */
double angerr(double x1, double y1, double z1, double x2, double y2, double z2)
{
  double ang;
  ang = RAD2DEG(acos(x1 * x2 + y1 * y2 + z1 * z2));
  return(ang);
}

double compute_best_rotation(double xcam, double ycam, double zcam,
			     double *x, double *y, double *z,
			     double *pan, double *tilt, int n,
			     double *bestrot)
{
  int i;
  double sumvec[9], err;
  double cx, cy, cz, wx, wy, wz, pan2, tilt2;

  for (i=0; i < 9; i++) 
    sumvec[i] = 0.0;
  for (i=0; i < n; i++) {
    /* lvcs unit vector pointing towards landmark */
    uvec(x[i] - xcam, y[i] - ycam, z[i] - zcam, &wx, &wy, &wz);
    /* corresponding unit vector in camera coords */
    pan_tilt_to_unit_vector(pan[i], tilt[i], &cx, &cy, &cz);
    /* accumulate products used by Horn method */
    sum_unit_products(sumvec,cx,cy,cz,wx,wy,wz);
  }
  err = best_rotation(bestrot,sumvec);
  return(err);
}

/***********************************************************************
 * Iterative solution for optimal translation
 ***********************************************************************/
#define TRANSLATION_FTOL 1.0e-4
#define MAXITER 100

void iterative_optimize_trans(double xcam, double ycam, double zcam,
			      double *x, double *y, double *z,
			      double *pan, double *tilt, int n,
			      double *besttrans)
{


  int i,j, iter;
  double err, dist;
  double ux, uy, uz, cx, cy, cz, oldx, oldy, oldz;
  double Mi[9], mat[9], vec[3], point[3], bestrot[9], quat[4], sumweight;

  besttrans[0] = xcam;   
  besttrans[1] = ycam;   
  besttrans[2] = zcam;

  /*
  printf("initial trans: %6.3f %6.3f %6.3f\n",
	 besttrans[0],besttrans[1],besttrans[2]);
  */
  for (iter=0; iter < 100; iter++) {
    oldx = besttrans[0]; 
    oldy = besttrans[1]; 
    oldz = besttrans[2]; 
    err = compute_best_rotation(besttrans[0], besttrans[1], besttrans[2],
				x, y, z, pan, tilt, n, quat);
    quaternion_to_rotvec(quat,bestrot);
    
    for (j=0; j < 9; j++) mat[j] = 0.0; 
    for (j=0; j < 3; j++) vec[j] = 0.0;
    sumweight = 0;
    
    for (i=0; i < n; i++) {
      cx = x[i] - besttrans[0]; 
      cy = y[i] - besttrans[1]; 
      cz = z[i] - besttrans[2];
      dist = sqrt(cx*cx + cy*cy + cz*cz);
      if (dist < 1.0) dist = 1.0;

      pan_tilt_to_unit_vector(pan[i], tilt[i], &ux, &uy, &uz);
      rotate_point(bestrot,&ux,&uy,&uz);
    
      sumweight += 1.0 / (dist * dist);

      vec[0] += (x[i]/dist - ux) / dist;
      vec[1] += (y[i]/dist - uy) / dist;
      vec[2] += (z[i]/dist - uz) / dist;
    }
    
    besttrans[0] = vec[0] / sumweight;
    besttrans[1] = vec[1] / sumweight;
    besttrans[2] = vec[2] / sumweight;
    
    /*
    printf("next trans: %6.3f %6.3f %6.3f\n",
	   besttrans[0],besttrans[1],besttrans[2]);
    */
    if ((fabs(besttrans[0] - oldx) < TRANSLATION_FTOL) &&
	(fabs(besttrans[1] - oldy) < TRANSLATION_FTOL) &&
	(fabs(besttrans[2] - oldz) < TRANSLATION_FTOL)) {
      printf("num trans iterations %d\n",iter+1);
      return;
    }	
  }
  return;
}


/***********************************************************************
 * OPTIMIZE_POSE
 ***********************************************************************/

double optimize_pose(int transflag,
		     double xcam, double ycam, double zcam,
		     double *x, double *y, double *z,
		     double *pan, double *tilt, int n,
		     double *bestrot, double *besttrans)
{
  double err;
  double xx,yy,zz;
  if (transflag) {
    iterative_optimize_trans(xcam,ycam,zcam,x,y,z,pan,tilt,n,besttrans);
  }
  else {
    besttrans[0] = xcam;
    besttrans[1] = ycam;
    besttrans[2] = zcam;
  }
  err = compute_best_rotation(besttrans[0], besttrans[1], besttrans[2], 
			      x, y, z, pan, tilt, n, bestrot);
  return(err);
}


/*----------------------------------------------------------------------*/

int main(int argc, char* argv[])
{
   int count,i, transflag;
   double x[MAX_LANDMARKS], y[MAX_LANDMARKS], z[MAX_LANDMARKS];
   double pan[MAX_LANDMARKS], tilt[MAX_LANDMARKS];
   double xcam, ycam, zcam, err, sumangerr;
   double besttrans[3], bestrot[4];
   double rotvec[9], vec1[3], vec2[3], tmp;
   double cx, cy, cz, wx, wy, wz, pan2, tilt2;

   if (argc < 4) {
     printf("Usage: posecalib landmarksfile pantiltfile transflag\n");
     printf("Solves for best camera pose based on landmark sightings\n");
     printf("  landmarksfile is a file of GPS landmarks\n");
     printf("  pantiltfile contains measured landmark pan/tilts\n");
     printf("  transflag = 0 to solve for rotation only\n");
     printf("              1 to solve both rotation and translation\n");
     exit(-1);
   }

   count = read_landmarks(argv[1]);
   printf("read %d landmarks\n",count);
   /*
   for (i=0; i < count; i++)
     printf("%d: %.3lf %.3lf %.3lf\n",i+1,xland[i],yland[i],zland[i]);
   */

   count = read_measurement_data(argv[2], x, y, z, pan, tilt);
   printf("Read location estimate + %d pan,tilt measurements\n",count-1);
   xcam = x[0];
   ycam = y[0];
   zcam = z[0];
   printf("init location is: %.3f %.3f %.3f\n",xcam,ycam,zcam);

   for (i=1; i < count; i++)
     printf("  %2d: %.3lf %.3lf %.3lf (%.3lf %.3lf)\n",
	    i,x[i],y[i],z[i],pan[i],tilt[i]);

   /* 0 =  rotation only, 1 =  rot + translation */
   sscanf(argv[3],"%d",&transflag);  

   err = optimize_pose(transflag, xcam,ycam,zcam,
		       x+1,y+1,z+1,pan+1,tilt+1,count-1,
		       bestrot, besttrans);
   xcam = besttrans[0];    
   ycam = besttrans[1];    
   zcam = besttrans[2]; 
   printf("pose optimized: minimum error score = %g\n",err);
   printf("best translation: %.3f %.3f %.3f\n",
	  besttrans[0], besttrans[1], besttrans[2]);
   printf("best unit quaternion rotation: %.3f %.3f %.3f %.3f\n",
	  bestrot[0], bestrot[1], bestrot[2], bestrot[3]);
   quaternion_to_rotvec(bestrot,rotvec);
   printf("Rotation matrix:\n");
   printf("   %6.3f  %6.3f  %6.3f\n", rotvec[0], rotvec[1], rotvec[2]);
   printf("   %6.3f  %6.3f  %6.3f\n", rotvec[3], rotvec[4], rotvec[5]);
   printf("   %6.3f  %6.3f  %6.3f\n", rotvec[6], rotvec[7], rotvec[8]);
   printf("Checking rotation\n");
   printf("         LVCS COORDS                   CAMERA COORDS        ANGERR\n");
   sumangerr = 0.0;
   for (i=1; i < count; i++) {
     /* landmark unit vector in lvcs coords */
     uvec(x[i] - xcam, y[i] - ycam, z[i] - zcam, &wx, &wy, &wz);
     /* corresponding unit vector in camera coords */
     pan_tilt_to_unit_vector(pan[i], tilt[i], &cx, &cy, &cz);
     rotate_point(rotvec,&cx,&cy,&cz);
     err = angerr(wx,wy,wz,cx,cy,cz);
     sumangerr += err;
     printf("    %6.3f  %6.3f  %6.3f        %6.3f %6.3f %6.3f      %6.3f\n",
	    wx, wy, wz, cx, cy, cz, err);
   }
   printf("average angular error = %.3f degrees\n",sumangerr/(count-1));
   printf("Computing predicted landmark pan-tilts\n");
   transpose_rotmat(rotvec);
   printf("            COMPUTED                    MEASURED\n");
   for (i=1; i < count; i++) {
     /* landmark unit vector in lvcs coords */
     uvec(x[i] - xcam, y[i] - ycam, z[i] - zcam, &wx, &wy, &wz);
     /* rotate into camera coords */
     rotate_point(rotvec,&wx,&wy,&wz);
     unit_vector_to_pan_tilt(wx, wy, wz, &pan2, &tilt2);
     printf("      %8.3f  %8.3f           %8.3f %8.3f\n",
	    pan2, tilt2, pan[i], tilt[i]);
   }
   transpose_rotmat(rotvec);

   printf("-----------------------------------------------------------------\n");
   printf("Recap of External Calibration\n");
   if (transflag) {
     printf("Solved for both location and rotation\n");
     printf("Initial location estimate\n");
     printf("  %.3f %.3f %.3f\n",x[0], y[0], z[0]);
   }
   else
     printf("Solved for rotation only\n");
   printf("Best location of camera\n");
   printf("  %.3f %.3f %.3f\n",xcam, ycam, zcam);
   printf("Best rotation as a unit quaternion\n");
   printf("  %.8f %.8f %.8f %.8f\n",
	  bestrot[0], bestrot[1], bestrot[2], bestrot[3]);
   printf("Rotation as a matrix:\n");
   printf("   %12.8f  %12.8f  %12.8f\n", rotvec[0], rotvec[1], rotvec[2]);
   printf("   %12.8f  %12.8f  %12.8f\n", rotvec[3], rotvec[4], rotvec[5]);
   printf("   %12.8f  %12.8f  %12.8f\n", rotvec[6], rotvec[7], rotvec[8]);

   return 0;
}
