bobby/open_dbm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

open source pkg v1

Browse Source
This commit is contained in:
Vijay Yadev
2020-08-04 19:12:31 -04:00
parent bef213dba9
commit c389fc2c47
3708 changed files with 1624220 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/apprRot.m Normal file
View File

@@ -0,0 +1,26 @@
function R = apprRot(Ra)
%R = apprRot(Ra)
i1 = 0.5; i2 = 0.5;
U = Ra(1,:);
V = Ra(2,:);
un = norm(U);
vn = norm(V);
Un = U/un;
Vn = V/vn;
vp = Un*Vn';
up = Vn*Un';
Vc = Vn-vp*Un; Vc = Vc/norm(Vc);
Uc = Un-up*Vn; Uc = Uc/norm(Uc);
Ua = i1*Un+i2*Uc; Ua = Ua/norm(Ua);
Va = i1*Vn+i2*Vc; Va = Va/norm(Va);
R = [Ua;Va;cross(Ua,Va)];
if det(R)<0, R(3,:) = -R(3,:); end;

710
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/computeH.c Normal file
View File

@@ -0,0 +1,710 @@
/* 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;
}

27
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/computeH.m Normal file
View File

@@ -0,0 +1,27 @@
function vecH_hat = computeH(Xc, Yc, E_z, E_zz, RR)
%vecH_hat = computeH(Xc, Yc, E_z, E_zz, RR)
fprintf('You are running the Matlab version of function ''computeH''. This program will run very slowly.... \nI recommend that you try to compile the CMEX code on your platform using command ''mex computeH.c'' (''mex computeH.c -l matlb'' under Unix)\n\n');
K = size(E_z,1);
J = size(Xc,2);
T = size(Xc,1);
KK2 = zeros(3*J*(K+1), 3*J*(K+1));
KK3 = zeros(3*J, K+1);
for t=1:T
P_t = [Xc(t,:); Yc(t,:)];
zz_hat_t = [1 E_z(:,t)'; E_z(:,t) E_zz((t-1)*K+1:t*K,:)];
R_t = [RR(t, :); RR(t+T, :)];
KK1 = kron(speye(J), R_t);
KK2 = KK2 + kron(zz_hat_t', KK1'*KK1);
KK3 = KK3 + KK1'* P_t(:) * [1 E_z(:,t)'];
end
vecH_hat = pinv(KK2) * KK3(:);

BIN
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/computeH.mexw64 Normal file
View File

Binary file not shown.

31
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/compute_log_lik.m Normal file
View File

@@ -0,0 +1,31 @@
function loglik = compute_log_lik(P, S_bar, V, E_z, E_zz, RO, Tr, sigma_sq)
[K, T] = size(E_z);
J = size(S_bar, 2);
M_t = zeros(2*J, K);
loglik = - 0.5*T * (2*J*log(sigma_sq));
for t = 1:T,
R_t = RO{t};
Sdef = S_bar;
for kk = 1:K,
Sdef = Sdef + E_z(kk,t)*V((kk-1)*3+[1:3],:);
M_t(1:J, kk) = (R_t(1,:)*V((kk-1)*3+[1:3],:))';
M_t(J+1:end, kk) = (R_t(2,:)*V((kk-1)*3+[1:3], :))';
end;
invSigmaSq_p = eye(2*J)./sigma_sq;
f_bar_t = R_t(1:2,:)*S_bar;
f_bar_t = [f_bar_t(1,:) f_bar_t(2,:)]';
f_t = [P(t, :) P(t+T, :)]';
t_vect_t = [Tr(t,1)*ones(J,1); Tr(t,2)*ones(J,1)];
covZ_t = E_zz((t-1)*K+1:t*K,:) - E_z(:,t)*E_z(:,t)';
loglik = loglik - 0.5*(((f_t-f_bar_t-t_vect_t)./sigma_sq)'*(f_t-f_bar_t-t_vect_t)) + (((f_t-f_bar_t-t_vect_t)'./sigma_sq)*M_t*E_z(:,t)) ...
- 0.5*trace(((M_t./sigma_sq)'*M_t) * E_zz((t-1)*K+1:t*K,:)) - 0.5*log(det(covZ_t));
end

161
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/em_sfm.m Normal file
View File

@@ -0,0 +1,161 @@
function [P3, S_bar, V, RO, Tr, Z, sigma_sq, phi, Q, mu0, sigma0] = em_sfm(P, MD, K, use_lds, tol, max_em_iter)
% Non-Rigid Structure From Motion with Gaussian/LDS Deformation Model
% Copyright (c) by Lorenzo Torresani, Stanford University
%
% Based on the following paper:
%
% Lorenzo Torresani, Aaron Hertzmann and Christoph Bregler,
% Learning Non-Rigid 3D Shape from 2D Motion, NIPS 16, 2003
% http://cs.stanford.edu/~ltorresa/projects/learning-nr-shape/
%
% Please refer to this publication if you use this program for
% research or for technical applications.
%
%
% INPUT:
%
% P - (2*T) x J tracking matrix: P([t t+T],:) contains the 2D projections of the J points at time t
% MD - T x J missing data binary matrix: MD(t, j)=1 if no valid data is available for point j at time t, 0 otherwise
% K - number of deformation basis
% use_lds - set to 1 to model deformations using a linear dynamical system; set to 0 otherwise
% tol - termination tolerance (proportional change in likelihood)
% max_em_iter - maximum number of EM iterations
%
%
% OUTPUT:
%
% P3 - (3*T) x J 3D-motion matrix: ( P3([t t+T t+2*T],:) contains the 3D coordinates of the J points at time t )
% S_bar - shape average: 3 x J matrix
% V - deformation shapes: (3*K) x J matrix ( V((n-1)*3+[1:3],:) contains the n-th deformation basis )
% RO - rotation: cell array ( RO{t} gives the rotation matrix at time t )
% Tr - translation: T x 2 matrix
% Z - deformation weights: T x K matrix
% sigma_sq - variance of the noise in feature position
% phi - LDS transition matrix
% Q - LDS state noise matrix
% mu0 - initial state mean
% sigma0 - initial state variance
if mod(size(P,1), 1) ~= 0,
fprintf('Error: size(P) must be (2*T)xJ\n');
return;
end
if (size(P,1)/2 ~= size(MD,1)) | (size(P,2) ~= size(MD,2))
fprintf('Error: Size incompatibility between P and MD\n');
return;
end
if mod(K, 1) ~= 0,
fprintf('Error: K must be an integer value\n');
return;
end
[T, J] = size(MD);
r = 3*(K + 1); % motion rank
P_hat = P; % if any of the points are missing, P_hat will be updated during the M-step
% uses rank 3 factorization to get a first initialization for rotation and S_bar
[R_init, Trvect, S_bar] = rigidfac(P_hat, MD);
Tr(:,1) = Trvect(1:T);
Tr(:,2) = Trvect(T+1:2*T);
R = zeros(2*T, 3);
% enforces rotation constraints
for t = 1:T,
Ru = R_init(t,:);
Rv = R_init(T+t,:);
Rz = cross(Ru,Rv); if det([Ru;Rv;Rz])<0, Rz = -Rz; end;
RO_approx = apprRot([Ru;Rv;Rz]);
RO{t} = RO_approx;
R(t,:) = RO_approx(1,:);
R(t+T,:) = RO_approx(2,:);
end;
% given the initial estimates of rotation, translation and shape average, it initializes
% deformation shapes and weights through LSQ minimization of the reprojection error
[V, Z] = init_SB(P_hat, Tr, R, S_bar, K);
% initializes sigma_sq
E_zz_init = cov(Z);
E_zz_init = repmat(E_zz_init, T, 1);
sigma_sq = mstep_update_noisevar(P_hat, S_bar, V, Z', E_zz_init, RO, Tr);
if use_lds,
[phi, mu0, sigma0, Q] = init_lds(P_hat, S_bar, V, R, Tr, sigma_sq);
else
phi = [];
mu0 = [];
sigma0 = [];
Q = [];
end
loglik = 0;
annealing_const = 60;
max_anneal_iter = round(max_em_iter/2);
for em_iter=1:max_em_iter,
if use_lds,
[E_z, E_zz, y, M, xt_n, Pt_n, Ptt1_n, xt_t1, Pt_t1] = estep_lds_compute_Z_distr(P_hat, S_bar, V, R, Tr, phi, mu0, sigma0, Q, sigma_sq);
[phi, Q, sigma_sq, mu0, sigma0] = mstep_lds_update(y, M, xt_n, Pt_n, Ptt1_n);
else
% computes the hidden variables distributions
[E_z, E_zz] = estep_compute_Z_distr(P_hat, S_bar, V, R, Tr, sigma_sq); % (Eq 17-18)
end
Z = E_z';
% updates shape basis
[S_bar, V] = mstep_update_shapebasis(P_hat, E_z, E_zz, R, Tr, S_bar, V); % (Eq 21)
% fills in missing points
if sum(MD(:))>0,
P_hat = mstep_update_missingdata(P_hat, MD, S_bar, V, E_z, RO, Tr); % (Eq 25)
end
% updates rotation
[RO, R] = mstep_update_rotation(P_hat, S_bar, V, E_z, E_zz, RO, Tr); % (Eq 24)
% updates translation
Tr = mstep_update_transl(P_hat, S_bar, V, E_z, RO); % (Eq 23)
if ~use_lds,
% updates noise variance
sigma_sq = mstep_update_noisevar(P_hat, S_bar, V, E_z, E_zz, RO, Tr); % (Eq 22)
if em_iter < max_anneal_iter,
sigma_sq = sigma_sq * (1 + annealing_const*(1 - em_iter/max_anneal_iter));
end
oldloglik = loglik;
% computes log likelihood
loglik = compute_log_lik(P_hat, S_bar, V, E_z, E_zz, RO, Tr, sigma_sq);
fprintf('LogLik(%d): %f\n', em_iter, loglik);
if (em_iter <= 2),
loglikbase = loglik;
elseif (loglik < oldloglik)
fprintf('Violation');
% keyboard;
elseif 0 & ((loglik-loglikbase)<(1 + tol)*(oldloglik-loglikbase)),
fprintf('\n');
break;
end
else
fprintf('Iteration %d/%d\n', em_iter, max_em_iter);
end
end
P3 = zeros(3*T, J);
for t = 1:T,
z_t = Z(t,:);
Rf = [R(t,:); R(t+T,:)];
S = S_bar;
for kk = 1:K,
S = S+z_t(kk)*V((kk-1)*3+[1:3],:);
end;
S = RO{t}*S;
P3([t t+T t+2*T], :) = S + [Tr(t, [1 2]) -mean(S(3,:))]'*ones(1,J);
end

37
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/estep_compute_Z_distr.m Normal file
View File

@@ -0,0 +1,37 @@
function [E_z, E_zz] = estep_compute_Z_distr(P, S_bar, V, RR, Tr, sigma_sq)
%[E_z, E_zz] = estep_compute_Z_distr(P, S_bar, V, RR, Tr, sigma_sq)
% Computes the distribution over Z given the current parameter estimates (see Eq 17-18)
K = size(V,1)/3;
[T, J] = size(P);
T = T/2;
Pc = P - Tr(:)*ones(1,J);
M_t = zeros(2*J, K);
P_hat_t = zeros(2*J, 1);
E_z = zeros(K, T);
E_zz = zeros(T*K, K);
invSigmaSq_p = eye(2*J)./sigma_sq;
for t=1:T,
R_t = [RR(t,:); RR(t+T,:)];
for kk = 1:K,
M_t(1:J, kk) = (R_t(1,:)*V(1+(kk-1)*3:kk*3, :))';
M_t(J+1:end, kk) = (R_t(2,:)*V(1+(kk-1)*3:kk*3, :))';
end
P_hat_t(1:J) = (R_t(1,:)*S_bar)';
P_hat_t(J+1:end) = (R_t(2,:)*S_bar)';
%beta_t = M_t' * inv(M_t*M_t' + sigma_sq*eye(2*J)); % (Eq 16)
% Can be computed much more efficiently using the matrix inversion lemma:
AA = M_t./sigma_sq;
beta_t = M_t'*(invSigmaSq_p - AA*inv(eye(K) + M_t'*M_t./sigma_sq)*AA'); % (Eq 16)
E_z(:, t) = beta_t*([Pc(t, :) Pc(t+T, :)]' - P_hat_t); % (Eq 17)
E_zz((t-1)*K+1:t*K,:) = eye(K) - beta_t*M_t + E_z(:,t)*E_z(:,t)'; % (Eq 18)
end

46
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/estep_lds_compute_Z_distr.m Normal file
View File

@@ -0,0 +1,46 @@
function [E_z, E_zz, y, M, xt_n, Pt_n, Ptt1_n, xt_t1, Pt_t1] = sfm_lds_Estep(P, S_bar, V, RR, Tr, phi, mu0, sigma0, Q, sigma_sq)
%[E_z, E_zz, y, M, xt_n, Pt_n, Ptt1_n, xt_t1, Pt_t1] = sfm_lds_Estep(P, S_bar, V, RR, Tr, phi, mu0, sigma0, Q, sigma_sq)
K = size(V,1)/3;
[T, J] = size(P);
T = T/2;
Pc = P - [Tr(:,1); Tr(:,2)]*ones(1,J);
M_t = zeros(2*J, K);
P_hat_t = zeros(2*J, 1);
E_z = zeros(K, T);
E_zz = zeros(T*K, K);
invSigmaSq_p = eye(2*J)./sigma_sq;
M = cell(1, T+1);
M{1} = [];
y = zeros(2*J, T+1);
y(:,1) = 0;
for t=1:T,
Pt = [P(t,:) P(t+T,:)]';
Rt = [RR(t,:); RR(t+T,:)];
for kk = 1:K,
M_t(1:J, kk) = (Rt(1,:)*V(1+(kk-1)*3:kk*3, :))';
M_t(J+1:end, kk) = (Rt(2,:)*V(1+(kk-1)*3:kk*3, :))';
end
P_hat_t(1:J) = (Rt(1,:)*S_bar)';
P_hat_t(J+1:end) = (Rt(2,:)*S_bar)';
y(:, t+1) = Pt - P_hat_t;
M{t+1} = M_t;
end
maxIter = 20;
[xt_n, Pt_n, Ptt1_n, xt_t1, Pt_t1] = kalmansmooth(y, M, maxIter, phi, mu0, sigma0, Q, sigma_sq, K, 2*J, T);
E_z = xt_n(:,2:end);
for t=1:T,
E_zz((t-1)*K+1:t*K,:) = Pt_n{t+1} + E_z(:,t)*E_z(:,t)';
end

68
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/findG.m Normal file
View File

@@ -0,0 +1,68 @@
function G = findG(Rhat)
[F,D] = size(Rhat); F = F/2;
% Build matrix Q such that Q * v = [1,...,1,0,...,0] where v is a six
% element vector containg all six distinct elements of the Matrix C
%clear Q
for f = 1:F,
g = f + F;
h = g + F;
Q(f,:) = zt2(Rhat(f,:), Rhat(f,:));
Q(g,:) = zt2(Rhat(g,:), Rhat(g,:));
Q(h,:) = zt2(Rhat(f,:), Rhat(g,:));
end
% Solve for v
rhs = [ones(2*F,1); zeros(F,1)];
v = Q \ rhs;
% C is a symmetric 3x3 matrix such that C = G * transpose(G)
n = 1;
for x = 1:D,
for y = x:D,
C(x,y) = v(n); C(y,x) = v(n);
n = n+1;
end;
end;
e = eig(C);
%disp(e)
if (any(e<= 0)),
[uu,dd,vv] = svd(C);
G = uu*sqrt(dd);
cc = G*G';
err = sum((cc(:)-C(:)).^2);
%if err>0.03,
if 0 & err>30,
G = [];
dbstack; keyboard
end;
else
G = sqrtm(C);
end
%neg = 0;
%if e(1) <= 0, neg = 1; end
%if e(2) <= 0, neg = 1; end
%if e(3) <= 0, neg = 1; end
%if neg == 1, G = [];
%else G = sqrtm(C);
%end
%-------------------------------------------------
function M = zt2(i,j)
D = length(i);
M = [];
for x = 1:D,
for y = x:D,
if x==y, M = [M, i(x)*j(y)];
else, M = [M, i(x)*j(y)+i(y)*j(x)];
end;
end;
end;

33
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/init_SB.m Normal file
View File

@@ -0,0 +1,33 @@
function [V, Z] = init_SB(P, Tr, RR, S_bar, K)
%[V, Z] = init_SB(P, Tr, RR, S_bar, K)
[T, J] = size(P);
T = T/2;
V = zeros(3*K, J);
Z = zeros(T, K);
W_tilda = P - RR*S_bar - [Tr(:,1); Tr(:,2)]*ones(1,J);
for kk=1:K, % iterates over deformations
V_kk = zeros(T, 3*J);
for t=1:T,
try
V_kk_t = pinv(RR([t t+T], :))*W_tilda([t t+T], :);
catch err
fprintf('Wrong at %d\n', t);
end
V_kk(t,:) = V_kk_t(:)';
end
[a,b,c] = svd(V_kk, 0);
sqrtb = sqrt(b(1,1));
Z(:, kk) = a(:,1) * sqrtb;
new_V_kk = sqrtb * c(:,1)';
V((kk-1)*3+1:kk*3, :) = reshape(new_V_kk, 3, J);
for t = 1:T,
W_tilda([t t+T], :) = W_tilda([t t+T], :) - RR([t t+T], :)*(Z(t,kk)*V((kk-1)*3+1:kk*3, :));
end
end

41
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/init_lds.m Normal file
View File

@@ -0,0 +1,41 @@
function [phiIn, mu0In, sigma0In, QIn] = iit_lds(P, S_bar, V, RR, Tr, sigma_sq)
K = size(V,1)/3;
[T, J] = size(P);
T = T/2;
y = zeros(2*J, T);
P_hat_t = zeros(2*J, 1);
M_t = zeros(2*J, K);
for t=1:T,
Pt = [P(t,:) P(t+T,:)]';
Rt = [RR(t,:); RR(t+T,:)];
P_hat_t(1:J,1) = (Rt(1,:)*S_bar)';
P_hat_t(J+1:end,1) = (Rt(2,:)*S_bar)';
for kk = 1:K,
M_t(1:J, kk) = (Rt(1,:)*V(1+(kk-1)*3:kk*3, :))';
M_t(J+1:end, kk) = (Rt(2,:)*V(1+(kk-1)*3:kk*3, :))';
end
M{t} = M_t;
y(:, t) = Pt - P_hat_t;
end
A = eye(2*J)./sigma_sq;
for t=1:T,
temp1 = A*M{t};
temp2 = A - temp1*inv(eye(K)+M{t}'*temp1)*temp1';
temp1b(t,:) = y(:,t)'*temp2*M{t};
end
mu0In = mean(temp1b)';
Q = cov(temp1b);
QIn = Q;
t1 = temp1b(1:T-1,:);
t2 = temp1b(2:T,:);
phiIn = inv(t1'*t1+Q)*t1'*t2;
sigma0In = QIn;

BIN
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/jaws.mat Normal file
View File

Binary file not shown.

69
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/kalmansmooth.m Normal file
View File

@@ -0,0 +1,69 @@
function [xt_n, Pt_n, Ptt1_n, xt_t1, Pt_t1] = kalmansmooth(y, M, maxIter, phi, mu0, sigma0, Q, sigma_sq, p, q, n)
% Adapted from code written by Hrishi Deshpande
% Based on eqn (A3)-(A12) of Appendix A in Shumway, R.H. & Stoffer, D.S. (1982),
% "An approach to time series smoothing and forecasting using the EM algorithm", Journal of Time Series Analysis, 3, 253-264
%- - - - - - - - - - - - - - - -
% Forward steps. Eqns (A3)-(A7).
xt_t1 = zeros(p, n+1); % t = 0, 1, ..., n
Pt_t1 = cell (1, n+1); % t = 0, 1, ..., n
K = cell (1, n+1); % t = 0, 1, ..., n
xt_t = zeros(p, n+1); % t = 0, 1, ..., n
Pt_t = cell (1, n+1); % t = 0, 1, ..., n
t = 0; tIdx = t+1;
xt_t(:,tIdx) = mu0;
Pt_t{tIdx} = sigma0;
for t = 1:n
tIdx = t+1;
xt_t1(:,tIdx) = phi*xt_t(:,tIdx-1); % (A3)
Pt_t1{tIdx} = phi*Pt_t{tIdx-1}*phi' + Q; % (A4)
if 1,
K{tIdx} = Pt_t1{tIdx}*M{tIdx}' * inv(M{tIdx}*Pt_t1{tIdx}*M{tIdx}' + sigma_sq*eye(q)); % (A5)
else
% Using the Matrix Inversion Lemma
% (see http://www-2.cs.cmu.edu/afs/cs.cmu.edu/user/zoubin/www/SALD/week5b.pdf)
invR = eye(q)./sigma_sq; AA = inv(inv(Pt_t1{tIdx}) + M{tIdx}'*invR*M{tIdx}); BB = (invR - invR*M{tIdx}*AA*M{tIdx}'*invR);
K{tIdx} = Pt_t1{tIdx}*M{tIdx}' * BB; % (A5)
end
xt_t(:,tIdx) = xt_t1(:,tIdx) + K{tIdx}*(y(:,tIdx) - M{tIdx}*xt_t1(:,tIdx)); % (A6)
Pt_t{tIdx} = Pt_t1{tIdx} - K{tIdx}*M{tIdx}*Pt_t1{tIdx}; % (A7)
end
%- - - - - - - - - - - - - - - -
% Backward steps. Eqns (A8)-(A10)
Jt = cell (1, n+1); % t = 0, 1, ..., n
xt_n = zeros(p, n+1); % t = 0, 1, ..., n
Pt_n = cell (1, n+1); % t = 0, 1, ..., n
t=n; tIdx = t+1;
xt_n(:,tIdx) = xt_t(:,tIdx); % (A9)
Pt_n{tIdx} = Pt_t{tIdx}; % (A10)
for t=n:-1:1
tIdx = t+1;
Jt{tIdx-1} = Pt_t{tIdx-1}*phi'*inv(Pt_t1{tIdx}); % (A8)
xt_n(:,tIdx-1) = xt_t(:,tIdx-1) + Jt{tIdx-1}*(xt_n(:,tIdx) - phi*xt_t(:,tIdx-1)); % (A9)
Pt_n{tIdx-1} = Pt_t{tIdx-1} + Jt{tIdx-1} * (Pt_n{tIdx} - Pt_t1{tIdx}) * Jt{tIdx-1}'; % (A10)
end
%- - - - - - - - - - - - - - - -
% Backward steps. Eqns (A11)-(A12)
Ptt1_n = cell(1, n+1); % t = 0, 1, ..., n
t = n; tIdx = t+1;
Ptt1_n{tIdx} = (eye(p) - K{tIdx}*M{tIdx}) * phi * Pt_t{tIdx-1}; % (A12)
for t=n:-1:2
tIdx = t+1;
Ptt1_n{tIdx-1} = Pt_t{tIdx-1}*Jt{tIdx-2}' + ...
Jt{tIdx-1}*(Ptt1_n{tIdx} - phi*Pt_t{tIdx-1})*Jt{tIdx-2}'; % (A11)
end

BIN
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/mpie_tor_68_pts.mat Normal file
View File

Binary file not shown.

37
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/mstep_lds_update.m Normal file
View File

@@ -0,0 +1,37 @@
function [phi, Q, sigma_sq, mu0, sigma0] = mstep_lds_update(y, M, xt_n, Pt_n, Ptt1_n)
% Adapted from code written by Hrishi Deshpande
% M-step as described in Shumway, R.H. & Stoffer, D.S. (1982), "An approach to time series smoothing and forecasting using the EM algorithm",
% Journal of Time Series Analysis, 3, 253-264
q = size(y, 1);
n = size(y, 2)-1;
p = size(M{2}, 2);
%- - - - - - - - - -
% Eqns (9)-(11)
A = zeros(p); B = zeros(p); C = zeros(p);
for t=1:n
tIdx = t+1;
A = A + Pt_n{tIdx-1} + xt_n(:,tIdx-1) * xt_n(:,tIdx-1)';
B = B + Ptt1_n{tIdx} + xt_n(:,tIdx) * xt_n(:,tIdx-1)';
C = C + Pt_n{tIdx} + xt_n(:,tIdx) * xt_n(:,tIdx)';
end
%- - - - - - - - - -
% Eqns (12)-(14)
invA = inv(A);
phi = B * invA;
Q = (C - B*invA*B') / n;
R = zeros(q);
for t=1:n
tIdx = t+1;
R = R + (y(:,tIdx)-M{tIdx}*xt_n(:,tIdx)) * (y(:,tIdx)-M{tIdx}*xt_n(:,tIdx))' + ...
M{tIdx}*Pt_n{tIdx}*M{tIdx}';
end
R = R / n;
R = eye(q)*mean(diag(R));
sigma_sq = R(1,1);
mu0 = xt_n(:, 1); % (Eq 22, Ghahramani 1996)
sigma0 = Pt_n{1}; % (Eq 24, Ghahramani 1996)

30
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/mstep_update_missingdata.m Normal file
View File

@@ -0,0 +1,30 @@
function P_hat = mstep_update_missingdata(P, MD, S_bar, V, E_z, RO, Tr)
% P_hat = mstep_update_missingdata(P, MD, S_bar, V, E_z, RO, Tr)
% Fills in missing data using Eq 25
[K, T] = size(E_z);
J = size(S_bar, 2);
ind = find(sum(MD')>0);
P_hat = P;
for kk = 1:length(ind),
t = ind(kk);
missingpoints_t = find(MD(t, :));
for s=1:length(missingpoints_t),
j = missingpoints_t(s);
H_j = [S_bar(:,j) reshape(V(:,j), 3, K)]; % H_j is 3x(K+1)
S_tj = H_j*[1; E_z(:,t)]; % S_tj is 3x1
newf_t = RO{t}(1:2,:) * S_tj + Tr(t,:)';
P_hat([t t+T], j) = newf_t;
end
end

39
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/mstep_update_noisevar.m Normal file
View File

@@ -0,0 +1,39 @@
function sigma_sq = mstep_update_noisevar_md(P, S_bar, V, E_z, E_zz, RO, Tr)
%sigma_sq = mstep_update_noisevar(P, S_bar, V, E_z, E_zz, RO, Tr)
% Updates noise variance (Eq 22)
[K, T] = size(E_z);
J = size(S_bar, 2);
M_t = zeros(2*J, K);
sigma_sq = 0;
for t = 1:T,
R_t = RO{t};
Sdef = S_bar;
for kk = 1:K,
Sdef = Sdef + E_z(kk,t)*V((kk-1)*3+[1:3],:);
M_t(1:J, kk) = (R_t(1,:)*V((kk-1)*3+[1:3],:))';
M_t(J+1:end, kk) = (R_t(2,:)*V((kk-1)*3+[1:3], :))';
end;
f_bar_t = R_t(1:2,:)*S_bar;
f_bar_t = [f_bar_t(1,:) f_bar_t(2,:)]';
f_t = [P(t, :) P(t+T, :)]';
t_vect_t = [Tr(t,1)*ones(J,1); Tr(t,2)*ones(J,1)];
s1 = (f_t - f_bar_t - t_vect_t)'*(f_t - f_bar_t - t_vect_t);
s2 = 2*(f_t - f_bar_t - t_vect_t)'*M_t*E_z(:,t);
s3 = trace(M_t'*M_t*E_zz((t-1)*K+1:t*K,:));
sigma_sq = sigma_sq + (s1 - s2 + s3);
end
sigma_sq = sigma_sq/(2*J*T);

63
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/mstep_update_rotation.m Normal file
View File

@@ -0,0 +1,63 @@
function [newRO, newRR] = mstep_update_rotation(P, S_bar, V, E_z, E_zz, RO, Tr)
%[newRO, newRR] = mstep_update_rotation(P, S_bar, V, E_z, E_zz, RO, Tr)
% Linearizes the expression in Eq 24 using exponential maps and
% solves for an improved rotation
% update step
tw_step = 0.3;
[K, T] = size(E_z);
J = size(S_bar, 2);
Pc = P - Tr(:)*ones(1,J);
newRR = zeros(2*T,3);
newRO = RO;
for iter=1:1,
for t = 1:T,
A = zeros(3);
B = zeros(2,3);
zz_hat_t = [1 E_z(:,t)'; E_z(:,t) E_zz((t-1)*K+1:t*K,:)];
for j=1:J,
H_j = [S_bar(:,j) reshape(V(:,j), 3, K)];
A = A + H_j*zz_hat_t*H_j';
B = B + ([Pc(t,j); Pc(t+T,j)] * [1 E_z(:,t)'] * H_j');
end
oldRO_t = RO{t};
C = oldRO_t*A;
D = B - oldRO_t(1:2,:)*A;
% now we solve the system: [1 0 0; 0 1 0]*twist*C = D
CC = [0 C(3,1) -C(2,1)
-C(3,1) 0 C(1,1)
0 C(3,2) -C(2,2)
-C(3,2) 0 C(1,2)
0 C(3,3) -C(2,3)
-C(3,3) 0 C(1,3)];
DD = D(:);
% twist optimization
twist_vect = tw_step*pinv(CC)*DD;
twh = [0 -twist_vect(3) twist_vect(2)
twist_vect(3) 0 -twist_vect(1)
-twist_vect(2) twist_vect(1) 0 ];
dR = expm(twh);
newRO_t = dR*oldRO_t;
newRO{t} = newRO_t;
newRR(t,:) = newRO_t(1,:);
newRR(t+T,:) = newRO_t(2,:);
end
RO = newRO;
end

23
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/mstep_update_shapebasis.m Normal file
View File

@@ -0,0 +1,23 @@
function [newS_bar, newV] = mstep_update_shapebasis(P, E_z, E_zz, RR, Tr, S_bar, V)
%[newS_bar, newV] = mstep_update_shapebasis(P, E_z, E_zz, RR, Tr, S_bar, V)
% Computes an improved version of S_bar and V (Eq 21)
% E_z is KxT, E_zz is a (F*K)xK matrix
% V is (3*K)xJ and S_bar is 3xJ
K = size(E_z,1);
[T, J] = size(P); T = T/2;
Uc = P(1:T, :) - Tr(:,1)*ones(1,J);
Vc = P(T+1:2*T, :) - Tr(:,2)*ones(1,J);
vecH_hat = computeH(Uc, Vc, E_z, E_zz, RR);
H_hat = reshape(vecH_hat, 3*J, K+1);
newS_bar = reshape(H_hat(:,1), 3, J);
newV = zeros(3*K, J);
for kk=1:K,
newV((kk-1)*3+[1:3],:) = reshape(H_hat(:,kk+1), 3, J);
end

22
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/mstep_update_transl.m Normal file
View File

@@ -0,0 +1,22 @@
function Tr = mstep_update_transl(P, S_bar, V, E_z, RO)
% Tr = mstep_update_transl(P, S_bar, V, E_z, RO)
% Updates translation using Eq 23
[K, T] = size(E_z);
J = size(S_bar, 2);
Tr = zeros(T, 2);
for t = 1:T,
Sdef = S_bar;
for kk = 1:K,
Sdef = Sdef + E_z(kk,t)*V((kk-1)*3+[1:3],:);
end;
R_t = RO{t};
XY = R_t(1:2,:)*Sdef;
t_t = sum(P([t t+T], :) - XY, 2)./J;
Tr(t, :) = t_t';
end

22
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/notes.txt Normal file
View File

@@ -0,0 +1,22 @@
Copyright © 2003 Lorenzo Torresani, Aaron Hertzmann, Chris Bregler
This software was written by
Lorenzo Torresani
Dept of Computer Science
Stanford University
ltorresa@cs.stanford.edu
This software is research code, meant for free non-commercial use
and distribution. I cannot promise that it works and I cannot guarantee
I will maintain it.
If you want to experiment with this code, please refer to this paper:
Lorenzo Torresani, Aaron Hertzmann and Christoph Bregler,
Learning Non-Rigid 3D Shape from 2D Motion, NIPS 16, 2003
http://cs.stanford.edu/~ltorresa/projects/learning-nr-shape/
If you find bugs, please send me an email at ltorresa@cs.stanford.edu.
Please email me if you get cool results or if you use this software for your research!

52
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/random_nr_motion.m Normal file
View File

@@ -0,0 +1,52 @@
function [P3, S_bar, V, Z, RO, Tr] = random_nr_motion(T, J, K, state)
% [P3, S_bar, V, Z, RO, Tr] = random_nr_motion(T, J, K, state)
%
% INPUT:
%
% T - number of frames
% J - number of points
% K - number of deformation basis
%
% OUTPUT:
%
% P3 - (3*T) x J 3D-motion matrix: P3([t t+T t+2*T],:) contains the 3D coordinates of the J points at time t
% S_bar - shape average: 3 x J matrix
% V - deformation shapes: (3*K) x J matrix ( V((n-1)*3+[1:3],:) contains the n-th deformation basis )
% Z - deformation weights: T x K matrix
% RO - rotation: cell array ( RO{t} gives the rotation matrix at time t )
% Tr - translation: T x 2 matrix
rand('state', state);
randn('state', state);
S_bar = rand(3, J);
[q,r] = qr(rand(3*J));
V = zeros(3*K, J);
for kk=1:K,
V(1+(kk-1)*3:3*kk, :) = reshape(q(:,kk), 3, J);
end
Z = randn(T,K);
Tr = randn(T,2);
a = (rand(T,1)-0.5)*2*pi;
b = (rand(T,1)-0.5)*2*pi;
c = (rand(T,1)-0.5)*2*pi;
P3 = zeros(3*T, J);
for t=1:T,
R1 = [1 0 0; 0 cos(a(t)) -sin(a(t)); 0 sin(a(t)) cos(a(t))];
R2 = [cos(b(t)) 0 sin(b(t)); 0 1 0; -sin(b(t)) 0 cos(b(t))];
R3 = [cos(c(t)) -sin(c(t)) 0; sin(c(t)) cos(c(t)) 0; 0 0 1];
RO{t} = R1*R2*R3;
Sdef = S_bar;
for kk = 1:K,
Sdef = Sdef+Z(t,kk)*V((kk-1)*3+[1:3],:);
end;
Sdef = RO{t}*Sdef +[Tr(t,:)'; 0]*ones(1, J);
P3([t t+T t+2*T], :) = Sdef;
end

83
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/randomshapes_demo.m Normal file
View File

@@ -0,0 +1,83 @@
% Random Shapes Demo
%
% Copyright (c) by Lorenzo Torresani, Stanford University
%
% A demo of Non-Rigid Structure From Motion on artificial random
% shapes.
%
%
% The 3D reconstruction technique is based on the following paper:
%
% Lorenzo Torresani, Aaron Hertzmann and Christoph Bregler,
% Learning Non-Rigid 3D Shape from 2D Motion, NIPS 16, 2003
% http://cs.stanford.edu/~ltorresa/projects/learning-nr-shape/
%
%
% Function em_sfm implements the algorithm "EM-Gaussian" and "EM-LDS" described
% in the paper
%
% I recommend that you try to compile the CMEX code for the function computeH:
% type 'mex computeH.c' in the Matlab Command Window ('mex computeH.c -l matlb' under Unix)
%
T = 100; % number of frames
J = 60; % number of points
K = 5; % number of deformation shapes
state = 1000; % random generator state
% generates a random sequence of non-rigid 3D motion according to
% the deformation model described by Eq (2) and (3)
[P3_gt, S_bar_gt, V_gt, Z_gt, RO_gt, Tr_gt] = random_nr_motion(T, J, K, state);
% 2D motion resulting from orthographic projection (Eq (1))
p2_obs = P3_gt(1:2*T, :);
% removes 40% of the data
missingdata_rate = 0.4;
if missingdata_rate>0,
rp = randperm(T*J);
ind_md = rp(1:round(T*J*missingdata_rate));
MD = zeros(T, J);
MD(ind_md) = 1;
[i_md, j_md] = ind2sub([T J], ind_md);
p2_obs(sub2ind([2*T J], [i_md i_md+T], [j_md j_md])) = 0; % set to 0 the values corresponding to missing data
else
MD = zeros(T, J);
end
% runs the non-rigid structure from motion algorithm with different number of deformation shapes
max_em_iter = 50;
use_lds = 0; % doesn't use LDS since the data was generated at random, w/o temporal smoothness
tol = 0.001;
k_values = [K-1:K+1];
Zcoords_gt = P3_gt(2*T+1:3*T,:) - mean(P3_gt(2*T+1:3*T,:),2)*ones(1,J);
Zdist = max(Zcoords_gt,[],2) - min(Zcoords_gt,[],2); % size of the 3D shape along the Z axis for each time frame
ze = [];
fprintf('3D reconstruction with %f missing data...\n', missingdata_rate*100);
for kk=k_values,
[P3, S_hat, V, RO, Tr, Z] = em_sfm(p2_obs, MD, kk, use_lds, tol, max_em_iter);
% Compares it with ground truth.
% Note that there are still 2 ambiguities that cannot be resolved:
% 1. depth direction (i.e. the shape could be "flipped" along the Z axis) -> we test both possibilities
% 2. Z translation -> we subtract the mean of the Z coords to evaluate reconstruction results
Zcoords_em = P3(2*T+1:3*T,:) - mean(P3(2*T+1:3*T,:),2)*ones(1,J);
Zerror1 = mean( mean(abs(Zcoords_em - Zcoords_gt), 2)./Zdist );
Zerror2 = mean( mean(abs(-Zcoords_em - Zcoords_gt), 2)./Zdist );
avg_zerror = 100*min(Zerror1, Zerror2);
ze = [ze avg_zerror];
hold off;
plot(k_values(1:length(ze)), ze, '-o');
title(['3D reconstruction with ' num2str(missingdata_rate*100) '% missing data'], 'fontweight', 'bold');
xlabel('K (number of deformation shapes)');
ylabel('% Z error');
grid on;
drawnow;
end

4
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/readme.txt Normal file
View File

@@ -0,0 +1,4 @@
See randomshapes_demo.m and shark_demo.m for more information.
I recommend that you try to compile the CMEX code for the function computeH:
type 'mex computeH.c' in the Matlab Command Window ('mex computeH.c -l matlb' under Unix)

39
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/rigidfac.m Normal file
View File

@@ -0,0 +1,39 @@
function [R, Tr, S] = rigidfac(P, MD)
% [R, Tr, S] = rigidfac(P, MD)
%
% Computes rank 3 factorization: P = R*S + Tr
Pnew = P;
[T, J] = size(Pnew); T = T/2;
if sum(MD(:)) > 0, % if there is missing data, then it uses an iterative solution to get a rough initialization for the missing points
[i,j] = find(MD==1);
ind = sub2ind(size(P), [i; i+T], [j; j]);
numIter = 10;
else
numIter = 1;
ind = [];
end
r = 3;
for iter=1:numIter,
Tr = Pnew*ones(J,1)/J;
Pnew_c = Pnew - Tr*ones(1,J);
[a,b,c] = svd(Pnew_c,0);
smallb = b(1:r,1:r);
sqrtb = sqrt(smallb);
Rhat = a(:,1:r) * sqrtb;
Shat = sqrtb * c(:,1:r)';
P_approx = Rhat*Shat + Tr*ones(1,J);
Pnew(ind) = P_approx(ind);
end
G = findG(Rhat);
R = Rhat*G;
S = inv(G)*Shat;

60
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/shark_demo.m Normal file
View File

@@ -0,0 +1,60 @@
% Shark Demo
%
% Copyright (c) by Lorenzo Torresani, Stanford University
%
% A demo of Non-Rigid Structure From Motion on artificial shark sequence
%
%
% The 3D reconstruction technique is based on the following paper:
%
% Lorenzo Torresani, Aaron Hertzmann and Christoph Bregler,
% Learning Non-Rigid 3D Shape from 2D Motion, NIPS 16, 2003
% http://cs.stanford.edu/~ltorresa/projects/learning-nr-shape/
%
%
% Function em_sfm implements the algorithms "EM-Gaussian" and "EM-LDS" described
% in the paper
%
% I recommend that you try to compile the CMEX code for the function computeH:
% type 'mex computeH.c' in the Matlab Command Window ('mex computeH.c -l matlb' under Unix)
%
% loads the matrix P3_gt containing the ground thruth data: P3_gt([t t+T t+2*T],:) contains the 3D coordinates of the J points at time t
% (T is the number of frames, J is the number of points)
load('jaws.mat');
[T, J] = size(P3_gt); T = T/3;
% 2D motion resulting from orthographic projection (Eq (1))
p2_obs = P3_gt(1:2*T, :);
% runs the non-rigid structure from motion algorithm
use_lds = 1;
max_em_iter = 60;
tol = 0.0001;
K = 2; % number of deformation shapes
Zcoords_gt = P3_gt(2*T+1:3*T,:) - mean(P3_gt(2*T+1:3*T,:),2)*ones(1,J);
Zdist = max(Zcoords_gt,[],2) - min(Zcoords_gt,[],2); % size of the 3D shape along the Z axis for each time frame
MD = zeros(T,J);
[P3, S_hat, V, RO, Tr, Z] = em_sfm(p2_obs, MD, K, use_lds, tol, max_em_iter);
%% Compares it with ground truth.
% Note that there are still 2 unresolvable ambiguities:
% 1. depth direction (i.e. the shape could be "flipped" along the Z axis) -> we test both possibilities
% 2. Z translation -> we subtract the mean of the Z coords to evaluate reconstruction results
Zcoords_em = P3(2*T+1:3*T,:) - mean(P3(2*T+1:3*T,:),2)*ones(1,J);
Zerror1 = mean( mean(abs(Zcoords_em - Zcoords_gt), 2)./Zdist );
Zerror2 = mean( mean(abs(-Zcoords_em - Zcoords_gt), 2)./Zdist );
if Zerror2 < Zerror1,
avg_zerror = 100*Zerror2;
P3(2*T+1:3*T,:) = -(P3(2*T+1:3*T,:) - mean(P3(2*T+1:3*T,:),2)*ones(1,J));
else
avg_zerror = 100*Zerror1;
P3(2*T+1:3*T,:) = P3(2*T+1:3*T,:) - mean(P3(2*T+1:3*T,:),2)*ones(1,J);
end
fprintf('Average reconstruction error in Z: %f%%\n', avg_zerror);
vis_reconstruction(P3_gt, P3);

59
pkg/OpenFace/model_training/pdm_generation/nrsfm-em/vis_reconstruction.m Normal file
View File

@@ -0,0 +1,59 @@
function vis_reconstruction(P3_gt, P3_rec)
[T, J] = size(P3_gt); T = T/3;
xmin = min(min(P3_gt(1:T,:)));
xmax = max(max(P3_gt(1:T,:)));
xmin = xmin - (xmax-xmin)*0.1;
xmax = xmax + (xmax-xmin)*0.1;
ymin = min(min(P3_gt(T+1:2*T,:)));
ymax = max(max(P3_gt(T+1:2*T,:)));
ymin = ymin - (ymax-ymin)*0.1;
ymax = ymax + (ymax-ymin)*0.1;
zmin = min(min(P3_gt(2*T+1:3*T,:)));
zmax = max(max(P3_gt(2*T+1:3*T,:)));
zmin = zmin - (zmax-zmin)*0.1;
zmax = zmax + (zmax-zmin)*0.1;
figure(3);
hold off;
plot([xmin xmax], [ymin ymin], 'k-');
hold on;
axis equal;
axis off;
set(gcf, 'color', [1 1 1]);
plot([xmin xmax], [ymax ymax], 'k-');
plot([xmin xmin], [ymin ymax], 'k-');
plot([xmax xmax], [ymin ymax], 'k-');
delta = ymin-zmax-10;
plot([xmin xmax], [zmin zmin]+delta, 'k-');
plot([xmin xmax], [zmax zmax]+delta, 'k-');
plot([xmin xmin], [zmin zmax]+delta, 'k-');
plot([xmax xmax], [zmin zmax]+delta, 'k-');
ht1=text((xmin+xmax)/2, ymax-10, 'Input 2D tracks', 'HorizontalAlignment', 'center', 'fontweight', 'bold', 'fontname', 'verdana');
ht2=text((xmin+xmax)/2, zmax+delta-10, '3D shape', 'HorizontalAlignment', 'center', 'fontweight', 'bold', 'fontname', 'verdana');
for t=1:T
if t>1,
delete(hs1);
delete(hs2);
delete(hs3);
end
hs1 = plot(P3_gt(t,:), P3_gt(t+T,:), 'g.');
hs2 = plot(P3_gt(t,:), P3_gt(t+2*T,:)-mean(P3_gt(t+2*T,:))+delta, 'b.');
hs3 = plot(P3_rec(t,:), P3_rec(t+2*T,:)-mean(P3_rec(t+2*T,:))+delta, 'ro');
if t==1,
hh=legend([hs2,hs3],'ground truth', 'reconstruction', 4);
set(hh, 'fontname', 'verdana');
end
drawnow;
I = getframe;
if 0,
str = sprintf('frame%04d', t);
imwrite(I.cdata, [str '.jpg'], 'Quality', 100);
end
end