/*----------------------------------------------------------------------PROGRAM: affine.cDATE:    3/9/94AUTHOR:  Baback Moghaddam, baback@media.mit.edu------------------------------------------------------------------------  Routines for computing 2D affine transformations and image affine  warping  NOTE: uses routines svdcmp.c & gaussj.c from Numerical Recipes---------------------------------------------------------------------- */#include <stdio.h>#include <stdlib.h>#include <string.h>#include <math.h>#include <float.h>#include "util.h"#include "io.h"#include "matrix.h"#include "affine.h"/*----------------------------------------------------------------------*/void pivoted_affine(float **P, float **Q, int N, float **A)     /* given a set of correspondences as 2-by-N matrices	P[1..2][1..N] and Q[1..2][1..N], this routine computes	the (least-squares) affine transformation given by 	the (homogeneous) affine matrix A[1..3][1..3]	This computation is pivoted by the 1st correspondance	ie., it is always lined up correctly	NOTE:  N must be >=3     */{  register int i,j;  float **Prt, **Pr, **Qr;  float **V, **Vt, **Si, *s;  float **temp1, **temp2, sum;  Pr = matrix(1, 2, 1, N-1);  Prt = matrix(1, N-1, 1, 2);   Qr = matrix(1, 2, 1, N-1);  V  = matrix(1, 2, 1, 2);  Vt = matrix(1, 2, 1, 2);  Si = matrix(1, 2, 1, 2);  s  = vector(1, 2);    /* subtract first point's coods from the rest */  for (i=1; i<=2; i++)     for (j=1; j<N; j++) {    Pr[i][j] = P[i][j+1] - P[i][1];    Qr[i][j] = Q[i][j+1] - Q[i][1];  }  /* form Pr transpose */  for (i=1; i<N; i++)    for (j=1; j<=2; j++)      Prt[i][j] = Pr[j][i];  /* compute svd of Prt */  svdcmp(Prt, N-1, 2, s, V);  /* construct inverse of S */  for (i=1; i<=2; i++)    for (j=1; j<=2; j++)      Si[i][j] = 0.0;  for (i=1; i<=2; i++)    if (s[i]>FLT_MIN)      Si[i][i] = 1.0/s[i];  /* transpose V */  for (i=1; i<=2; i++)    for (j=1; j<=2; j++)      Vt[i][j] = V[j][i];  /* compute A = Qr*U*inv(S)*Vt and T*/      temp1 = matrix(1, 2, 1, 2);  temp2 = matrix(1, 2, 1, 2);    matrix_multiply(Si, Vt, 2, 2, 2, temp1);  matrix_multiply(Qr, Prt, 2, N-1, 2, temp2);  matrix_multiply(temp2, temp1, 2, 2, 2, A);    free_matrix(temp1, 1, 2, 1, 2);  free_matrix(temp2, 1, 2, 1, 2);    /* now compute the translation vector */    for (i=1; i<=2; i++) {    for (j=1, sum=0.0; j<=2; j++)      sum += -A[i][j]*P[j][1];    A[i][3] = sum + Q[i][1];  }    A[3][1] = A[3][2] = 0.0;   /* clean up the homogeneous part */  A[3][3] = 1.0;             /* for more stable inverses      */    free_matrix(Pr,  1, 2, 1, N-1);  free_matrix(Prt, 1, N-1, 1, 2);   free_matrix(Qr,  1, 2, 1, N-1);  free_matrix(V,   1, 2, 1, 2);  free_matrix(Vt,  1, 2, 1, 2);  free_matrix(Si,  1, 2, 1, 2);  free_vector(s,   1, 2);}/*----------------------------------------------------------------------*/void affine(float **P, float **Q, int N, float **A)     /* given a set of correspondences as 2-by-N matrices	P[1..2][1..N] and Q[1..2][1..N], this routine computes	the (least-squares) affine transformation given by 	the (homogeneous) matrix A[1..3][1..3]	NOTE:  N must be >=3     */{  register int i,j;  float **myPt, **myQ, **V, **Vt, **Si, *s;  float **temp1, **temp2, sum;  myPt = matrix(1, N, 1, 3);   myQ  = matrix(1, 3, 1, N);  V    = matrix(1, 3, 1, 3);  Vt   = matrix(1, 3, 1, 3);  Si   = matrix(1, 3, 1, 3);  s    = vector(1, 3);  temp1 = matrix(1, 3, 1, 3);  temp2 = matrix(1, 3, 1, 3);  /* form myPt and myQ matrices */  for (i=1; i<=2; i++)     for (j=1; j<=N; j++) {      myPt[j][i] = P[i][j];      myPt[j][3] = 1.0;      myQ[i][j]  = Q[i][j];      myQ[3][j]  = 1.0;    }  /* compute svd of myPt (note U is returned in myPt) */  svdcmp(myPt, N, 3, s, V);  /* construct inverse of S */  for (i=1; i<=3; i++)    for (j=1; j<=3; j++)      Si[i][j] = 0.0;  for (i=1; i<=3; i++)    if (s[i]>FLT_MIN)      Si[i][i] = 1.0/s[i];  /* transpose V */  for (i=1; i<=3; i++)    for (j=1; j<=3; j++)      Vt[i][j] = V[j][i];  /* compute A = Q*U*inv(S)*Vt */  matrix_multiply(Si, Vt, 3, 3, 3, temp1);  matrix_multiply(myQ, myPt, 3, N, 3, temp2);  matrix_multiply(temp2, temp1, 3, 3, 3, A);    A[3][1] = A[3][2]= 0.0;    /* clean up the homogeneous part */  A[3][3] = 1.0;             /* for more stable inverses      */      free_matrix(temp1, 1, 3, 1, 3);  free_matrix(temp2, 1, 3, 1, 3);  free_matrix(myPt, 1, N, 1, 3);   free_matrix(myQ,  1, 3, 1, N);  free_matrix(V,   1, 3, 1, 3);  free_matrix(Vt,  1, 3, 1, 3);  free_matrix(Si,  1, 3, 1, 3);  free_vector(s,   1, 3);}/*----------------------------------------------------------------------*/void rigid(float **P, float **Q, int N, float **A, int c1, int c2)     /* given a set of correspondences as 2-by-N matrices	P[1..2][1..N] and Q[1..2][1..N], this routine computes	the rigid transformation (rotation/scale/translation)	based on the correspondances in columns c1 and c2. 	The transform is given by the (homogeneous) matrix	A[1..3][1..3]	NOTE:  N must be >=2     */{  register int i,j;  float dp[3], dq[3], T[3], SR[3][3];  float scale, angle;  /* --- compute the difference vector --- */  for (i=1; i<=2; i++) {    dp[i] = P[i][c2] - P[i][c1];    dq[i] = Q[i][c2] - Q[i][c1];  }  /* --- compute the scale change --- */  scale = sqrt(SQR(dq[1])+SQR(dq[2])) / sqrt(SQR(dp[1])+SQR(dp[2]));  /* --- compute the angle shift --- */  angle = atan2(dq[2],dq[1]) - atan2(dp[2],dp[1]);  /* --- form the scale/rotation matrix --- */  SR[1][1] =   scale * cos(angle);  SR[1][2] = - scale * sin(angle);  SR[2][1] = - SR[1][2];  SR[2][2] =   SR[1][1];  /* --- compute the translation vector --- */  for (i=1; i<=2; i++)     T[i] = Q[i][c1] - SR[i][1]*P[1][c1] - SR[i][2]*P[2][c1];     /* --- assign the composite rigid transform matrix A --- */  for (i=1; i<=2; i++)     for (j=1; j<=2; j++)       A[i][j] = SR[i][j];  A[1][3] = T[1];  A[2][3] = T[2];  A[3][1] = A[3][2]= 0.0;    /* clean up the homogeneous part */  A[3][3] = 1.0;             /* for more stable inverses      */}/*----------------------------------------------------------------------*/int affine_warp(float **image_in, float **image_out, 		int M, int N, float **A, float bgvalue)          /*  Will apply the affine warp specified by A[1..3][1..3]	 to the input image image_in and return the result in the 	 pre-allocated matrix image_out. Both images are of size	 M-by-N.	 	 */{  register int i,j,k;  float **Ainv;  float x, y, f, a, b;  float f1,f2,f3,f4;  int r,c;  float scale;  float **image_temp1, **image_temp2;  float lowpass_filter[] = {0, 0.2, 0.6, 0.2};  int lowpass_ntaps = 3;  int num_gp_levels;  /* first compute inverse warp */  Ainv = matrix(1, 3, 1, 3);  matrix_inverse(A, 3, Ainv);  image_temp1 = matrix(1, M, 1, N);  /* before warping compute determinant and apply a     Gaussian Pyramid filter n times if necessary          */  scale = A[1][1]*A[2][2] - A[2][1]*A[1][2];  scale = sqrt(ABS(scale));  num_gp_levels = (int) (-log(scale)/log(2.0) + 0.5);  if (num_gp_levels>0) {    image_temp2 = matrix(1, M, 1, N);    for (i=1; i<=M; i++)      for (j=1; j<=N; j++)	image_temp1[i][j] = image_in[i][j];    for (k=1; k<=num_gp_levels; k++) {      conv2d_sep(image_temp1, image_temp2, M, N,		 lowpass_filter, lowpass_ntaps,		 lowpass_filter, lowpass_ntaps);      for (i=1; i<=M; i++)	for (j=1; j<=N; j++)	  image_temp1[i][j] = image_temp2[i][j];    }    for (i=1; i<=M; i++)      for (j=1; j<=N; j++)	image_temp1[i][j] = image_temp2[i][j];    free_matrix(image_temp2, 1, M, 1, N);  }  else {   for (i=1; i<=M; i++)      for (j=1; j<=N; j++)	image_temp1[i][j] = image_in[i][j];  }     /* apply warp by inverse-warping from the result */    for (i=1; i<=M; i++)     for (j=1; j<=N; j++) {            image_out[i][j] = bgvalue;         x = Ainv[1][1]*i + Ainv[1][2]*j + Ainv[1][3];      y = Ainv[2][1]*i + Ainv[2][2]*j + Ainv[2][3];            if (x>=1 & x<M & y>=1 & y<N) {		/* do bilinear interpolation */		r = (int) x;	c = (int) y;	a = x - r;	b = y - c;	f1 = image_temp1[r][c];	f2 = image_temp1[r][c+1];	f3 = image_temp1[r+1][c];	f4 = image_temp1[r+1][c+1];		f = (1-a)*(1-b)*f1 + b*(1-a)*f2 + a*(1-b)*f3 + a*b*f4;		image_out[i][j] = f;      }          }    free_matrix(Ainv, 1, 3, 1, 3);  free_matrix(image_temp1, 1, M, 1, N);  return num_gp_levels;}