sustaining_gazes/matlab_version/pdm_generation/nrsfm-em/computeH.c
2016-04-28 15:40:36 -04:00

710 lines
17 KiB
C

/* Code written by Lorenzo Torresani and Shoumen Saha */
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "mat.h"
#include "mex.h"
#define IN
#define OUT
#define MYDEBUG 0
static mxArray * McomputeH(int nargout_,
const mxArray * Uc,
const mxArray * Vc,
const mxArray * E_z,
const mxArray * E_zz,
const mxArray * RR);
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (nlhs > 1) {
mexErrMsgTxt(
"Run-time Error: File: computeh Line: 1 Column:"
" 1 The function \"computeh\" was called with"
" more than the declared number of outputs (1).");
}
if (nrhs > 5) {
mexErrMsgTxt(
"Run-time Error: File: computeh Line: 1 Column:"
" 1 The function \"computeh\" was called with"
" more than the declared number of inputs (5).");
}
plhs[0] = McomputeH(nlhs, prhs[0], prhs[1], prhs[2], prhs[3], prhs[4]);
}
double ** getMxArray(IN const mxArray *mxArr,
OUT int *rows,
OUT int *cols)
{
int i, j, numDim;
double *arrDataPt, **matrix;
if((numDim = mxGetNumberOfDimensions(mxArr)) != 2)
{
printf("Input matrix has %d dimensions, not 2!\n", numDim);
return NULL;
}
*rows = mxGetM(mxArr);
*cols = mxGetN(mxArr);
if(((matrix = (double **) malloc(sizeof(double *)*(*rows))) == NULL) ||
((matrix[0] = (double *) malloc(sizeof(double)*(*rows)*(*cols))) == NULL))
return NULL;
for(i=1; i<(*rows); i++)
matrix[i] = matrix[0] + i*(*cols);
arrDataPt = mxGetPr(mxArr);
for(j=0; j<(*cols); j++)
for(i=0; i<(*rows); i++)
matrix[i][j] = *(arrDataPt ++);
return matrix;
}
mxArray * putMxArray(double *img, int rows, int cols/*, char *nameImg*/)
{
int i, j;
double *ptr;
mxArray *mxArr;
mxArr = mxCreateDoubleMatrix(rows, cols, mxREAL);
ptr = mxGetPr(mxArr);
for(j=0; j<cols; j++)
for(i=0; i<rows; i++)
*(ptr ++) = img[i*cols + j];
/*mxSetPr(mxArr, data);*/
/*mxSetName(mxArr, nameImg);*/
return mxArr;
}
double ** createMatrix(IN int rows,
IN int cols)
{
int i;
double **matrix;
if(((matrix = (double **) malloc(sizeof(double *)*rows)) == NULL) ||
((matrix[0] = (double *) malloc(sizeof(double)*rows*cols)) == NULL))
return NULL;
for(i=1; i<rows; i++)
matrix[i] = matrix[0] + i*cols;
memset(matrix[0], 0, sizeof(double)*rows*cols);
return matrix;
}
int ** createIntMatrix(IN int rows,
IN int cols)
{
int i;
int **matrix;
if(((matrix = (int **) malloc(sizeof(int *)*rows)) == NULL) ||
((matrix[0] = (int *) malloc(sizeof(int)*rows*cols)) == NULL))
return NULL;
for(i=1; i<rows; i++)
matrix[i] = matrix[0] + i*cols;
memset(matrix[0], 0, sizeof(int)*rows*cols);
return matrix;
}
void freeMatrix(IN double **matrix)
{
free(*matrix);
free(matrix);
}
void freeIntMatrix(IN int **matrix)
{
free(*matrix);
free(matrix);
}
void IO_MLabCreateFile(char *fileName)
{
MATFile *fp;
fp = matOpen(fileName, "w");
matClose(fp);
}
void IO_MLabWriteDoubleImg(char *fileName, char *nameImg, double *img, int rows, int cols)
{
MATFile *fp;
int i, j, dims[2];
double *ptr;
mxArray *mxArr;
dims[0] = rows;
dims[1] = cols;
fp = matOpen(fileName, "u");
mxArr = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
ptr = mxGetPr(mxArr);
for(j=0; j<cols; j++)
for(i=0; i<rows; i++)
*(ptr ++) = img[i*cols + j];
/*mxSetName(mxArr, nameImg);
matPutArray(fp, mxArr);*/
matPutVariable(fp, nameImg, mxArr);
matClose(fp);
mxDestroyArray(mxArr);
}
void kron(IN int rowsA,
IN int colsA,
IN double **A,
IN int rowsB,
IN int colsB,
IN double **B,
IN int rowsC,
IN int colsC,
OUT double **C)
{
int i, j, k, l;
if ((rowsC != (rowsA*rowsB)) || (colsC != (colsA*colsB)))
{
printf("kron: incompatible matrix size\n");
return;
}
for(i=0; i<rowsA; i++)
for(k=0; k<rowsB; k++)
for(j=0; j<colsA; j++)
for(l=0; l<colsB; l++)
C[rowsB*i+k][colsB*j+l] = A[i][j]*B[k][l];
}
double conjgradient(IN int n,
IN double **A,
IN double **b,
IN int i_max, /* max # iterations */
IN double epsilon, /* error tolerance */
IN OUT double **x)
{
//my conjugate gradient solver for .5*x'*A*x -b'*x, based on the
// tutorial by Jonathan Shewchuk
//FILE * fd;
int i=0, numrecompute, row, col;
double Ax, delta_old, delta_new, delta_0, **r, **d, **q, alpha, alpha_denom, beta;
r = createMatrix(1, n);
d = createMatrix(1, n);
q = createMatrix(1, n);
/* r = b - A*x */
/* delta_new = r'*r */
delta_new = 0;
for (row=0; row<n; row++)
{
Ax = 0;
for (col=0; col<n; col++)
Ax += A[row][col]*x[0][col];
d[0][row] = r[0][row] = b[0][row] - Ax;
delta_new += r[0][row]*r[0][row];
}
delta_0 = delta_new;
numrecompute = (int) sqrt((double) n);
// printf("numrecompute = %d\n",numrecompute);
//fd = fopen("err.txt", "w");
while ((i < i_max) && (delta_new > epsilon*epsilon*delta_0))
{
//printf("Step %d, delta_new = %f \r",i,delta_new);
//fprintf(fd, "%d %f\n", i, (float)delta_new);
/* q = A*d */
alpha_denom = 0;
for (row=0; row<n; row++)
{
q[0][row] = 0;
for (col=0; col<n; col++)
q[0][row] += A[row][col]*d[0][col];
alpha_denom += d[0][row]*q[0][row];
}
alpha = delta_new / alpha_denom;
/* x += d*alpha */
for (row=0; row<n; row++)
x[0][row] += alpha*d[0][row];
if (i % numrecompute == 0)
{
/* r = b - A*x */
for (row=0; row<n; row++)
{
Ax = 0;
for (col=0; col<n; col++)
Ax += A[row][col]*x[0][col];
r[0][row] = b[0][row] - Ax;
}
}
else
{
/* r = r-q*alpha; */
for (row=0; row<n; row++)
r[0][row] -= alpha*q[0][row];
}
delta_old = delta_new;
/* delta_new = r'*r */
delta_new = 0;
for (row=0; row<n; row++)
delta_new += r[0][row]*r[0][row];
beta = delta_new/delta_old;
/* d = r + beta*d */
for (row=0; row<n; row++)
d[0][row] = r[0][row] + beta*d[0][row];
i++;
}
//fclose(fd);
freeMatrix(r);
freeMatrix(d);
freeMatrix(q);
return delta_new;
}
double conjgradient2(IN int n,
IN double **A,
IN double **b,
IN int i_max, /* max # iterations */
IN double epsilon, /* error tolerance */
IN int k,
IN OUT double **x)
{
//my conjugate gradient solver for .5*x'*A*x -b'*x, based on the
// tutorial by Jonathan Shewchuk (or is it +b'*x?)
//FILE * fd;
int i=0, numrecompute, row, sparsity_offset, j, ind, col_offset;
double Ax, delta_old, delta_new, delta_0, **r, **d, **q, alpha, alpha_denom, beta;
r = createMatrix(1, n);
d = createMatrix(1, n);
q = createMatrix(1, n);
sparsity_offset = n / (k+1);
/* r = b - A*x */
/* delta_new = r'*r */
delta_new = 0;
for (row=0; row<n; row++)
{
Ax = 0;
col_offset = ((row%sparsity_offset)/3) * 3;
for (j=0; j<(k+1); j++)
{
ind = col_offset + j*sparsity_offset;
Ax += A[row][ind]*x[0][ind] + A[row][ind+1]*x[0][ind+1] + A[row][ind+2]*x[0][ind+2];
}
d[0][row] = r[0][row] = b[0][row] - Ax;
delta_new += r[0][row]*r[0][row];
}
delta_0 = delta_new;
numrecompute = (int) sqrt((double) n);
// printf("numrecompute = %d\n",numrecompute);
//fd = fopen("err.txt", "w");
while ((i < i_max) && (delta_new > epsilon*epsilon*delta_0))
{
//printf("Step %d, delta_new = %f \r",i,delta_new);
//fprintf(fd, "%d %f\n", i, (float)delta_new);
/* q = A*d */
alpha_denom = 0;
for (row=0; row<n; row++)
{
q[0][row] = 0;
col_offset = ((row%sparsity_offset)/3) * 3;
for (j=0; j<(k+1); j++)
{
ind = col_offset + j*sparsity_offset;
q[0][row] += A[row][ind]*d[0][ind] + A[row][ind+1]*d[0][ind+1] + A[row][ind+2]*d[0][ind+2];
}
alpha_denom += d[0][row]*q[0][row];
}
alpha = delta_new / alpha_denom;
/* x += d*alpha */
for (row=0; row<n; row++)
x[0][row] += alpha*d[0][row];
if (i % numrecompute == 0)
{
/* r = b - A*x */
for (row=0; row<n; row++)
{
Ax = 0;
col_offset = ((row%sparsity_offset)/3) * 3;
for (j=0; j<(k+1); j++)
{
ind = col_offset + j*sparsity_offset;
Ax += A[row][ind]*x[0][ind] + A[row][ind+1]*x[0][ind+1] + A[row][ind+2]*x[0][ind+2];
}
r[0][row] = b[0][row] - Ax;
}
}
else
{
/* r = r-q*alpha; */
for (row=0; row<n; row++)
r[0][row] -= alpha*q[0][row];
}
delta_old = delta_new;
/* delta_new = r'*r */
delta_new = 0;
for (row=0; row<n; row++)
delta_new += r[0][row]*r[0][row];
beta = delta_new/delta_old;
/* d = r + beta*d */
for (row=0; row<n; row++)
d[0][row] = r[0][row] + beta*d[0][row];
i++;
}
//fclose(fd);
freeMatrix(r);
freeMatrix(d);
freeMatrix(q);
return delta_new;
}
static mxArray * McomputeH(int nargout_,
const mxArray * Uc,
const mxArray * Vc,
const mxArray * E_z,
const mxArray * E_zz,
const mxArray * RR) {
double **Ucc, **Vcc, **E_zc, **E_zzc, **RRc, **KK1c, **KK2c, **kk3c, **P_tc, **R_tc, **zz_hat_tc, **Tmp1c, **Tmp2c, **vecH_hatc=NULL,
delta_err, epsilon;
int tc, Fc, Nc, junk1, junk2, kc, ic, jc, ii, jj, kk, ll, i_max, **sparsityMap;
mxArray * vecH_hat = NULL;
mxArray * KK1 = NULL;
mxArray * R_t = NULL;
mxArray * P_t = NULL;
mxArray * t = NULL;
mxArray * KK2 = NULL;
mxArray * F = NULL;
mxArray * N = NULL;
mxArray * k = NULL;
#if MYDEBUG
#define DEBUGFILENAME ".\\debug.mat"
IO_MLabCreateFile(DEBUGFILENAME);
#endif
Ucc = getMxArray(Uc, &Fc, &Nc);
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "Ucc", Ucc[0], Fc, Nc);
#endif
Vcc = getMxArray(Vc, &junk1, &junk2);
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "Vcc", Vcc[0], Fc, Nc);
#endif
E_zc = getMxArray(E_z, &kc, &junk1);
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "E_zc", E_zc[0], kc, Fc);
#endif
E_zzc = getMxArray(E_zz, &junk1, &junk2);
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "E_zzc", E_zzc[0], kc*Fc, kc);
#endif
RRc = getMxArray(RR, &junk1, &junk2);
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "RRc", RRc[0], 2*Fc, 3);
#endif
/*
* KK2 = zeros(3*N*(k+1), 3*N*(k+1));
*/
KK2c = createMatrix(3*Nc*(kc+1), 3*Nc*(kc+1));
/*
* KK3 = zeros(3*N, k+1);
*/
kk3c = createMatrix(1,3*Nc*(kc+1));
P_tc = createMatrix(1, 2*Nc);
zz_hat_tc = createMatrix(kc+1, kc+1);
R_tc = createMatrix(2, 3);
KK1c = createMatrix(2*Nc, 3*Nc);
Tmp1c = createMatrix(3*Nc, 3*Nc);
Tmp2c = createMatrix(1, 3*Nc);
for(tc=0; tc<Fc; tc++)
{
/*
* P_t = [Uc(t,:); Vc(t,:)];
*/
memset(P_tc[0], 0, sizeof(double)*2*Nc);
for(jc=0; jc<Nc; jc++)
{
P_tc[0][2*jc] = Ucc[tc][jc];
P_tc[0][2*jc+1] = Vcc[tc][jc];
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "P_tc", P_tc[0], 1, 2*Nc);
#endif
/*
* zz_hat_t = [1 E_z(:,t)'; E_z(:,t) E_zz((t-1)*k+1:t*k,:)];
*/
memset(zz_hat_tc[0], 0, sizeof(double)*(kc+1)*(kc+1));
zz_hat_tc[0][0] = 1;
for(jc=1; jc<kc+1; jc++)
{
zz_hat_tc[jc][0] = E_zc[jc-1][tc];
zz_hat_tc[0][jc] = E_zc[jc-1][tc];
}
for(ic=1; ic<kc+1; ic++)
{
for(jc=1; jc<kc+1; jc++)
{
zz_hat_tc[ic][jc] = E_zzc[tc*kc + (ic-1)][jc-1];
}
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "zz_hat_tc", zz_hat_tc[0], kc+1, kc+1);
#endif
/*
* R_t = [RR(t, :); RR(t+F, :)];
*/
for(jc=0; jc<3; jc++)
{
R_tc[0][jc] = RRc[tc][jc];
R_tc[1][jc] = RRc[tc+Fc][jc];
}
/*
* KK1 = kron(speye(N), R_t);
*/
memset(KK1c[0], 0, sizeof(double)*2*Nc*3*Nc);
for(ic=0; ic<Nc; ic++)
{
KK1c[ic*2][ic*3] = R_tc[0][0];
KK1c[ic*2][ic*3+1] = R_tc[0][1];
KK1c[ic*2][ic*3+2] = R_tc[0][2];
KK1c[ic*2+1][ic*3] = R_tc[1][0];
KK1c[ic*2+1][ic*3+1] = R_tc[1][1];
KK1c[ic*2+1][ic*3+2] = R_tc[1][2];
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "KK1c", KK1c[0], 2*Nc, 3*Nc);
#endif
/* computes KK1'*KK1 */
memset(Tmp1c[0], 0, sizeof(double)*3*Nc*3*Nc);
Tmp1c[0][0] = KK1c[0][0]*KK1c[0][0] + KK1c[1][0]*KK1c[1][0];
Tmp1c[0][1] = KK1c[0][0]*KK1c[0][1] + KK1c[1][0]*KK1c[1][1];
Tmp1c[0][2] = KK1c[0][0]*KK1c[0][2] + KK1c[1][0]*KK1c[1][2];
Tmp1c[1][0] = KK1c[0][1]*KK1c[0][0] + KK1c[1][1]*KK1c[1][0];
Tmp1c[1][1] = KK1c[0][1]*KK1c[0][1] + KK1c[1][1]*KK1c[1][1];
Tmp1c[1][2] = KK1c[0][1]*KK1c[0][2] + KK1c[1][1]*KK1c[1][2];
Tmp1c[2][0] = KK1c[0][2]*KK1c[0][0] + KK1c[1][2]*KK1c[1][0];
Tmp1c[2][1] = KK1c[0][2]*KK1c[0][1] + KK1c[1][2]*KK1c[1][1];
Tmp1c[2][2] = KK1c[0][2]*KK1c[0][2] + KK1c[1][2]*KK1c[1][2];
for(ic=1; ic<Nc; ic++)
{
Tmp1c[ic*3][ic*3] = Tmp1c[0][0];
Tmp1c[ic*3][ic*3+1] = Tmp1c[0][1];
Tmp1c[ic*3][ic*3+2] = Tmp1c[0][2];
Tmp1c[ic*3+1][ic*3] = Tmp1c[1][0];
Tmp1c[ic*3+1][ic*3+1] = Tmp1c[1][1];
Tmp1c[ic*3+1][ic*3+2] = Tmp1c[1][2];
Tmp1c[ic*3+2][ic*3] = Tmp1c[2][0];
Tmp1c[ic*3+2][ic*3+1] = Tmp1c[2][1];
Tmp1c[ic*3+2][ic*3+2] = Tmp1c[2][2];
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "Tmp1c", Tmp1c[0], 3*Nc, 3*Nc);
#endif
/*
* KK2 = KK2 + kron(zz_hat_t', KK1'*KK1);
*/
/*for(ii=0; ii<(kc+1); ii++)
for(kk=0; kk<(3*Nc); kk++)
for(jj=0; jj<(kc+1); jj++)
for(ll=0; ll<(3*Nc); ll++)
KK2c[(3*Nc)*ii+kk][(3*Nc)*jj+ll] += zz_hat_tc[ii][jj]*Tmp1c[kk][ll];*/
for(ii=0; ii<(kc+1); ii++)
for(kk=0; kk<Nc; kk++)
for(jj=0; jj<(kc+1); jj++)
{
KK2c[(3*Nc)*ii+kk*3][(3*Nc)*jj+kk*3] += zz_hat_tc[ii][jj]*Tmp1c[kk*3][kk*3];
KK2c[(3*Nc)*ii+kk*3][(3*Nc)*jj+kk*3+1] += zz_hat_tc[ii][jj]*Tmp1c[kk*3][kk*3+1];
KK2c[(3*Nc)*ii+kk*3][(3*Nc)*jj+kk*3+2] += zz_hat_tc[ii][jj]*Tmp1c[kk*3][kk*3+2];
KK2c[(3*Nc)*ii+kk*3+1][(3*Nc)*jj+kk*3] += zz_hat_tc[ii][jj]*Tmp1c[kk*3+1][kk*3];
KK2c[(3*Nc)*ii+kk*3+1][(3*Nc)*jj+kk*3+1] += zz_hat_tc[ii][jj]*Tmp1c[kk*3+1][kk*3+1];
KK2c[(3*Nc)*ii+kk*3+1][(3*Nc)*jj+kk*3+2] += zz_hat_tc[ii][jj]*Tmp1c[kk*3+1][kk*3+2];
KK2c[(3*Nc)*ii+kk*3+2][(3*Nc)*jj+kk*3] += zz_hat_tc[ii][jj]*Tmp1c[kk*3+2][kk*3];
KK2c[(3*Nc)*ii+kk*3+2][(3*Nc)*jj+kk*3+1] += zz_hat_tc[ii][jj]*Tmp1c[kk*3+2][kk*3+1];
KK2c[(3*Nc)*ii+kk*3+2][(3*Nc)*jj+kk*3+2] += zz_hat_tc[ii][jj]*Tmp1c[kk*3+2][kk*3+2];
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "KK2c", KK2c[0], 3*Nc*(kc+1), 3*Nc*(kc+1));
#endif
/*
* KK3 = KK3 + KK1'* P_t(:) * [1 E_z(:,t)'];
*/
/* computes KK1'*P_t(:) */
memset(Tmp2c[0], 0, sizeof(double)*3*Nc);
for(ic=0; ic<Nc; ic++)
{
Tmp2c[0][3*ic] = KK1c[2*ic][3*ic]*P_tc[0][2*ic] + KK1c[2*ic+1][3*ic]*P_tc[0][2*ic+1];
Tmp2c[0][3*ic+1] = KK1c[2*ic][3*ic+1]*P_tc[0][2*ic] + KK1c[2*ic+1][3*ic+1]*P_tc[0][2*ic+1];
Tmp2c[0][3*ic+2] = KK1c[2*ic][3*ic+2]*P_tc[0][2*ic] + KK1c[2*ic+1][3*ic+2]*P_tc[0][2*ic+1];
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "Tmp2c", Tmp2c[0], 1, 3*Nc);
#endif
for(ic=0; ic<3*Nc; ic++)
{
/*KK3c[ic][0] = KK3c[ic][0] + Tmp2c[0][ic];*/
kk3c[0][ic] = kk3c[0][ic] + Tmp2c[0][ic];
for(jc=1; jc<(kc+1); jc++)
/*KK3c[ic][jc] = KK3c[ic][jc] + Tmp2c[0][ic]*E_zc[jc-1][tc];*/
kk3c[0][ic + jc*(3*Nc)] = kk3c[0][ic + jc*(3*Nc)] + Tmp2c[0][ic]*E_zc[jc-1][tc];
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "kk3c", kk3c[0], 1, 3*Nc*(kc+1));
#endif
//printf("debug\n");
}
freeMatrix(Ucc);
freeMatrix(Vcc);
freeMatrix(E_zc);
freeMatrix(E_zzc);
freeMatrix(RRc);
freeMatrix(KK1c);
freeMatrix(P_tc);
freeMatrix(R_tc);
freeMatrix(zz_hat_tc);
freeMatrix(Tmp1c);
freeMatrix(Tmp2c);
vecH_hatc = createMatrix(1, 3*Nc*(kc+1));
for(ic=0; ic<3*Nc; ic++)
vecH_hatc[0][ic] = (double) rand() / (double) RAND_MAX;
epsilon = 0.000000001;
i_max = 1000;
//sparsityMap = createIntMatrix(3*Nc*(kc+1), 3*Nc*(kc+1)+1);
//getSparsityMap(3*Nc*(kc+1), 3*Nc*(kc+1), KK2c, sparsityMap);
//delta_err = conjgradient(3*Nc*(kc+1), KK2c, kk3c, i_max, epsilon, vecH_hatc);
delta_err = conjgradient2(3*Nc*(kc+1), KK2c, kk3c, i_max, epsilon, kc, vecH_hatc);
if (delta_err>0.01)
printf("Large CG error\n");
vecH_hat = mxCreateDoubleMatrix(3*Nc*(kc+1), 1, mxREAL);
memcpy(mxGetPr(vecH_hat), vecH_hatc[0], sizeof(double)*3*Nc*(kc+1));
freeMatrix(KK2c);
freeMatrix(kk3c);
freeMatrix(vecH_hatc);
return vecH_hat;
}