affine.c

FERET人脸库的处理代码。内函预处理

/*----------------------------------------------------------------------
PROGRAM: affine.c
DATE:    3/9/94
AUTHOR:  Baback Moghaddam, [e-mail]
------------------------------------------------------------------------

  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;
}