void DOsvd(float *a,float *res,float *comp,float *values,int nframes,int framesize,int compressedsize)
{
int *remap;
int i,j;
int nrows;
nrows=nframes;
if (nrows<framesize)
nrows=framesize;
double **da=allocMatrix(nrows,framesize);
double **v=allocMatrix(framesize,framesize);
double *w=allocVect(framesize);
float mx;
int bestat;
for (j=0;j<framesize;j++)
{
for (i=0;i<nframes;i++)
da[j+1][i+1]=a[i*framesize+j];
for (;i<nrows;i++)
da[j+1][i+1]=0.0;
}
svdcmp(da,nrows,framesize,w,v);
remap=new int[framesize];
for (i=0;i<framesize;i++)
remap[i]=-1;
for (j=0;j<compressedsize;j++)
{
mx=-1.0f;
for (i=0;i<framesize;i++)
{
if (remap[i]<0&&fabs(w[i+1])>mx)
{
mx=fabs(w[i+1]);
bestat=i;
}
}
assert(mx>-.5f);
remap[bestat]=j;
}
// josh **DO NOT** put your dof>nframes mod here
for (i=0;i<framesize;i++)
{
if (remap[i]<0)
w[i+1]=0.0;
else
{
values[remap[i]]=w[i+1];
for (j=0;j<framesize;j++)
res[remap[i]*framesize+j]=v[j+1][i+1];
}
}
freeVect(w);
freeMatrix(v,framesize);
freeMatrix(da,nrows);
delete[] remap;
}
void NR::svdfit(Vec_I_DP &x, Vec_I_DP &y, Vec_I_DP &sig, Vec_O_DP &a,
Mat_O_DP &u, Mat_O_DP &v, Vec_O_DP &w, DP &chisq,
void funcs(const DP, Vec_O_DP &))
{
int i,j;
const DP TOL=1.0e-13;
DP wmax,tmp,thresh,sum;
int ndata=x.size();
int ma=a.size();
Vec_DP b(ndata),afunc(ma);
for (i=0;i<ndata;i++) {
funcs(x[i],afunc);
tmp=1.0/sig[i];
for (j=0;j<ma;j++) u[i][j]=afunc[j]*tmp;
b[i]=y[i]*tmp;
}
svdcmp(u,w,v);
wmax=0.0;
for (j=0;j<ma;j++)
if (w[j] > wmax) wmax=w[j];
thresh=TOL*wmax;
for (j=0;j<ma;j++)
if (w[j] < thresh) w[j]=0.0;
svbksb(u,w,v,b,a);
chisq=0.0;
for (i=0;i<ndata;i++) {
funcs(x[i],afunc);
sum=0.0;
for (j=0;j<ma;j++) sum += a[j]*afunc[j];
chisq += (tmp=(y[i]-sum)/sig[i],tmp*tmp);
}
}
void SVD(Array2d<double>& a,double* w,Array2d<double>& v)
{ // w -- signular values; a = UWV', where U is stored in a
// NOTE: a.nrow >= a.ncol is required
Array2dC<double*> nr_a(1,a.nrow+1),nr_v(1,a.ncol+1);
v.Create(a.ncol,a.ncol);
for(int i=0;i<a.nrow;i++) nr_a.buf[i+1] = a.p[i] - 1;
for(int i=0;i<a.ncol;i++) nr_v.buf[i+1] = v.p[i] - 1;
svdcmp(nr_a.buf,a.nrow,a.ncol,w-1,nr_v.buf);
}
VALUE decompose(VALUE module, VALUE matrix_ruby, VALUE m_ruby, VALUE n_ruby) {
int m = NUM2INT(m_ruby);
int n = NUM2INT(n_ruby);
float **u = matrix(1, m, 1, n);
float **v = matrix(1, m, 1, n);
float *w = vector(1, n);
VALUE *matrix_values = RARRAY_PTR(matrix_ruby);
int offset = 0;
int i, j;
/* output arrays */
VALUE u_output = rb_ary_new();
VALUE v_output = rb_ary_new();
VALUE w_output = rb_ary_new();
VALUE output = rb_ary_new();
/* precondition */
if((m*n) != RARRAY_LEN(matrix_ruby)) {
rb_raise(rb_eRangeError, "Size of the array is not equal to m * n");
return output;
}
/* convert to u matrix */
for(i = 1; i <= m; i++) {
for(j = 1; j <= n; j++) {
offset = ((i-1)*n) + (j-1);
u[i][j] = (float) NUM2DBL(matrix_values[offset]);
}
}
/* perform SVD */
svdcmp(u, m, n, w, v);
/* create w output array */
for(i = 1; i <= n; i++)
rb_ary_push(w_output, rb_float_new(w[i]));
/* create u arrays */
for(i = 1; i <= m; i++) {
for(j = 1; j <= n; j++) {
rb_ary_push(u_output, rb_float_new(u[i][j]));
}
}
/* create v arrays */
for(i = 1; i <= n; i++) {
for(j = 1; j <= n; j++) {
rb_ary_push(v_output, rb_float_new(v[i][j]));
}
}
rb_ary_push(output, u_output);
rb_ary_push(output, w_output);
rb_ary_push(output, v_output);
return output;
}
void svdfit(int ndata,dfprec *a,int ma,dfprec *u,dfprec *v,dfprec *w, dfprec *b){
int j;
dfprec wmax,thresh;
svdcmp(u,ndata,ma,w,v);
wmax=0.0;
for (j=0;j<ma;j++)
if (w[j] > wmax) wmax=w[j];
thresh=TOL*wmax;
for (j=0;j<ma;j++)
if (w[j] < thresh) w[j]=0.0;
svbksb(u,w,v,ndata,ma,b,a);
}
void inverse( double** a, int nRows, int nCols)
{
int i,j,k;
double* w = new double[nCols];
double** v = allocate2DDouble( nRows, nCols);
double** temp = allocate2DDouble( nRows, nCols );
for( i=0; i<nRows; i++)
memset( v[i], 0, sizeof(double) * nCols );
memset( w, 0, sizeof(double) * nCols );
// SVD
copyMatrix( a, temp, nRows, nCols);
svdcmp(temp, nRows, nCols, w, v);
for( i=0; i<nCols; i++)
{
if( !nearzero( w[i] ) )
w[i] = 1.0 / w[i];
}
for( i=0; i<nCols; i++)
for( j=0; j<nRows; j++)
{
temp[j][i] = temp[j][i] * w[i];
}
for( i=0; i<nRows; i++)
memset( a[i], 0, sizeof(double) * nCols );
for( i=0; i<nRows; i++)
for( j=0; j<nCols; j++)
for( k=0; k<nCols; k++)
{
a[i][j] += temp[i][k] * v[j][k]; // coz' v is not transposed
}
SAFEDELARR( w );
if( v != NULL )
{
for( i=0; i<nRows; i++ )
SAFEDELARR( v[i] );
SAFEDELARR( v );
}
if( temp != NULL )
{
for( i=0; i<nRows; i++ )
SAFEDELARR( temp[i] );
SAFEDELARR( temp );
}
}
void covar_pca_xx(double **covar, double **evect, double *eval, int ndata)
{
double **tmp, **tmp1, **tmp2, **diag, **tmp3;
int n,i,j,k,nrot;
double cumu = 0;
n = ndata;
tmp = dmatrix(1,n,1,n);
for(i=1;i<=n;++i)
for(j=1;j<=n;++j)
tmp[i][j] = covar[i][j]/sqrt(covar[i][i]*covar[j][j]);
svdcmp(tmp,n,n,eval,evect);
return;
}
/******************************************************************************
General linear least squares fit routine
from section 15.4 of Numerical Recipes.
yfit(x) = function which fills f[i],i=0..o-1 with the o
fitting functions evaluated at x.
fom = if nonzero figure-of-merit is returned here.
a = fitting parameters
av = if (av) error variances for the fitting parameters returned here.
x = n abscissas
y = n ordinates
ys = if (ys) = n error standard deviations for y values
tol = smallest fraction of maximum singular value (eigenvalues, roughly)
which a small singular value can equal -- smaller values are
set to zero, assumed to indicate redundancy. NR suggests
of order 10^-6
n = number of abscissas.
o = number of fitting parameters.
*/
static fit_rc fit_lsq(void (*yfit)(), double *fom, double *a, double *av,
const double *x, const double *y, const double *ys,
double tol, int n, int o) {
double wmax,wmin,xsq,sum ;
int i,j ;
const char *me = "fit_lsq" ;
if (check_memory(o,n) != OK) return(memfail(__LINE__,me)) ;
for(i=0;i<n;i++) {
yfit(x[i]) ;
for(j=0;j<o;j++) u[i][j] = f[j] * (ys ? 1.0/ys[i] : 1.0) ;
} ;
memcpy(b,y,n*sizeof(double)) ;
if (ys) for(i=0;i<n;i++) b[i] /= ys[i] ;
if (svdcmp(u,n,o) != OK)
return(punt(__LINE__,me,"singular value decomposition failed.")) ;
wmax = 0.0 ;
for(wmax=0.0,j=0;j<o;j++) if (w[j] > wmax) wmax = w[j] ;
wmin = tol * wmax ;
for(j=0;j<o;j++) if (w[j] < wmin) w[j] = 0.0 ;
if (svbksb(a,n,o) != OK)
return(punt(__LINE__,me,"back substitution failed.")) ;
if (av) {
if (svdvar(o) != OK)
return(punt(__LINE__,me,"variance calculation failed.")) ;
for(i=0;i<o;i++) av[i] = cvm[i][i] ;
} ;
if (fom) {
xsq = 0.0 ;
for(i=0;i<o;i++) {
yfit(x[i]) ;
sum = 0.0 ;
for(j=0;j<o;j++) sum += a[j] * f[j] ;
sum = (y[i] - sum)/(ys ? ys[i]*ys[i] : 1.0) ;
xsq += sum*sum ;
} ;
*fom = xsq ;
} ;
return(OK) ;
}
void testSVD() {
int m = 3;
int n = 3;
ublas::matrix<double> mat(m,n);
ublas::matrix<double> u(m,n);
ublas::matrix<double> v(n,n);
ublas::vector<double> w(n);
/* mat(0,0) = 1;
mat(0,1) = 1;
mat(0,2) = 2;
mat(1,0) = 3;
mat(1,1) = 4;
mat(1,2) = 5;
mat(2,0) = 6;
mat(2,1) = 7;
mat(2,2) = 8;
*/
mat(0,0) = .1875;
mat(0,1) = -.0625;
mat(0,2) = -.0625;
mat(1,0) = -.0625;
mat(1,1) = .1875;
mat(1,2) = -.0625;
mat(2,0) = -.0625;
mat(2,1) = -.0625;
mat(2,2) = .1875;
svdcmp(mat, u, w, v);
print("mat", mat);
print("u", u);
print("w", w);
print("v",v);
ublas::matrix<double> wm(n,n);
wm*=0;
wm(0,0) = w(0);
wm(1,1) = w(1);
wm(2,2) = w(2);
ublas::matrix<double> orig(m,n);
orig = prod(wm, trans(v));
orig = prod(u, orig);
print("orig", orig);
}
void DOsvdPlane(float *pnts,int npnts,float *n,float *base)
{
int i,j;
double **da=allocMatrix(npnts,3);
double **v=allocMatrix(3,3);
double *w=allocVect(3);
float mn=1E30f;
int bestat;
assert(npnts>=3);
base[0]=pnts[0];
base[1]=pnts[1];
base[2]=pnts[2];
for (i=1;i<npnts;i++)
{
for (j=0;j<3;j++)
base[j]+=pnts[i*3+j];
}
base[0]/=(float)(npnts);
base[1]/=(float)(npnts);
base[2]/=(float)(npnts);
for (i=0;i<3;i++)
{
for (j=0;j<npnts;j++)
da[j+1][i+1]=pnts[j*3+i]-base[i];
}
svdcmp(da,npnts,3,w,v);
for (i=0;i<3;i++)
{
if (fabs(w[i+1])<mn)
{
mn=(float) fabs(w[i+1]);
bestat=i;
}
}
n[0]=(float) v[1][bestat+1];
n[1]=(float) v[2][bestat+1];
n[2]=(float) v[3][bestat+1];
freeVect(w);
freeMatrix(v,3);
freeMatrix(da,npnts);
}
/* This is the SVD algorithm from numerical recipes in c second edition*/
result_t LeastSquaresSolve(float x[], float y[], float sig[], int ndata, float a[], int ma, float **u, float **v, float w[], float *chisq)
{
int j,i;
float wmax,tmp,thresh,sum;
float b[MAX_MEMBERS_AnchorHood+1], afunc[MAX_MEMBERS_AnchorHood+1];
if(ndata > MAX_MEMBERS_AnchorHood+1)
;
// dbg(DBG_ERR, "too large matrix needed\n"); //not really gives any error info when make mica(2). some err handling necessary here
for (i=1;i<=ndata;i++) {
LeastSquaresEvaluateBasisFunctions(x[i],afunc,ma);
tmp=1.0/sig[i];
for (j=1;j<=ma;j++) u[i][j]=afunc[j]*tmp;
b[i]=y[i]*tmp;
}
{
uint8_t i;
printf("z ");
for(i=1;i<=ndata;i++) {
uint8_t j;
for(j=1;j<=ma;j++) {
printf("%f ",u[i][j]);
}
}
printf("\n");
}
if(svdcmp(u,ndata,ma,w,v)==FAIL) return FAIL;
wmax=0.0;
for (j=1;j<=ma;j++)
if (w[j] > wmax) wmax=w[j];
thresh=TOL*wmax;
for (j=1;j<=ma;j++)
if (w[j] < thresh) w[j]=0.0;
svbksb(u,w,v,ndata,ma,b,a); //@@
*chisq=0.0;
for (i=1;i<=ndata;i++) {
LeastSquaresEvaluateBasisFunctions(x[i],afunc,ma);
for (sum=0.0,j=1;j<=ma;j++) sum += a[j]*afunc[j];
*chisq += (tmp=(y[i]-sum)/sig[i],tmp*tmp);
}
return SUCCESS;
}
void covar_pca(int ii)
{
double **tmp, **tmp1, **evect, *eval, **tmp2, **diag, **tmp3;
int n,i,j,k,nrot;
double cumu = 0;
n = wpl.ndata[ii];
tmp = dmatrix(1,n,1,n);
tmp2 = dmatrix(1,n,1,n);
tmp3 = dmatrix(1,n,1,n);
diag = dmatrix(1,n,1,n);
tmp1 = dmatrix(1,n,1,1);
evect = dmatrix(1,n,1,n);
eval = dvector(1,n);
wpl.evect[ii] = dmatrix(1,n,1,n);
wpl.eval[ii] = dvector(1,n);
wpl.npca = PCA;
for(i=1;i<=n;++i)
wpl.edata[ii][i] = sqrt(wpl.covar[ii][i][i]);
for(i=1;i<=n;++i)
for(j=1;j<=n;++j)
tmp[i][j] = wpl.covar[ii][i][j]/wpl.edata[ii][i]/wpl.edata[ii][j];
for(i=1;i<=-n;++i)
for(j=1;j<=n;++j)
tmp3[i][j] = tmp[i][j] = wpl.covar[ii][i][j];
//jacobi(tmp,n,eval,evect,&nrot);
/* replace jacobi with a singular-value decomposition
*/
svdcmp(tmp,n,n,eval,evect);
for(i=1;i<=n;++i)
{
wpl.eval[ii][i] = eval[i];
for(j=1;j<=n;++j)
wpl.evect[ii][i][j] = evect[i][j];
}
return;
}
int svdsolve(eusfloat_t **a, int m, int n, eusfloat_t *b, eusfloat_t *x)
{
int j;
eusfloat_t **v, *w, wmax, wmin;
v = nr_matrix(1,n,1,n);
w = nr_vector(1,n);
if ( svdcmp(a,m,n,w,v) < 0 ) {
free_nr_vector(w,1,n);
free_nr_matrix(v,1,n,1,n);
return -1;
}
wmax = 0.0;
for (j=1; j<=n; j++) if (w[j] > wmax) wmax = w[j];
wmin = wmax*1.0e-6;
for (j=1; j<=n; j++) if (w[j] < wmin) w[j] = 0.0;
svbksb(a,w,v,m,n,b,x);
free_nr_vector(w,1,n);
free_nr_matrix(v,1,n,1,n);
return 1;
}
boolean DL_largematrix::prep_for_solve(){
// returns if there were any singularities
switch (sm) {
case lud_bcksub:
if ((2*bandw>nrrows?ludcmp():ludcmpbw())>=0) {
// ((near) singular value detected)
set_solve_method(conjug_grad);
return prep_for_solve();
}
return FALSE;
case conjug_grad:
if (2*nrnonzero<nrelem)
// use previously calculated sparse matrix representation
rep=riss;
return FALSE;
case svd:
return (svdcmp()!=0);
}
return FALSE;
}
static int SVD(double *U, double *W, double *V, double *matx, int M, int N) {
// Assumes allocation for U is MxN
double **nrU = (double **)aom_malloc((M) * sizeof(*nrU));
double **nrV = (double **)aom_malloc((N) * sizeof(*nrV));
int problem, i;
problem = !(nrU && nrV);
if (!problem) {
for (i = 0; i < M; i++) {
nrU[i] = &U[i * N];
}
for (i = 0; i < N; i++) {
nrV[i] = &V[i * N];
}
} else {
if (nrU) aom_free(nrU);
if (nrV) aom_free(nrV);
return 1;
}
/* copy from given matx into nrU */
for (i = 0; i < M; i++) {
memcpy(&(nrU[i][0]), matx + N * i, N * sizeof(*matx));
}
/* HERE IT IS: do SVD */
if (svdcmp(nrU, M, N, W, nrV)) {
aom_free(nrU);
aom_free(nrV);
return 1;
}
/* aom_free Numerical Recipes arrays */
aom_free(nrU);
aom_free(nrV);
return 0;
}
/* static */
void _least_squares_solution(double **a, int M, int N, double *b, double *x)
{
/*
Solve the matrix equation
Ep z = f
Here we use SVD from Numerical Recipes since
we cannot ensure that M==N.
In other languages, such as IgorPro,
could use QR or LU decomposition instead
if the chosen routine allows the M!=N case.
*/
double *w, **v, wMax, wMin;
int j;
w = vector(1,N);
v = matrix(1,N, 1,N);
/*
Singular Value decomposition a[][] = u[][] w[] v[][]
a[][] is changed into u[][] by SVD
*/
svdcmp(a, M, N, w, v);
/* find and zero the insignificant singular values */
wMax = w[1];
for (j=1; j<=N; j++) if (w[j]>wMax) wMax = w[j];
wMin = wMax * SV_CUTOFF;
/* DEBUG_MARKER; SHOW_VECTOR("SVD w", w, 1, N, "%lg"); */
/* printf ("singular value cutoff = %lg\n", wMin); */
for (j=1; j<=N; j++) if (w[j]<wMin) w[j] = 0.0;
/*
Singular Value backsubstitution solution of a z = b
*/
svbksb(a, w, v, M, N, b, x);
/* DEBUG_MARKER; SHOW_VECTOR("SVD bksb x", x, 1, N, "%lg"); */
free_matrix(v, 1,N, 1,N);
free_vector(w, 1,N);
}
void svdfit(double **x, double *y, double *sig, int ndata, double *a, int ma,
double **u, double **v, double *w, double *chisq,
void (*funcs)(double *,double *,int))
{
int j,i;
double wmax,tmp,thresh,sum,*b,*afunc,*dvector();
void svdcmp(),svbksb(),free_dvector();
b=dvector(1,ndata);
afunc=dvector(1,ma);
for (i=1;i<=ndata;i++)
{ /* accumulate coefficients of the fitting matrix */
(*funcs)(x[i],afunc,ma);
tmp=1.0/sig[i];
for (j=1;j<=ma;j++) u[i][j]=afunc[j]*tmp;
b[i]=y[i]*tmp;
}
svdcmp(u,ndata,ma,w,v);
wmax=0.0;
for (j=1;j<=ma;j++)
if (w[j] > wmax) wmax=w[j];
thresh=TOL*wmax;
for (j=1;j<=ma;j++)
if (w[j] < thresh) w[j]=0.0;
svbksb(u,w,v,ndata,ma,b,a);
*chisq=0.0;
for (i=1;i<=ndata;i++)
{
(*funcs)(x[i],afunc,ma);
for (sum=0.0,j=1;j<=ma;j++) sum += a[j]*afunc[j];
*chisq += (tmp=(y[i]-sum)/sig[i],tmp*tmp);
}
free_dvector(afunc,1,ma);
free_dvector(b,1,ndata);
}
/**
* B returns the pseudoinverse of A (A with dimension N rows x 3 columns).
* It is the matrix v.[diag(1/wi)].(u)t (cf. svdcmp())
* Function returns True if B has maximum rank, False otherwise
* @param A
* @param N
* @param B
* @return
*/
int pseudo_inverse(double **A, int N, double **B)
{
void svdcmp();
double **V, temp[3][3];
double W[3];
double WMAX;
double TOL = 0.01;
int i, j, k;
int isMaxRank = 1;/* stays true if no singular value under tolerance level */
/*allocations*/
V = init_double_array(nbCoords, nbCoords);
/*Singular value decomposition*/
if (debug_svd)
print_matrix(A, N, nbCoords);
svdcmp(A, N, nbCoords, W, V);
if (debug_svd)
{
print_matrix(A, N, nbCoords);
print_matrix(V, nbCoords, nbCoords);
print_vector(W, nbCoords);
}
/*Getting largest singular value*/
WMAX = 0.0;
for (i = 0; i < nbCoords; i++)
{
if (W[i] > WMAX)
WMAX = W[i];
}
/*Checking for signular values smaller than TOL times the largest one*/
for (i = 0; i < nbCoords; i++)
{
if (W[i] < TOL * WMAX)
{
W[i] = 0;
isMaxRank = 0;
return isMaxRank;
}
}
if (isMaxRank)
{
/*Computing B*/
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
temp[i][j] = V[i][j] / W[j];
}
}
for (i = 0; i < 3; i++)
{
for (j = 0; j < N; j++)
{
B[i][j] = 0.0;
for (k = 0; k < 3; k++)
{
B[i][j] += temp[i][k] * A[j][k];
}
}
}
if (debug_svd)
print_matrix(B, nbCoords, N);
/*deallocations*/
for (i = 0; i < nbCoords; i++)
free(V[i]);
return isMaxRank;
}
return isMaxRank;
}
/**
* ICA function. Computes the W matrix from the
* preprocessed data.
*/
static mat ICA_compute(mat X, int rows, int cols)
{
mat TXp, GWX, W, Wd, W1, D, TU, TMP;
vect d, lim;
int i, it;
// matrix creation
TXp = mat_create(cols, rows);
GWX = mat_create(rows, cols);
W = mat_create(rows, rows);
Wd = mat_create(rows, rows);
D = mat_create(rows, rows);
TMP = mat_create(rows, rows);
TU = mat_create(rows, rows);
W1 = mat_create(rows, rows);
d = vect_create(rows);
// W rand init
mat_apply_fx(W, rows, rows, fx_rand, 0);
// sW <- La.svd(W)
mat_copy(W, rows, rows, Wd);
svdcmp(Wd, rows, rows, d, D);
// W <- sW$u %*% diag(1/sW$d) %*% t(sW$u) %*% W
mat_transpose(Wd, rows, rows, TU);
vect_apply_fx(d, rows, fx_inv, 0);
mat_diag(d, rows, D);
mat_mult(Wd, rows, rows, D, rows, rows, TMP);
mat_mult(TMP, rows, rows, TU, rows, rows, D);
mat_mult(D, rows, rows, W, rows, rows, Wd); // W = Wd
// W1 <- W
mat_copy(Wd, rows, rows, W1);
// lim <- rep(1000, maxit); it = 1
lim = vect_create(MAX_ITERATIONS);
for (i=0; i<MAX_ITERATIONS; i++)
lim[i] = 1000;
it = 0;
// t(X)/p
mat_transpose(X, rows, cols, TXp);
mat_apply_fx(TXp, cols, rows, fx_div_c, cols);
while (lim[it] > TOLERANCE && it < MAX_ITERATIONS) {
// wx <- W %*% X
mat_mult(Wd, rows, rows, X, rows, cols, GWX);
// gwx <- tanh(alpha * wx)
mat_apply_fx(GWX, rows, cols, fx_tanh, 0);
// v1 <- gwx %*% t(X)/p
mat_mult(GWX, rows, cols, TXp, cols, rows, TMP); // V1 = TMP
// g.wx <- alpha * (1 - (gwx)^2)
mat_apply_fx(GWX, rows, cols, fx_1sub_sqr, 0);
// v2 <- diag(apply(g.wx, 1, FUN = mean)) %*% W
mat_mean_rows(GWX, rows, cols, d);
mat_diag(d, rows, D);
mat_mult(D, rows, rows, Wd, rows, rows, TU); // V2 = TU
// W1 <- v1 - v2
mat_sub(TMP, TU, rows, rows, W1);
// sW1 <- La.svd(W1)
mat_copy(W1, rows, rows, W);
svdcmp(W, rows, rows, d, D);
// W1 <- sW1$u %*% diag(1/sW1$d) %*% t(sW1$u) %*% W1
mat_transpose(W, rows, rows, TU);
vect_apply_fx(d, rows, fx_inv, 0);
mat_diag(d, rows, D);
mat_mult(W, rows, rows, D, rows, rows, TMP);
mat_mult(TMP, rows, rows, TU, rows, rows, D);
mat_mult(D, rows, rows, W1, rows, rows, W); // W1 = W
// lim[it + 1] <- max(Mod(Mod(diag(W1 %*% t(W))) - 1))
mat_transpose(Wd, rows, rows, TU);
mat_mult(W, rows, rows, TU, rows, rows, TMP);
lim[it+1] = fabs(mat_max_diag(TMP, rows, rows) - 1);
// W <- W1
mat_copy(W, rows, rows, Wd);
it++;
}
// clean up
mat_delete(TXp, cols, rows);
mat_delete(GWX, rows, cols);
mat_delete(W, rows, rows);
mat_delete(D, rows, rows);
mat_delete(TMP, rows, rows);
mat_delete(TU, rows, rows);
mat_delete(W1, rows, rows);
vect_delete(d);
return Wd;
}
/**
* Main FastICA function. Centers and whitens the input
* matrix, calls the ICA computation function ICA_compute()
* and computes the output matrixes.
*/
void fastICA(mat X, int rows, int cols, int compc, mat K, mat W, mat A, mat S)
{
mat XT, V, TU, D, X1, _A;
vect scale, d;
// matrix creation
XT = mat_create(cols, rows);
X1 = mat_create(compc, rows);
V = mat_create(cols, cols);
D = mat_create(cols, cols);
TU = mat_create(cols, cols);
scale = vect_create(cols);
d = vect_create(cols);
/*
* CENTERING
*/
mat_center(X, rows, cols, scale);
/*
* WHITENING
*/
// X <- t(X); V <- X %*% t(X)/rows
mat_transpose(X, rows, cols, XT);
mat_apply_fx(X, rows, cols, fx_div_c, rows);
mat_mult(XT, cols, rows, X, rows, cols, V);
// La.svd(V)
svdcmp(V, cols, cols, d, D); // V = s$u, d = s$d, D = s$v
// D <- diag(c(1/sqrt(d))
vect_apply_fx(d, cols, fx_inv_sqrt, 0);
mat_diag(d, cols, D);
// K <- D %*% t(U)
mat_transpose(V, cols, cols, TU);
mat_mult(D, cols, cols, TU, cols, cols, V); // K = V
// X1 <- K %*% X
mat_mult(V, compc, cols, XT, cols, rows, X1);
/*
* FAST ICA
*/
_A = ICA_compute(X1, compc, rows);
/*
* OUTPUT
*/
// X <- t(x)
mat_transpose(XT, cols, rows, X);
mat_decenter(X, rows, cols, scale);
// K
mat_transpose(V, compc, cols, K);
// w <- a %*% K; S <- w %*% X
mat_mult(_A, compc, compc, V, compc, cols, D);
mat_mult(D, compc, cols, XT, cols, rows, X1);
// S
mat_transpose(X1, compc, rows, S);
// A <- t(w) %*% solve(w * t(w))
mat_transpose(D, compc, compc, TU);
mat_mult(D, compc, compc, TU, compc, compc, V);
mat_inverse(V, compc, D);
mat_mult(TU, compc, compc, D, compc, compc, V);
// A
mat_transpose(V, compc, compc, A);
// W
mat_transpose(_A, compc, compc, W);
// cleanup
mat_delete(XT, cols, rows);
mat_delete(X1, compc, rows);
mat_delete(V, cols, cols);
mat_delete(D, cols, cols);
mat_delete(TU,cols, cols);
vect_delete(scale);
vect_delete(d);
}
//majorAxis and minorAxis is the estimated particle size in px
void ProgSortByStatistics::processInprocessInputPrepareSPTH(MetaData &SF, bool trained)
{
//#define DEBUG
PCAMahalanobisAnalyzer tempPcaAnalyzer0;
PCAMahalanobisAnalyzer tempPcaAnalyzer1;
PCAMahalanobisAnalyzer tempPcaAnalyzer2;
PCAMahalanobisAnalyzer tempPcaAnalyzer3;
PCAMahalanobisAnalyzer tempPcaAnalyzer4;
//Morphology
tempPcaAnalyzer0.clear();
//Signal to noise ratio
tempPcaAnalyzer1.clear();
tempPcaAnalyzer2.clear();
tempPcaAnalyzer3.clear();
//Histogram analysis, to detect black points and saturated parts
tempPcaAnalyzer4.clear();
double sign = 1;//;-1;
int numNorm = 3;
int numDescriptors0=numNorm;
int numDescriptors2=4;
int numDescriptors3=11;
int numDescriptors4 = 10;
MultidimArray<float> v0(numDescriptors0);
MultidimArray<float> v2(numDescriptors2);
MultidimArray<float> v3(numDescriptors3);
MultidimArray<float> v4(numDescriptors4);
if (verbose>0)
{
std::cout << " Sorting particle set by new xmipp method..." << std::endl;
}
int nr_imgs = SF.size();
if (verbose>0)
init_progress_bar(nr_imgs);
int c = XMIPP_MAX(1, nr_imgs / 60);
int imgno = 0, imgnoPCA=0;
bool thereIsEnable=SF.containsLabel(MDL_ENABLED);
bool first=true;
// We assume that at least there is one particle
size_t Xdim, Ydim, Zdim, Ndim;
getImageSize(SF,Xdim,Ydim,Zdim,Ndim);
//Initialization:
MultidimArray<double> nI, modI, tempI, tempM, ROI;
MultidimArray<bool> mask;
nI.resizeNoCopy(Ydim,Xdim);
modI.resizeNoCopy(Ydim,Xdim);
tempI.resizeNoCopy(Ydim,Xdim);
tempM.resizeNoCopy(Ydim,Xdim);
mask.resizeNoCopy(Ydim,Xdim);
mask.initConstant(true);
MultidimArray<double> autoCorr(2*Ydim,2*Xdim);
MultidimArray<double> smallAutoCorr;
Histogram1D hist;
Matrix2D<double> U,V,temp;
Matrix1D<double> D;
MultidimArray<int> radial_count;
MultidimArray<double> radial_avg;
Matrix1D<int> center(2);
MultidimArray<int> distance;
int dim;
center.initZeros();
v0.initZeros(numDescriptors0);
v2.initZeros(numDescriptors2);
v3.initZeros(numDescriptors3);
v4.initZeros(numDescriptors4);
ROI.resizeNoCopy(Ydim,Xdim);
ROI.setXmippOrigin();
FOR_ALL_ELEMENTS_IN_ARRAY2D(ROI)
{
double temp = std::sqrt(i*i+j*j);
if ( temp < (Xdim/2))
A2D_ELEM(ROI,i,j)= 1;
else
A2D_ELEM(ROI,i,j)= 0;
}
Image<double> img;
FourierTransformer transformer(FFTW_BACKWARD);
FOR_ALL_OBJECTS_IN_METADATA(SF)
{
if (thereIsEnable)
{
int enabled;
SF.getValue(MDL_ENABLED,enabled,__iter.objId);
if ( (enabled==-1) )
{
imgno++;
continue;
}
}
img.readApplyGeo(SF,__iter.objId);
if (targetXdim!=-1 && targetXdim!=XSIZE(img()))
selfScaleToSize(LINEAR,img(),targetXdim,targetXdim,1);
MultidimArray<double> &mI=img();
mI.setXmippOrigin();
mI.statisticsAdjust(0,1);
mask.setXmippOrigin();
//The size of v1 depends on the image size and must be declared here
int numDescriptors1 = XSIZE(mI)/2; //=100;
MultidimArray<float> v1(numDescriptors1);
v1.initZeros(numDescriptors1);
double var = 1;
normalize(transformer,mI,tempI,modI,0,var,mask);
modI.setXmippOrigin();
tempI.setXmippOrigin();
nI = sign*tempI*(modI*modI);
tempM = (modI*modI);
A1D_ELEM(v0,0) = (tempM*ROI).sum();
int index = 1;
var+=2;
while (index < numNorm)
{
normalize(transformer,mI,tempI,modI,0,var,mask);
modI.setXmippOrigin();
tempI.setXmippOrigin();
nI += sign*tempI*(modI*modI);
tempM += (modI*modI);
A1D_ELEM(v0,index) = (tempM*ROI).sum();
index++;
var+=2;
}
nI /= tempM;
tempPcaAnalyzer0.addVector(v0);
nI=(nI*ROI);
auto_correlation_matrix(mI,autoCorr);
if (first)
{
radialAveragePrecomputeDistance(autoCorr, center, distance, dim);
first=false;
}
fastRadialAverage(autoCorr, distance, dim, radial_avg, radial_count);
for (int n = 0; n < numDescriptors1; ++n)
A1D_ELEM(v1,n)=(float)DIRECT_A1D_ELEM(radial_avg,n);
tempPcaAnalyzer1.addVector(v1);
#ifdef DEBUG
//String name = "000005@Images/Extracted/run_002/extra/BPV_1386.stk";
String name = "000010@Images/Extracted/run_001/extra/KLH_Dataset_I_Training_0028.stk";
//String name = "001160@Images/Extracted/run_001/DefaultFamily5";
std::cout << img.name() << std::endl;
if (img.name()==name2)
{
FileName fpName = "test_1.txt";
mI.write(fpName);
fpName = "test_2.txt";
nI.write(fpName);
fpName = "test_3.txt";
tempM.write(fpName);
fpName = "test_4.txt";
ROI.write(fpName);
//exit(1);
}
#endif
nI.binarize(0);
int im = labelImage2D(nI,nI,8);
compute_hist(nI, hist, 0, im, im+1);
size_t l;
int k,i,j;
hist.maxIndex(l,k,i,j);
A1D_ELEM(hist,j)=0;
hist.maxIndex(l,k,i,j);
nI.binarizeRange(j-1,j+1);
double x0=0,y0=0,majorAxis=0,minorAxis=0,ellipAng=0;
size_t area=0;
fitEllipse(nI,x0,y0,majorAxis,minorAxis,ellipAng,area);
A1D_ELEM(v2,0)=majorAxis/((img().xdim) );
A1D_ELEM(v2,1)=minorAxis/((img().xdim) );
A1D_ELEM(v2,2)= (fabs((img().xdim)/2-x0)+fabs((img().ydim)/2-y0))/((img().xdim)/2);
A1D_ELEM(v2,3)=area/( (double)((img().xdim)/2)*((img().ydim)/2) );
for (int n=0 ; n < numDescriptors2 ; n++)
{
if ( std::isnan(std::abs(A1D_ELEM(v2,n))))
A1D_ELEM(v2,n)=0;
}
tempPcaAnalyzer2.addVector(v2);
//mI.setXmippOrigin();
//auto_correlation_matrix(mI*ROI,autoCorr);
//auto_correlation_matrix(nI,autoCorr);
autoCorr.window(smallAutoCorr,-5,-5, 5, 5);
smallAutoCorr.copy(temp);
svdcmp(temp,U,D,V);
for (int n = 0; n < numDescriptors3; ++n)
A1D_ELEM(v3,n)=(float)VEC_ELEM(D,n); //A1D_ELEM(v3,n)=(float)VEC_ELEM(D,n)/VEC_ELEM(D,0);
tempPcaAnalyzer3.addVector(v3);
double minVal=0.;
double maxVal=0.;
mI.computeDoubleMinMax(minVal,maxVal);
compute_hist(mI, hist, minVal, maxVal, 100);
for (int n=0 ; n <= numDescriptors4-1 ; n++)
{
A1D_ELEM(v4,n)= (hist.percentil((n+1)*10));
}
tempPcaAnalyzer4.addVector(v4);
#ifdef DEBUG
if (img.name()==name1)
{
FileName fpName = "test.txt";
mI.write(fpName);
fpName = "test3.txt";
nI.write(fpName);
}
#endif
imgno++;
imgnoPCA++;
if (imgno % c == 0 && verbose>0)
progress_bar(imgno);
}
tempPcaAnalyzer0.evaluateZScore(2,20,trained);
tempPcaAnalyzer1.evaluateZScore(2,20,trained);
tempPcaAnalyzer2.evaluateZScore(2,20,trained);
tempPcaAnalyzer3.evaluateZScore(2,20,trained);
tempPcaAnalyzer4.evaluateZScore(2,20,trained);
pcaAnalyzer.push_back(tempPcaAnalyzer0);
pcaAnalyzer.push_back(tempPcaAnalyzer1);
pcaAnalyzer.push_back(tempPcaAnalyzer1);
pcaAnalyzer.push_back(tempPcaAnalyzer3);
pcaAnalyzer.push_back(tempPcaAnalyzer4);
}
void estimateModel_line(double *l, double **P, int n) {
int i;
if(n<2) {
perror("Need at least two points\n");
return;
}
double p[2] = {0, 0};
double **Q;
double **V;
double *W;
Q = malloc(n * sizeof(double *));
if(Q == NULL) { perror("out of memory\n"); exit(0); }
V = malloc(n * sizeof(double *));
if(V == NULL) { perror("out of memory\n"); exit(0); }
W = malloc(2 * sizeof(double));
if(W == NULL) { perror("out of memory\n"); exit(0); }
for(i = 0; i < n; i++)
{
Q[i] = malloc(2 * sizeof(double));
if(Q[i] == NULL) { perror("out of memory\n"); exit(0); }
V[i] = malloc(2 * sizeof(double));
if(V[i] == NULL) { perror("out of memory\n"); exit(0); }
}
// one = ones(m, 1);
// centroid of all the points
// p = (P' * one) / m;
for(i = 0; i < n; i++)
{
p[0] += P[i][0];
p[1] += P[i][1];
}
p[0] = p[0]/n;
p[1] = p[1]/n;
// matrix of centered coordinates
// Q = P - one * p';
for(i = 0; i < n; i++)
{
Q[i][0] = P[i][0] - p[0];
Q[i][1] = P[i][1] - p[1];
}
// [U Sigma V] = svd(Q);
svdcmp(Q, n, 2, W, V);
// printf("U=\n"); printMatrix(Q,n,2);
// printf("W=\n"); printVector(W,2);
// printf("V=\n"); printMatrix(V,2,2);
// the line normal is the second column of V
// n = V(:, 2);
// assemble the three line coefficients into a column vector
// l = [n ; p' * n];
l[0] = V[0][0];
l[1] = V[0][1];
l[2] = -(p[0]*l[0] + p[1]*l[1]);
// the smallest singular value of Q
// measures the residual fitting error
// residue = Sigma(2, 2);
//residue = W[1];
for(i = 0; i < n; i++)
{
free(Q[i]);
free(V[i]);
}
free(Q);
free(V);
free(W);
}
/**
* ICA function. Computes the W matrix from the
* preprocessed data.
*/
static mat ICA_compute(mat X, int rows, int cols) {
mat TXp, GWX, W, Wd, W1, D, TU, TMP;
vect d, lim;
int i, it;
FILE *OutputFile;
clock_t clock1, clock2;
float time;
//char ascii_path[512];
//strcpy(ascii_path, "/storage/sdcard0/NickGun/EEG/Log.txt");
//FILE *Log;
// matrix creation
TXp = mat_create(cols, rows);
GWX = mat_create(rows, cols);
W = mat_create(rows, rows);
Wd = mat_create(rows, rows);
D = mat_create(rows, rows);
TMP = mat_create(rows, rows);
TU = mat_create(rows, rows);
W1 = mat_create(rows, rows);
d = vect_create(rows);
// W rand init
mat_apply_fx(W, rows, rows, fx_rand, 0);
// sW <- La.svd(W)
mat_copy(W, rows, rows, Wd);
svdcmp(Wd, rows, rows, d, D);
// W <- sW$u %*% diag(1/sW$d) %*% t(sW$u) %*% W
mat_transpose(Wd, rows, rows, TU);
vect_apply_fx(d, rows, fx_inv, 0);
mat_diag(d, rows, D);
mat_mult(Wd, rows, rows, D, rows, rows, TMP);
mat_mult(TMP, rows, rows, TU, rows, rows, D);
mat_mult(D, rows, rows, W, rows, rows, Wd); // W = Wd
// W1 <- W
mat_copy(Wd, rows, rows, W1);
// lim <- rep(1000, maxit); it = 1
lim = vect_create(MAX_ITERATIONS);
for (i = 0; i < MAX_ITERATIONS; i++)
lim[i] = 1000;
it = 0;
// t(X)/p
mat_transpose(X, rows, cols, TXp);
mat_apply_fx(TXp, cols, rows, fx_div_c, cols);
while (lim[it] > TOLERANCE && it < MAX_ITERATIONS) {
// wx <- W %*% X
mat_mult(Wd, rows, rows, X, rows, cols, GWX);
// gwx <- tanh(alpha * wx)
mat_apply_fx(GWX, rows, cols, fx_tanh, 0);
// v1 <- gwx %*% t(X)/p
mat_mult(GWX, rows, cols, TXp, cols, rows, TMP); // V1 = TMP
// g.wx <- alpha * (1 - (gwx)^2)
mat_apply_fx(GWX, rows, cols, fx_1sub_sqr, 0);
// v2 <- diag(apply(g.wx, 1, FUN = mean)) %*% W
mat_mean_rows(GWX, rows, cols, d);
mat_diag(d, rows, D);
mat_mult(D, rows, rows, Wd, rows, rows, TU); // V2 = TU
// W1 <- v1 - v2
mat_sub(TMP, TU, rows, rows, W1);
// sW1 <- La.svd(W1)
mat_copy(W1, rows, rows, W);
svdcmp(W, rows, rows, d, D);
// W1 <- sW1$u %*% diag(1/sW1$d) %*% t(sW1$u) %*% W1
mat_transpose(W, rows, rows, TU);
vect_apply_fx(d, rows, fx_inv, 0);
mat_diag(d, rows, D);
mat_mult(W, rows, rows, D, rows, rows, TMP);
mat_mult(TMP, rows, rows, TU, rows, rows, D);
mat_mult(D, rows, rows, W1, rows, rows, W); // W1 = W
// lim[it + 1] <- max(Mod(Mod(diag(W1 %*% t(W))) - 1))
mat_transpose(Wd, rows, rows, TU); //chuyen vi
mat_mult(W, rows, rows, TU, rows, rows, TMP); //TMP=WxTU
lim[it + 1] = fabs(mat_max_diag(TMP, rows, rows) - 1);
if(lim[it+1]<0.1)
break;
/*
OutputFile = fopen("/storage/sdcard0/Nickgun/EEG/data/lim",
"at");
fprintf(OutputFile, "%f \n", lim[it+1]);
fclose(OutputFile);
// W <- W1
*/
mat_copy(W, rows, rows, Wd);
it++;
}
// clean up
mat_delete(TXp, cols, rows);
mat_delete(GWX, rows, cols);
mat_delete(W, rows, rows);
mat_delete(D, rows, rows);
mat_delete(TMP, rows, rows);
mat_delete(TU, rows, rows);
mat_delete(W1, rows, rows);
vect_delete(d);
return Wd;
}
/**
* Main FastICA function. Centers and whitens the input
* matrix, calls the ICA computation function ICA_compute()
* and computes the output matrixes.
*/
void fastICA(mat X, int rows, int cols, int compc, mat K, mat W, mat A, mat S) {
mat XT, V, TU, D, X1, _A;
vect scale, d;
clock_t clock1, clock2;
float time;
//char ascii_path[512];
//strcpy(ascii_path, "/storage/sdcard0/NickGun/EEG/Log.txt");
//FILE *Log;
//chu thich voi truong hop 14 kenh, 2s (256mau) du lieu, 14 thanh phan doc lap>>> cols = 14, rows = 256, compc = 14
// matrix creation
XT = mat_create(cols, rows); //14x256
X1 = mat_create(compc, rows); //14x256
V = mat_create(cols, cols); //14x14
D = mat_create(cols, cols); //14x14
TU = mat_create(cols, cols); //14x14
scale = vect_create(cols); //14
d = vect_create(cols); //14
clock1 = clock();
/*
* CENTERING
*/
mat_center(X, rows, cols, scale); //tru di gia tri trung binh cua moi cot
clock2 = clock();
time = (clock2 - clock1) / CLOCKS_PER_SEC;
//Log = fopen(ascii_path, "wb");
//fprintf(Log, "CENTERING %f \n", time);
//fclose(Log);
clock1 = clock();
/*
* WHITENING
*/
// X <- t(X); V <- X %*% t(X)/rows
mat_transpose(X, rows, cols, XT); //XT la chuyen vi cua ma tran X[256][14] >>> XT[14][256]
mat_apply_fx(X, rows, cols, fx_div_c, rows); //lay tung gia tri cua X[i][j] chia cho 14
mat_mult(XT, cols, rows, X, rows, cols, V); //V=XT*X >>>V[14][14]
// La.svd(V)
svdcmp(V, cols, cols, d, D); // V = s$u, d = s$d, D = s$v
// D <- diag(c(1/sqrt(d))
vect_apply_fx(d, cols, fx_inv_sqrt, 0);
mat_diag(d, cols, D);
// K <- D %*% t(U)
mat_transpose(V, cols, cols, TU);
mat_mult(D, cols, cols, TU, cols, cols, V); // K = V
// X1 <- K %*% X
mat_mult(V, compc, cols, XT, cols, rows, X1);
clock2 = clock();
time = (clock2 - clock1) / CLOCKS_PER_SEC;
//Log = fopen(ascii_path, "at");
//fprintf(Log, "WHITENING %f \n", time);
//fclose(Log);
clock1 = clock();
/*
* FAST ICA
*/
_A = ICA_compute(X1, compc, rows);
clock2 = clock();
time = (clock2 - clock1) / CLOCKS_PER_SEC;
//Log = fopen(ascii_path, "at");
//fprintf(Log, "FASTICA %f \n", time);
//fclose(Log);
clock1 = clock();
/*
* OUTPUT
*/
// X <- t(x)
mat_transpose(XT, cols, rows, X);
mat_decenter(X, rows, cols, scale);
// K
mat_transpose(V, compc, cols, K);
// w <- a %*% K; S <- w %*% X
mat_mult(_A, compc, compc, V, compc, cols, D);
mat_mult(D, compc, cols, XT, cols, rows, X1);
// S
mat_transpose(X1, compc, rows, S);
// A <- t(w) %*% solve(w * t(w))
mat_transpose(D, compc, compc, TU);
mat_mult(D, compc, compc, TU, compc, compc, V);
mat_inverse(V, compc, D); //ham nay tinh mat tran ngich dao
mat_mult(TU, compc, compc, D, compc, compc, V);
// A
mat_transpose(V, compc, compc, A);
// W
mat_transpose(_A, compc, compc, W);
// cleanup
mat_delete(XT, cols, rows);
mat_delete(X1, compc, rows);
mat_delete(V, cols, cols);
mat_delete(D, cols, cols);
mat_delete(TU, cols, cols);
vect_delete(scale);
vect_delete(d);
clock2 = clock();
time = (clock2 - clock1) / CLOCKS_PER_SEC;
//Log = fopen(ascii_path, "at");
//fprintf(Log, "OUTPUT %f \n", time);
//fclose(Log);
}
void DOsvd(float *a,float *res,float *comp,float *values,int nframes,int framesize,int compressedsize)
{
int usedfs;
int *remap;
int i,j;
double **da;
double **v;
double *w;
int DOFerr;
float mx;
int bestat;
if (nframes>framesize)
usedfs=nframes;
else
usedfs=framesize;
da=allocMatrix(usedfs,nframes);
v=allocMatrix(nframes,nframes);
w=allocVect(nframes);
DOFerr = 0; //false
for (i=0;i<nframes;i++)
{
for (j=0;j<framesize;j++)
da[j+1][i+1]=a[i*framesize+j];
for (;j<usedfs;j++)
da[j+1][i+1]=0.0;
}
svdcmp(da,usedfs,nframes,w,v);
remap = calloc(sizeof(int), (size_t)nframes);
for (i=0;i<nframes;i++)
remap[i]=-1;
for (j=0;j<compressedsize;j++)
{
mx=-1.0f;
for (i=0;i<nframes;i++)
{
if (remap[i]<0&&fabs(w[i+1])>mx)
{
mx=(float) fabs(w[i+1]);
bestat=i;
}
}
if(mx>0)
{
remap[bestat]=j;
}
else
{
DOFerr = 1; //true
}
}
if(DOFerr)
{
printf("Warning: To many degrees of freedom! File size may increase\n");
for (i=0;i<compressedsize;i++)
{
values[i]=0;
for (j=0;j<framesize;j++)
res[i*framesize+j]=0;
}
}
for (i=0;i<nframes;i++)
{
if (remap[i]<0)
w[i+1]=0.0;
else
{
values[remap[i]]=(float) w[i+1];
for (j=0;j<framesize;j++)
res[remap[i]*framesize+j]=(float) da[j+1][i+1];
}
}
freeVect(w);
freeMatrix(v,nframes);
freeMatrix(da,framesize);
free(remap);
}
/* ----------------------------- MNI Header -----------------------------------
@NAME : procrustes
@INPUT : npoints - number of input point pairs
ndim - number of dimensions for each point
Apoints - Matrix of point set 1 (in zero offset
form). The dimensions of this matrix should be defined
to be 1 to npoints and 1 to ndim (when calling the numerical
recipes routine matrix).
Bpoints - Matrix of point set 2 (in zero offset
form). The dimensions of this matrix should be defined
to be 1 to npoints and 1 to ndim (when calling the numerical
recipes routine matrix).
@OUTPUT : translation - zero offset vector (1 to ndim) that
specifies the translation to be applied to Bpoints to line
up the centroid with that of Apoints. Calling routine must
allocate space for this vector.
centre_of_rotation - zero offset vector (1 to ndim) that
specifies the centre of rotation and scaling (this is
in fact only the centroid of Apoints). Calling routine must
allocate space for this vector.
rotation - zero offset matrix (1 to ndim by 1 to ndim)
to rotate translated Bpoints so that they line up with
Apoints. Calling routine must allocate space for this
matrix.
scale - Scalar value giving global scaling to be applied to
translated and rotated Bpoints to match Apoints.
@RETURNS : (nothing)
@DESCRIPTION: Calculates n-dimensional linear transformation from one set
of points to another, minimizing distance between equivalent
points. Transformation from Bpoints to Apoints is calculated.
@METHOD : See Matrix Computations, Golub and Van Loan, pp. 425-426 and
paper by Sibson, Robin, J.R.Statist.Soc. B(1978), Vol. 40,
No. 2, pp 234-238.
Steps of calculations are as follows :
1) Calculate translation that aligns the centroids of the
two point sets.
2) Calculate rotation/reflexion that minimizes residual.
3) Calculate scaling of points to minimize residual.
The process can be broken into independent steps because the
best translation aligns centroids independently of the choice
of rotation/reflexion and scaling and the best rotation/reflexion
can be found independently of scale (after the best translation
has been found). (See Sibson for more).
@GLOBALS : (none)
@CALLS : calc_centroid
translate
transpose
matrix_multiply
svdcmp (zero offset)
trace
@CREATED : Long time ago (Sean Marrett)
@MODIFIED : Some time later (Shyan Ku)
Feb. 26, 1990 (Weiqian Dai)
January 30, 1992 (Peter Neelin)
- complete rewrite for roughly NIL-abiding code. Modified
name and calling parameters.
---------------------------------------------------------------------------- */
void procrustes(int npoints, int ndim,
float **Apoints, float **Bpoints,
float *translation, float *centre_of_rotation,
float **rotation, float *scale)
{
int i;
float *Atranslation, *Btranslation, *svd_W;
float **Ashift, **Bshift, **Atranspose, **Btranspose;
float **svd_U, **svd_V, **svd_VT;
float **Brotated, **product;
float trace1, trace2;
/* Get the vectors for centroids */
Atranslation=vector(1,ndim);
Btranslation=vector(1,ndim);
svd_W=vector(1,ndim);
/* Get various matrices */
Ashift=matrix(1,npoints,1,ndim);
Bshift=matrix(1,npoints,1,ndim);
Atranspose=matrix(1,ndim,1,npoints);
Btranspose=matrix(1,ndim,1,npoints);
svd_U=matrix(1,ndim,1,ndim);
svd_V=matrix(1,ndim,1,ndim);
svd_VT=matrix(1,ndim,1,ndim);
Brotated=matrix(1,npoints,1,ndim);
product=matrix(1,npoints,1,npoints);
/* Calculate the centroids, remove them from A and B points and
save the translation */
calc_centroid(npoints, ndim, Apoints, centre_of_rotation);
for (i=1; i<=ndim; i++) Atranslation[i] = -centre_of_rotation[i];
translate(npoints, ndim, Apoints, Atranslation, Ashift);
calc_centroid(npoints, ndim, Bpoints, Btranslation);
for (i=1; i<=ndim; i++) Btranslation[i] *= -1;
translate(npoints, ndim, Bpoints, Btranslation, Bshift);
for (i=1; i<=ndim; i++) translation[i] = Btranslation[i] - Atranslation[i];
/* Calculate the rotation/reflexion matrix */
transpose(npoints, ndim, Bshift, Btranspose);
matrix_multiply(ndim, npoints, ndim, Btranspose, Ashift, svd_U);
svdcmp(svd_U, ndim, ndim, svd_W, svd_V);
transpose(ndim, ndim, svd_V, svd_VT);
matrix_multiply(ndim, ndim, ndim, svd_U, svd_VT, rotation);
/* Calculate the scale */
matrix_multiply(npoints, ndim, ndim, Bshift, rotation, Brotated);
transpose(npoints, ndim, Ashift, Atranspose);
matrix_multiply(npoints, ndim, npoints, Brotated, Atranspose, product);
trace1 = trace(npoints, product);
matrix_multiply(npoints, ndim, npoints, Bshift, Btranspose, product);
trace2 = trace(npoints, product);
if (trace2 != 0.0) {
*scale = trace1 / trace2;
}
else {
*scale = 0.0;
}
/* transpose back the rotation matrix */
transpose(ndim, ndim, rotation, rotation);
/* Free vectors */
free_vector(Atranslation,1,ndim);
free_vector(Btranslation,1,ndim);
free_vector(svd_W,1,ndim);
/* Free matrices */
free_matrix(Ashift,1,npoints,1,ndim);
free_matrix(Bshift,1,npoints,1,ndim);
free_matrix(Atranspose,1,ndim,1,npoints);
free_matrix(Btranspose,1,ndim,1,npoints);
free_matrix(svd_U,1,ndim,1,ndim);
free_matrix(svd_V,1,ndim,1,ndim);
free_matrix(svd_VT,1,ndim,1,ndim);
free_matrix(Brotated,1,npoints,1,ndim);
free_matrix(product,1,npoints,1,npoints);
}
int main()
{
// Load: time, a.x, a.y, a.z from raw_data.csv
// Initialize variables
double timestamp, acceleration_x, acceleration_y, acceleration_z = 0;
int numLines = 0;
// Create file pointer
FILE *fp;
// Open stream to count lines in file
fp = fopen("raw_data.csv", "r");
int ch;
while (!feof(fp)){
ch = fgetc(fp);
if (ch == '\n'){
numLines ++;
}
}
numLines ++;
fclose(fp);
// Allocate space for array of times
// double *timeArray = malloc(sizeof(double) * numLines);
// Allocate space for array of acceleration
// double *accelerationArray = malloc(sizeof(double) * numLines * 3);
double timeArray[n];
double xyzAccelerationArray[m][n][3];
// Open stream to read the data
fp = fopen("raw_data.csv", "r");
char string[maxLength];
char* str;
int counter = 0;
int row;
int col;
while (fgets(string, maxLength, fp)) {
// Remove trailing \n
size_t ln = strlen(string) - 1;
if (string[ln] == '\n'){
string[ln] = '\0';
}
// Split the string by comma
str = _strdup(string);
char *element1 = strtok(str, ",");
char *element2 = strtok(NULL, ",");
str = _strdup(NULL);
char *element3 = strtok(str, ",");
char *element4 = strtok(NULL, ",");
// Change strings to doubles
timestamp = atof(element1);
acceleration_x = atof(element2);
acceleration_y = atof(element3);
acceleration_z = atof(element4);
// Store timestamps
timeArray[counter] = timestamp;
row = counter / n;
col = counter % n;
// Store acceleration
xyzAccelerationArray[row][col][0] = acceleration_x;
xyzAccelerationArray[row][col][1] = acceleration_y;
xyzAccelerationArray[row][col][2] = acceleration_z;
// printf("%s %s %s %s\n", element1, element2, element3, element4);
// printf("%f %f %f %f\n", timestamp, acceleration_x, acceleration_y, acceleration_z);
// printf("%s\n", string);
counter ++;
}
fclose(fp);
// End read from file
// Run Singular Variable Decomposition
double uMatrix[3][3];
double sMatrix[3][3];
svdcmp(xyzAccelerationArray, 3, numLines, uMatrix, sMatrix);
// Get U and S
Vector uVector = { uMatrix[0][0], uMatrix[0][1], uMatrix[0][2] };
Vector vVector = { uMatrix[1][0], uMatrix[1][1], uMatrix[1][2] };
Vector normalVector = { uMatrix[2][0], uMatrix[2][1], uMatrix[2][2] };
normalVector = unitVector(normalVector);
// Find intersection of vectors (0, 0, 0)
Point intersection = intersectionOfThreeVectors(uVector, vVector, normalVector);
// Create plane
Plane plane = { uVector, vVector, normalVector, intersection };
// Put points on to plane
// Find linear combination (rref)
// Return data
printf("\n");
printf("numLines: %d\n", numLines);
printf("end of file \n");
// printf("%f", timeArray[0]);
getchar();
return 0;
}
void HessianLLE::reduceDimensionality()
{
Matrix2D<int> neighboursMatrix;
Matrix2D<double> distanceNeighboursMatrix;
kNearestNeighbours(*X, kNeighbours, neighboursMatrix, distanceNeighboursMatrix);
size_t sizeY = MAT_YSIZE(*X);
size_t dp = outputDim * (outputDim+1)/2;
Matrix2D<double> weightMatrix, thisX, U, V, Vpr, Yi, Yi_complete, Yt, R, Pii;
Matrix1D<double> D, vector;
weightMatrix.initZeros(dp*sizeY,sizeY);
for(size_t index=0; index<MAT_YSIZE(*X);++index)
{
extractNearestNeighbours(*X, neighboursMatrix, index, thisX);
subtractColumnMeans(thisX);
thisX = thisX.transpose();
svdcmp(thisX, U, D, Vpr); // thisX = U * D * Vpr^t
// Copy the first columns of Vpr onto V
V.resizeNoCopy(MAT_YSIZE(Vpr),outputDim);
for (size_t y=0; y<MAT_YSIZE(V); ++y)
memcpy(&MAT_ELEM(V,y,0),&MAT_ELEM(Vpr,y,0),outputDim*sizeof(double));
//Basically, the above is applying PCA to the neighborhood of Xi.
//The PCA mapping that is found (and that is contained in V) is an
//approximation for the tangent space at Xi.
//Build Hessian estimator
buildYiHessianEstimator(V,Yi,outputDim,dp);
completeYt(V, Yi, Yt);
orthogonalizeColumnsGramSchmidt(Yt);
//Get the transpose of the last columns
size_t indexExtra = outputDim+1;
size_t Ydim = MAT_XSIZE(Yt)-indexExtra;
Pii.resizeNoCopy(Ydim,MAT_YSIZE(Yt));
FOR_ALL_ELEMENTS_IN_MATRIX2D(Pii)
MAT_ELEM(Pii,i,j) = MAT_ELEM(Yt,j,indexExtra+i);
//Double check weights sum to 1
for (size_t j=0; j<dp; j++)
{
Pii.getRow(j,vector);
double sum = vector.sum();
if(sum > 0.0001)
vector*=1.0/sum;
//Fill weight matrix
for(int k = 0; k<kNeighbours; k++){
size_t neighbourElem = MAT_ELEM(neighboursMatrix,index,k);
MAT_ELEM(weightMatrix,index*dp+j,neighbourElem) = VEC_ELEM(vector,k);
}
}
}
Matrix2D<double> G;
matrixOperation_AtA(weightMatrix,G);
Matrix1D<double> v;
eigsBetween(G,1,outputDim,v,Y);
Y*=sqrt(sizeY);
}
int bp(char *matfile, char *vecfile, char *oldbase, char *newbase)
{
int i, j=0;
int m, n = 4; /* m and n are rows and columns of matrix A */
float sum, rms;
float **A, **Acp, **At;
float **U, **Ut;
float **V, **Vt;
float **S, **St, **Si;
float *Sv;
float *b, *bT, *db, *x, *dx, *x0;
float *Utb, *SiUtb, *VSiUtb;
float **UUt, **VVt, **VtV, **US, **SVt, **USVt;
float bperp, dbperp, bpar, dbpar, btemp;
FILE *fp, *fpNew, *fpOld;
/*
* Part I: Singular Value Decomposition of matrix A
*/
/* printf("Beginning singular value decomposition of matrix A...\n");*/
/* determine number of rows 'm' */
m = get_matrix_rows(matfile,vecfile);
/* establish matrix A and vector b */
b = alloc_vector(1, m);
db = alloc_vector(1, m);
bT = alloc_vector(1, m);
x = alloc_vector(1, n);
dx = alloc_vector(1, n);
x0 = alloc_vector(1, n);
A = matrix(1, m, 1, n);
Acp = matrix(1, m, 1, n);
At = matrix(1, n, 1, m);
/* establish decomposition matrices */
U = matrix(1, m, 1, n);
Ut = matrix(1, n, 1, m);
S = matrix(1, n, 1, n);
St = matrix(1, n, 1, n);
Si = matrix(1, n, 1, n);
V = matrix(1, n, 1, n);
Vt = matrix(1, n, 1, n);
/* establish product matrices */
UUt = matrix(1, n, 1, n);
VVt = matrix(1, n, 1, n);
VtV = matrix(1, n, 1, n);
US = matrix(1, m, 1, n);
SVt = matrix(1, n, 1, n);
/* establish SVD product matrices */
USVt = matrix(1, m, 1, n);
/* vector version of diagonal matrix S */
Sv = alloc_vector(1, m);
/* vector products */
Utb = alloc_vector(1, n);
SiUtb = alloc_vector(1, n);
VSiUtb = alloc_vector(1, n);
/* read matrix and vector from input files */
fp = FOPEN(matfile,"r");
for (i = 1; i <= m; i++) {
for (j = 1; j <= n; j++) {
fscanf(fp, "%f", &A[i][j]);
}
}
fclose(fp);
fp = FOPEN(vecfile, "r");
for (i = 1; i <= m; i++)
fscanf(fp, "%f", &b[i]);
fclose(fp);
/* copy A into Acp */
for (i = 1; i <= m; i++) {
for (j = 1; j <= n; j++) {
Acp[i][j] = A[i][j];
}
}
/* transpose A into At */
for (i = 1; i <= m; i++) {
for (j = 1; j <= n; j++) {
At[j][i] = A[i][j];
}
}
/* NR fn to decompose A = U x S x Vt, where U is written into A */
svdcmp(A, m, n, Sv, V);
/* copy Sv into the diagonal of S and St */
for (i = 1; i <= 4; i++)
St[i][i] = S[i][i] = Sv[i];
/* copy A into U where it belongs, copy Acp back into A */
for (i = 1; i <= m; i++) {
for (j = 1; j <= n; j++) {
U[i][j] = A[i][j];
A[i][j] = Acp[i][j];
}
}
/* establish Ut and Vt */
for (i = 1; i <= m; i++) {
for (j = 1; j <= n; j++) {
Ut[j][i] = U[i][j];
}
}
for (i = 1; i <= n; i++) {
for (j = 1; j <= n; j++) {
Vt[j][i] = V[i][j];
}
}
/* check that SVD of A == A */
matrix_multiply(U, S, US, m, n, n);
matrix_multiply(US, Vt, USVt, m, n, n);
for (i = 1; i <= m; i++) {
for (j = 1; j <= n; j++) {
if (fabs(A[i][j] - USVt[i][j]) > 1e-12) {
/* FIXME: This check needs to be examined and reintroduced */
/* Exit(" reconstruction of A from SVD failed"); */
}
}
}
/* invert S into Si, automatically fixing small singular values */
for (i = 1; i <= n; i++) {
if (fabs(S[i][i]) < 0.0) {
Exit("svdcmp() found a negative singular value");
}
if (S[i][i] < 1e-6) {
printf(" singular value %d = %f; auto-set inverse to zero\n", i, S[i][i]);
Si[i][i] = 0.0;
}
else {
Si[i][i] = 1.0 / S[i][i];
}
}
/* breathe sigh of relief having gotten through SVD */
/* printf("\nSVD of A is ok\n\n");*/
/*
* Part II: Solve for n-vector x0
*/
/* multiply matrix Ut x vector b = vector Utb */
for (i = 1; i <= n; i++) {
for (j = 1, sum = 0.0; j <= m; j++) {
sum += Ut[i][j] * b[j];
}
Utb[i] = sum;
}
/* multiply matrix Si x vector Utb = vector SiUtb */
for (i = 1; i <= n; i++) {
SiUtb[i] = Si[i][i] * Utb[i];
}
/* multiply matrix V x vector SiUtb = vector VSiUtb */
for (i = 1; i <= n; i++) {
for (j = 1, sum = 0.0; j <= n; j++) {
sum += V[i][j] * SiUtb[j];
}
VSiUtb[i] = sum;
}
/* copy VSiUtb into x0 */
for (i = 1; i <= n; i++) {
x0[i] = VSiUtb[i];
}
/* calculate A x x0 */
for (i = 1; i <= m; i++) {
for (j = 1, sum = 0.0; j <= n; j++) {
sum += A[i][j] * x0[j];
}
bT[i] = sum;
}
/*print_vector(bT, 1, m, "b check, compare with ...");
print_vector(b, 1, m, "b");
print_vector(x0, 1, n, "x0");*/
for (i = 1, sum = 0.0; i <= m; i++) {
sum += (bT[i] - b[i])*(bT[i] - b[i]);
}
rms = sqrt(sum/(float)(m));
if (!quietflag) printf(" RMS of b-reconstructed and b = %f\n\n", rms);
/* test for sign of deltas */
fpOld = FOPEN(oldbase,"r");
fscanf(fpOld, "%f %f %f %f %f", &bperp, &dbperp, &bpar, &dbpar, &btemp);
fclose(fpOld);
printf(" New Baseline: Normal: %f, delta: %f\n"
" Parallel: %f, delta: %f\n"
" Temporal: %f days\n\n", x0[1], x0[2], x0[3], x0[4], btemp);
if (logflag) {
sprintf(logbuf," New Baseline: Normal: %f, delta: %f\n"
" Parallel: %f, delta: %f\n"
" Temporal: %f days\n\n", x0[1], x0[2], x0[3], x0[4], btemp);
printLog(logbuf);
}
fpNew = FOPEN(newbase,"w");
fprintf(fpNew, "%14.7f %14.7f %14.7f %14.7f %14.7f\n",
x0[1], x0[2], x0[3], x0[4], btemp);
fclose(fpNew);
/* free memory */
free_vector(b,1,m);
free_vector(db,1,m);
free_vector(bT,1,m);
free_vector(x,1,m);
free_vector(dx,1,m);
free_vector(x0,1,m);
free_matrix(A,1,n,1,m);
free_matrix(Acp,1,n,1,m);
free_matrix(At,1,n,1,m);
free_matrix(U,1,m,1,n);
free_matrix(Ut,1,n,1,m);
free_matrix(S,1,n,1,n);
free_matrix(St,1,n,1,n);
free_matrix(Si,1,n,1,n);
free_matrix(V,1,n,1,n);
free_matrix(Vt,1,n,1,n);
free_matrix(UUt,1,n,1,n);
free_matrix(VVt,1,n,1,n);
free_matrix(VtV,1,n,1,n);
free_matrix(US,1,n,1,n);
free_matrix(SVt,1,n,1,n);
free_matrix(USVt,1,m,1,n);
free_vector(Sv,1,m);
free_vector(Utb,1,n);
free_vector(SiUtb,1,n);
free_vector(VSiUtb,1,n);
return(0);
}
int svdfit(double *x, double *y, double *sig, int ndata, double *a, int ma,
double ***u, double ***v, double **w, double *chisq,
int (*funcs)(double, double *, int)) {
int i=0,j=0;
double wmax=0.0,tmp=0.0,thresh=0.0,sum=0.0;
double *b=NULL,*afunc=NULL;
/* Allocate memory for relevant arrays/matrices */
if ((b=darray(ndata))==NULL) {
nferrormsg("svdfit(): Cannot allocate memory to b\n\tarray of size %d",
ndata); return 0;
}
if ((afunc=darray(ma))==NULL) {
nferrormsg("svdfit(): Cannot allocate memory to afunc\n\tarray of size %d",
ma); return 0;
}
if ((*w=darray(ma))==NULL) {
nferrormsg("svdfit(): Cannot allocate memory to w\n\tarray of size %d",
ma); return 0;
}
if ((*u=dmatrix(ndata,ma))==NULL) {
nferrormsg("svdfit(): Cannot allocate memory to matrix\n\t of size %dx%d",
ndata,ma); return 0;
}
if ((*v=dmatrix(ma,ma))==NULL) {
nferrormsg("svdfit(): Cannot allocate memory to matrix\n\t of size %dx%d",
ma,ma); return 0;
}
/* Begin SVD fitting */
for (i=0; i<ndata; i++) {
if (!(*funcs)(x[i],afunc,ma)) {
nferrormsg("svdfit(): Error returned from fitting function");
return 0;
}
tmp=1.0/sig[i];
for (j=0; j<ma; j++) (*u)[i][j]=afunc[j]*tmp;
b[i]=y[i]*tmp;
}
if (!svdcmp(*u,ndata,ma,*w,*v)) {
nferrormsg("svdfit(): Error returned from svdcmp()"); return 0;
}
wmax=0.0; for (j=0; j<ma; j++) wmax=MAX(wmax,(*w)[j]); thresh=SVDFIT_TOL*wmax;
for (j=0; j<ma; j++) {
if ((*w)[j]<thresh) {
(*w)[j]=0.0;
warnmsg("svdfit(): Setting coefficient %d's singular value to zero",j);
}
}
if (!svbksb(*u,*w,*v,ndata,ma,b,a)) {
nferrormsg("svdfit(): Error returned from svbksb()"); return 0;
}
*chisq=0.0;
for (i=0; i<ndata; i++) {
if (!(*funcs)(x[i],afunc,ma)) {
nferrormsg("svdfit(): Error returned from fitting function");
return 0;
}
for (sum=0.0,j=0; j<ma; j++) sum+=a[j]*afunc[j];
*chisq+=(tmp=(y[i]-sum)/sig[i],tmp*tmp);
}
/* Clean up */
free(b);
free(afunc);
return 1;
}
