static void update_prob( m_elem **P_pre, m_elem **R, m_elem **H,
m_elem **P_post, m_elem **K )
{
#ifdef PRINT_DEBUG
printf( "ekf: updating prob\n" );
#endif
#ifdef DIV_DEBUG
print_matrix( "P", P_pre, state_size, state_size );
#endif
mat_mult( H, P_pre, temp_meas_state,
measurement_size, state_size, state_size );
mat_mult_transpose( H, temp_meas_state, temp_meas_meas,
measurement_size, state_size, measurement_size );
mat_add( temp_meas_meas, R, temp_meas_meas,
measurement_size, measurement_size );
take_inverse( temp_meas_meas, temp_meas_2, measurement_size );
#ifdef DIV_DEBUG
print_matrix( "1 / (HPH + R)", temp_meas_2,
measurement_size, measurement_size );
#endif
mat_transpose_mult( temp_meas_state, temp_meas_2, K,
state_size, measurement_size, measurement_size );
/*
print_matrix( "Kalman Gain", K, state_size, measurement_size );
*/
mat_mult( K, temp_meas_state, temp_state_state,
state_size, measurement_size, state_size );
#ifdef PRINT_DEBUG
printf( "ekf: updating prob 3\n" );
#endif
mat_add( temp_state_state, P_pre, P_post, state_size, state_size );
}
void Projector::_draw ()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
GLsizei W_ = static_cast<GLsizei>(W),
h_ = static_cast<GLsizei>(h);
float trail_time = 0.0f;
if (trail) {
if (trailing) trail->add_point(animator->theta, animator->time);
if (not trail_paused) trail_time = animator->time;
}
if (in_stereo) {
//right eye
glViewport(0,0,W_,h_);
mat_mult(tilt, animator->twist_theta(-TWIST_ANGLE), temp1);
drawing->reproject(temp1);
drawing->display();
if (trail) trail->display(temp1, trail_time);
//left eye
glViewport(W_,0,W_,h_);
mat_mult(tilt, animator->twist_theta(TWIST_ANGLE), temp1);
drawing->reproject(temp1);
drawing->display();
if (trail) trail->display(temp1, trail_time);
} else {
glViewport(0,0,W_,h_);
mat_mult(tilt, animator->theta, temp1);
drawing->reproject(temp1);
drawing->display();
if (trail) trail->display(temp1, trail_time);
}
}
/***************************************************
* covariance *
* -- *
* Determines the mean shape for the shape space. *
* Calculates teh covariance matrix using: *
* 1 *
* cov = ----- = sum(dx[i] * dx[i]') *
* (N-1) *
* Where dx[i] = x[i] - mean_x *
* If bias is one then divide by N instead of N-1 *
***************************************************/
double *covariance(double **shapes, double *shape_space_mean, int num_landmarks, int num_shapes, int bias) {
int i, j, k;
double *shape = (double *) malloc(sizeof(double) * num_landmarks * 2);
double *covariance_matrix = (double *) malloc(sizeof(double) * 4 * num_landmarks * num_landmarks);
double *temp_matrix = (double *) malloc(sizeof(double) * 4 * num_landmarks * num_landmarks);
//find mean shape
for (i = 0; i < num_shapes; ++i) {
for (j = 0; j < num_landmarks; ++j) {
shape_space_mean[j*2] += (shapes[i][j*2] / num_shapes);
shape_space_mean[(j*2)+1] += (shapes[i][(j*2)+1] / num_shapes);
}
}
// 1
//cov = ----- sum(dx*dx')
// (N-1)
//subtract out the mean
for (j = 0; j < (num_landmarks*2); ++j) {
shape[j] = shapes[0][j] - shape_space_mean[j];
}
//multiply dx*dx'
//shape = shape' the way we have it set up, just indexed differently
mat_mult(shape, num_landmarks*2, shape, 1, num_landmarks*2, covariance_matrix);
for (i = 1; i < num_shapes; ++i) {
//subtract out the mean
for (j = 0; j < num_landmarks*2; ++j) {
shape[j] = shapes[i][j] - shape_space_mean[j];
}
//multiply dx*dx'
//shape = hape' the way we have it set up, just indexed differently
mat_mult(shape, num_landmarks*2, shape, 1, num_landmarks*2, temp_matrix);
//sum into the existing covariance
for (j = 0; j < (num_landmarks*num_landmarks*4); ++j) {
covariance_matrix[j] += temp_matrix[j];
}
}
//divide by N or N-1 depending
for (i = 0; i < (num_landmarks*num_landmarks*2); ++i) {
covariance_matrix[i] /= (num_shapes - ((bias) ? 0 : 1));
}
free_array((void *)shape);
free_array((void *)temp_matrix);
return covariance_matrix;
}
void icaTransformInverse(double** S, int rows, int cols, int comp, int comp_rm,
double** X, double** K, double** W, double** RowData, int startEb,
int stopEb) {
double **A;
double *Total;
FILE * OutputFile;
int i, j;
A = mat_create(comp, comp);
Total = malloc(cols * sizeof(double));
for (j = 0; j < cols; j++) {
Total[j] = 0;
for (i = 0; i < rows; i++)
Total[j] = Total[j] + RowData[i][j];
Total[j] = Total[j] / rows;
}
puts("Before");
for (j = startEb; j < stopEb; j++)
//for (j = 40; j < 90; j++)
S[j][comp_rm] = 0;//////////////////////////////////////////////////////////////////////////////////
puts("Debug");
mat_mult(K, cols, comp, W, comp, comp, A); //A[cols, comp]
mat_inverse(A, comp, W);
mat_mult(S, rows, comp, W, comp, comp, X);
//strcat(pPathFile, "_recontruction");
OutputFile = fopen("/home/truongnh/eeg-lab411/SignalProcessing/RemoveEyeblink/Data/Output.txt",
"wb");
for (i = 0; i < rows; i++) {
for (j = 0; j < cols; j++) {
if(j == comp_rm)
fprintf(OutputFile, "%f\n", X[i][j] + Total[j]);
}
//fprintf(OutputFile, "\n");
}
fclose(OutputFile);
for (j = 0; j < cols; j++) {
for (i = 0; i < rows; i++)
X[i][j] = X[i][j] + Total[j];
}
free(Total);
}
void kont_env_maping(K_EDITOR * p_cnf, EDIT_KONTEJNER * p_kont)
{
GLMATRIX m;
int mod = p_cnf->mod;
int o, v;
float tx, ty, tz;
float m11, m21, m31, m12, m22, m32;
mat_mult(p_cnf->p_word, p_cnf->p_camera, &m);
m11 = m._11;
m21 = m._21;
m31 = m._31;
m12 = m._12;
m22 = m._22;
m32 = m._32;
for (o = 0; o < p_kont->max_objektu; o++) {
if (!p_kont->p_obj[o])
continue;
for (v = 0; v < p_kont->p_obj[o]->vertexnum; v++) {
tx = p_kont->p_obj[o]->p_vertex[v].nx;
ty = p_kont->p_obj[o]->p_vertex[v].ny;
tz = p_kont->p_obj[o]->p_vertex[v].nz;
p_kont->p_obj[o]->p_vertex[v].tu2 = 0.5f * (1.0f + (tx * m11 + ty * m21 + tz * m31)); // x bez posunu
p_kont->p_obj[o]->p_vertex[v].tv2 = 0.5f * (1.0f + (tx * m12 + ty * m22 + tz * m32)); // y bez posunu
}
}
}
void translate_matrix(struct matrix *mat, float x, float y, float z)
{
struct matrix translator, temp;
memcpy(&temp, mat, sizeof(temp));
translator.elements[0][0] = 1.0;
translator.elements[1][0] = 0.0;
translator.elements[2][0] = 0.0;
translator.elements[3][0] = x;
translator.elements[0][1] = 0.0;
translator.elements[1][1] = 1.0;
translator.elements[2][1] = 0.0;
translator.elements[3][1] = y;
translator.elements[0][2] = 0.0;
translator.elements[1][2] = 0.0;
translator.elements[2][2] = 1.0;
translator.elements[3][2] = z;
translator.elements[0][3] = 0.0;
translator.elements[1][3] = 0.0;
translator.elements[2][3] = 0.0;
translator.elements[3][3] = 1.0;
mat_mult(mat, &translator, &temp);
}
/* Polar Decomposition of 3x3 matrix in 4x4,
* M = QS. See Nicholas Higham and Robert S. Schreiber,
* Fast Polar Decomposition of An Arbitrary Matrix,
* Technical Report 88-942, October 1988,
* Department of Computer Science, Cornell University.
*/
double polar_decomp(HMatrix M, HMatrix Q, HMatrix S)
{
#define TOL 1.0e-6
HMatrix Mk, MadjTk, Ek;
double det, M_one, M_inf, MadjT_one, MadjT_inf, E_one, gamma, g1, g2;
int i, j;
mat_tpose(Mk,=,M,3);
M_one = norm_one(Mk); M_inf = norm_inf(Mk);
do {
adjoint_transpose(Mk, MadjTk);
det = vdot(Mk[0], MadjTk[0]);
if (det==0.0) {do_rank2(Mk, MadjTk, Mk); break;}
MadjT_one = norm_one(MadjTk); MadjT_inf = norm_inf(MadjTk);
gamma = sqrt(sqrt((MadjT_one*MadjT_inf)/(M_one*M_inf))/fabs(det));
g1 = gamma*0.5;
g2 = 0.5/(gamma*det);
mat_copy(Ek,=,Mk,3);
mat_binop(Mk,=,g1*Mk,+,g2*MadjTk,3);
mat_copy(Ek,-=,Mk,3);
E_one = norm_one(Ek);
M_one = norm_one(Mk); M_inf = norm_inf(Mk);
} while (E_one>(M_one*TOL));
mat_tpose(Q,=,Mk,3); mat_pad(Q);
mat_mult(Mk, M, S); mat_pad(S);
for (i=0; i<3; i++) for (j=i; j<3; j++)
S[i][j] = S[j][i] = 0.5*(S[i][j]+S[j][i]);
return (det);
}
/* Rotate a matrix about the X, Y or Z axis */
void mat_rotate (Matrix mat1, Matrix mat2, int axis, float angle)
{
Matrix mat;
float cosa, sina;
cosa = cos ((PI/180.0) * angle);
sina = sin ((PI/180.0) * angle);
mat_identity (mat);
switch (axis) {
case X:
mat[1][1] = cosa;
mat[1][2] = sina;
mat[2][1] = -sina;
mat[2][2] = cosa;
break;
case Y:
mat[0][0] = cosa;
mat[0][2] = -sina;
mat[2][0] = sina;
mat[2][2] = cosa;
break;
case Z:
mat[0][0] = cosa;
mat[0][1] = sina;
mat[1][0] = -sina;
mat[1][1] = cosa;
break;
}
mat_mult (mat1, mat2, mat);
}
void mat_axis_rotate (Matrix mat1, Matrix mat2, Vector axis, float angle)
{
float cosa, sina;
Matrix mat;
cosa = cos ((PI/180.0) * angle);
sina = sin ((PI/180.0) * angle);
mat[0][0] = (axis[X] * axis[X]) + ((1.0 - (axis[X] * axis[X]))*cosa);
mat[0][1] = (axis[X] * axis[Y] * (1.0 - cosa)) - (axis[Z] * sina);
mat[0][2] = (axis[X] * axis[Z] * (1.0 - cosa)) + (axis[Y] * sina);
mat[0][3] = 0.0;
mat[1][0] = (axis[X] * axis[Y] * (1.0 - cosa)) + (axis[Z] * sina);
mat[1][1] = (axis[Y] * axis[Y]) + ((1.0 - (axis[Y] * axis[Y])) * cosa);
mat[1][2] = (axis[Y] * axis[Z] * (1.0 - cosa)) - (axis[X] * sina);
mat[1][3] = 0.0;
mat[2][0] = (axis[X] * axis[Z] * (1.0 - cosa)) - (axis[Y] * sina);
mat[2][1] = (axis[Y] * axis[Z] * (1.0 - cosa)) + (axis[X] * sina);
mat[2][2] = (axis[Z] * axis[Z]) + ((1.0 - (axis[Z] * axis[Z])) * cosa);
mat[2][3] = 0.0;
mat[3][0] = mat[3][1] = mat[3][2] = mat[3][3] = 0.0;
mat_mult (mat1, mat2, mat);
}
void rotate(t_env *e)
{
cl_float3 mat[3];
ident(e->data->mat);
rot_mat(mat, 0, e->rot.x);
mat_mult(e->data->mat, mat);
rot_mat(mat, 1, e->rot.y);
mat_mult(e->data->mat, mat);
rot_mat(mat, 2, e->rot.z);
mat_mult(e->data->mat, mat);
e->data->dir = v_mult_mat(e->data->mat, float3(0, 0, 1));
e->data->corners[0] = v_mult_mat(e->data->mat, e->corners[0]);
e->data->corners[1] = v_mult_mat(e->data->mat, e->corners[1]);
e->data->corners[2] = v_mult_mat(e->data->mat, e->corners[2]);
transpose(e->data->mat);
}
void ekf_filter_predict(struct ekf_filter* filter, double *u) {
/* prediction :
X += Xdot * dt
Pdot = F * P * F' + Q ( or Pdot = F*P + P*F' + Q for continuous form )
P += Pdot * dt
*/
int n = filter->state_dim;
double dt;
/* fetch dt, Xdot and F */
filter->ffun(u, filter->X, &dt, filter->Xdot, filter->F);
/* X = X + Xdot * dt */
mat_add_scal_mult(n, 1, filter->X, filter->X, dt, filter->Xdot);
#ifdef EKF_UPDATE_CONTINUOUS
/*
continuous update
Pdot = F * P + P * F' + Q
*/
mat_mult(n, n, n, filter->tmp1, filter->F, filter->P);
mat_transpose(n, n, filter->tmp2, filter->F);
mat_mult(n, n, n, filter->tmp3, filter->P, filter->tmp2);
mat_add(n, n, filter->Pdot, filter->tmp1, filter->tmp3);
mat_add(n, n, filter->Pdot, filter->Pdot, filter->Q);
#endif
#ifdef EKF_UPDATE_DISCRETE
/*
discrete update
Pdot = F * P * F' + Q
*/
mat_mult(n, n, n, filter->tmp1, filter->F, filter->P);
mat_transpose(n, n, filter->tmp2, filter->F);
mat_mult(n, n, n, filter->tmp3, filter->tmp1, filter->tmp2);
mat_add(n, n, filter->Pdot, filter->tmp3, filter->Q);
#endif
/* P = P + Pdot * dt */
mat_add_scal_mult(n, n, filter->P, filter->P, dt, filter->Pdot);
}
mat_t *mat_set_view(vec_t eye, vec_t u, vec_t v, vec_t w, mat_t *a){
mat_t *b = mat_set_trans(vec_neg(eye),mat_new_zero());
mat_set_row(0,u,a);
mat_set_row(1,v,a);
mat_set_row(2,w,a);
mat_set_row(3,vec_new(0,0,0,1),a);
mat_set_col(3,vec_new(0,0,0,1),a);
mat_mult(a,b);
mat_free(b);
return a;
}
mat_t *mat_set_ortho(vec_t lbn, vec_t rtf, mat_t *a){
mat_t *b = mat_set_id(mat_new_zero());
b->x.w = -(lbn.x+rtf.x)/2.0;
b->y.w = -(lbn.y+rtf.y)/2.0;
b->z.w = -(lbn.z+rtf.z)/2.0;
mat_set_id(a);
a->x.x = 2.0/(rtf.x-lbn.x);
a->y.y = 2.0/(rtf.y-lbn.y);
a->z.z = 2.0/(rtf.z-lbn.z);
mat_mult(a,b);
mat_free(b);
return a;
}
static void estimate_prob( m_elem **P_post, m_elem **Phi, m_elem **GQGt,
m_elem **P_pre )
{
#ifdef PRINT_DEBUG
printf( "ekf: estimating prob\n" );
#endif
mat_mult_transpose( P_post, Phi, temp_state_state,
state_size, state_size, state_size );
mat_mult( Phi, temp_state_state, P_pre,
state_size, state_size, state_size );
mat_add( P_pre, GQGt, P_pre, state_size, state_size );
}
void BaseStateEstimation::transformImuStateToBase()
{
MY_MATRIX(S_angrate, 1,3,1,3);
vectorToSkewMatrix(O_angrate_I_, S_angrate);
MY_MATRIX(S_angacc, 1,3,1,3);
vectorToSkewMatrix(O_angacc_I_, S_angacc);
MY_MATRIX(S_helper, 1,3,1,3);
mat_mult(S_angrate, S_angrate, S_helper);
mat_add(S_angacc, S_helper, S_helper);
mat_mult(O_rot_I_, I_rot_B_, O_rot_B_);
mat_vec_mult(O_rot_I_, I_linpos_B_, O_linpos_B_);
vec_add(O_linpos_I_, O_linpos_B_, O_linpos_B_);
mat_vec_mult(O_rot_I_, I_linpos_B_, O_linvel_B_);
mat_vec_mult(S_angrate, O_linvel_B_, O_linvel_B_);
vec_add(O_linvel_I_, O_linvel_B_, O_linvel_B_);
mat_vec_mult(O_rot_I_, I_linpos_B_, O_linacc_B_);
mat_vec_mult(S_helper, O_linacc_B_, O_linacc_B_);
vec_add(O_linacc_I_, O_linacc_B_, O_linacc_B_);
}
void ekf_filter_update(struct ekf_filter* filter, double *y) {
/* update
E = H * P * H' + R
K = P * H' * inv(E)
P = P - K * H * P
X = X + K * err
*/
double err;
int n = filter->state_dim;
int m = filter->measure_dim;
filter->mfun(y, &err, filter->X, filter->H);
/* E = H * P * H' + R */
mat_mult(m, n, n, filter->tmp1, filter->H, filter->P);
mat_transpose(m, n, filter->tmp2, filter->H);
mat_mult(m, n, m, filter->tmp3, filter->tmp1, filter->tmp2);
mat_add(m, m, filter->E, filter->tmp3, filter->R);
/* K = P * H' * inv(E) */
mat_transpose(m, n, filter->tmp1, filter->H);
mat_mult(n, n, m, filter->tmp2, filter->P, filter->tmp1);
if (filter->measure_dim != 1) { printf("only dim 1 measure implemented for now\n"); exit(-1);}
mat_scal_mult(n, m, filter->K, 1./filter->E[0], filter->tmp2);
/* P = P - K * H * P */
mat_mult(n, m, n, filter->tmp1, filter->K, filter->H);
mat_mult(n, n, n, filter->tmp2, filter->tmp1, filter->P);
mat_sub(n, n, filter->P, filter->P, filter->tmp2);
/* X = X + err * K */
mat_add_scal_mult(n, m, filter->X, filter->X, err, filter->K);
}
void Ball::rotate_vector(float *x1, float *y1, float angle)
{
angle *= (3.1415*2)/360;
float v[4];
v[0] = *x1;
v[1] = *y1;
v[2] = 0;
v[3] = 0;
float m[16];
memset(m, 0, sizeof(m));
m[0] = cos(angle);
m[1] = -sin(angle);
m[4] = sin(angle);
m[5] = cos(angle);
m[10] = 1;
m[15] = 1;
mat_mult(m, v);
*x1 = v[0];
*y1 = v[1];
}
void rotate_matrix(struct matrix *mat, float angle, float x, float y, float z)
{
float s, c;
angle = 2.0f * M_PI * angle / 360.0f;
s = sinf(angle);
c = cosf(angle);
struct matrix rotator, temp;
memcpy(&temp, mat, sizeof(temp));
memset(rotator.elements, 0, 16 * sizeof(GLfloat));
rotator.elements[0][0] = x * x * (1 - c) + c;
rotator.elements[1][0] = y * x * (1 - c) + z * s;
rotator.elements[2][0] = x * z * (1 - c) - y * s;
rotator.elements[3][0] = 0;
rotator.elements[0][1] = x * y * (1 - c) - z * s;
rotator.elements[1][1] = y * y * (1 - c) + c;
rotator.elements[2][1] = y * z * (1 - c) + x * s;
rotator.elements[3][1] = 0;
rotator.elements[0][2] = x * z * (1 - c) + y * s;
rotator.elements[1][2] = y * z * (1 - c) - x * s;
rotator.elements[2][2] = z * z * (1 - c) + c;
rotator.elements[3][2] = 0;
rotator.elements[0][3] = 0.0;
rotator.elements[1][3] = 0.0;
rotator.elements[2][3] = 0.0;
rotator.elements[3][3] = 1.0;
mat_mult(mat, &rotator, &temp);
}
/**
* 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);
}
void expm(Matrix A, Matrix B, double h) {
static int firsttime = TRUE;
static Matrix M;
static Matrix M2;
static Matrix M3;
static Matrix D;
static Matrix N;
if (firsttime) {
firsttime = FALSE;
M = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
M2 = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
M3 = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
D = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
N = my_matrix(1,3*N_DOFS,1,3*N_DOFS);
}
int norm = 0;
double c = 0.5;
int q = 6; // uneven p-q order for Pade approx.
int p = 1;
int i,j,k;
// Form the big matrix
mat_mult_scalar(A, h, A);
mat_equal_size(A, 2*N_DOFS, 2*N_DOFS, M);
for (i = 1; i <= 2*N_DOFS; ++i) {
for (j = 1; j <= N_DOFS; ++j) {
M[i][2*N_DOFS + j] = h * B[i][j];
}
}
//print_mat("M is:\n", M);
// scale M by power of 2 so that its norm < 1/2
norm = (int)(log2(inf_norm(M))) + 2;
//printf("Inf norm of M: %f \n", inf_norm(M));
if (norm < 0)
norm = 0;
mat_mult_scalar(M, 1/pow(2,(double)norm), M);
//printf("Norm of M logged, floored and 2 added is: %d\n", norm);
//print_mat("M after scaling is:\n", M);
for (i = 1; i <= 3*N_DOFS; i++) {
for (j = 1; j <= 3*N_DOFS; j++) {
N[i][j] = c * M[i][j];
D[i][j] = -c * M[i][j];
}
N[i][i] = N[i][i] + 1.0;
D[i][i] = D[i][i] + 1.0;
}
// set M2 equal to M
mat_equal(M, M2);
// start pade approximation
for (k = 2; k <= q; k++) {
c = c * (q - k + 1) / (double)(k * (2*q - k + 1));
mat_mult(M,M2,M2);
mat_mult_scalar(M2,c,M3);
mat_add(N,M3,N);
if (p == 1)
mat_add(D,M3,D);
else
mat_sub(D,M3,D);
p = -1 * p;
}
// multiply denominator with nominator i.e. D\E
my_inv_ludcmp(D, 3*N_DOFS, D);
mat_mult(D,N,N);
// undo scaling by repeated squaring
for (k = 1; k <= norm; k++)
mat_mult(N,N,N);
// get the matrices out
// read off the entries
for (i = 1; i <= 2*N_DOFS; i++) {
for (j = 1; j <= 2*N_DOFS; j++) {
A[i][j] = N[i][j];
}
for (j = 1; j <= N_DOFS; j++) {
B[i][j] = N[i][2*N_DOFS + j];
}
}
}
void
test_constraint(void)
{
int i,j,n,m;
static Matrix Jbig;
static Matrix dJbigdt;
static Vector dsbigdt;
static Matrix JbigMinvJbigt;
static Matrix JbigMinvJbigtinv;
static Matrix Minv;
static Matrix MinvJbigt;
static Matrix Jbigt;
static Vector dJbigdtdsbigdt;
static Matrix Jbart;
static Matrix Jbar;
static Vector lv;
static Vector lambda;
static Vector f;
static Matrix R;
static Vector v;
static int firsttime = TRUE;
int debug_print = TRUE;
if (firsttime) {
firsttime = FALSE;
Jbig = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6);
Jbigt = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6);
dJbigdt = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6);
JbigMinvJbigt = my_matrix(1,N_ENDEFFS*6,1,N_ENDEFFS*6);
JbigMinvJbigtinv = my_matrix(1,N_ENDEFFS*6,1,N_ENDEFFS*6);
Minv = my_matrix(1,N_DOFS+6,1,N_DOFS+6);
MinvJbigt = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6);
dsbigdt = my_vector(1,N_DOFS+6);
dJbigdtdsbigdt = my_vector(1,N_ENDEFFS*6);
Jbar = my_matrix(1,N_DOFS+6,1,N_ENDEFFS*6);
Jbart = my_matrix(1,N_ENDEFFS*6,1,N_DOFS+6);
lv = my_vector(1,N_ENDEFFS*6);
lambda = my_vector(1,N_ENDEFFS*6);
f = my_vector(1,N_DOFS+6);
R = my_matrix(1,N_CART,1,N_CART);
v = my_vector(1,N_CART);
// the base part of Jbig is just the identity matrix for both constraints
for (i=1; i<=6; ++i)
Jbig[i][N_DOFS+i] = 1.0;
for (i=1; i<=6; ++i)
Jbig[i+6][N_DOFS+i] = 1.0;
}
// build the Jacobian including the base -- just copy J into Jbig
mat_equal_size(J,N_ENDEFFS*6,N_DOFS,Jbig);
mat_trans(Jbig,Jbigt);
// build the Jacobian derivative
mat_equal_size(dJdt,N_ENDEFFS*6,N_DOFS,dJbigdt);
// build the augmented state derivate vector
for (i=1; i<=N_DOFS; ++i)
dsbigdt[i] = joint_state[i].thd;
for (i=1; i<=N_CART; ++i)
dsbigdt[N_DOFS+i] = base_state.xd[i];
for (i=1; i<=N_CART; ++i)
dsbigdt[N_DOFS+N_CART+i] = base_orient.ad[i];
// compute J-bar transpose
my_inv_ludcmp(rbdInertiaMatrix,N_DOFS+6, Minv);
mat_mult(Minv,Jbigt,MinvJbigt);
mat_mult(Jbig,MinvJbigt,JbigMinvJbigt);
my_inv_ludcmp(JbigMinvJbigt,N_ENDEFFS*6,JbigMinvJbigtinv);
mat_mult(MinvJbigt,JbigMinvJbigtinv,Jbar);
mat_trans(Jbar,Jbart);
// compute C+G-u
for (i=1; i<=N_DOFS; ++i)
f[i] = -joint_state[i].u;
for (i=1; i<=6; ++i)
f[N_DOFS+i] = 0.0;
vec_add(f,rbdCplusGVector,f);
// compute first part of lambda
mat_vec_mult(Jbart,f,lambda);
// compute second part of lambda
mat_vec_mult(dJbigdt,dsbigdt,dJbigdtdsbigdt);
mat_vec_mult(JbigMinvJbigtinv,dJbigdtdsbigdt,lv);
// compute final lambda
vec_sub(lambda,lv,lambda);
// ----------------------------------------------------------------------
// express lambda in the same coordinate at the foot sensors
/* rotation matrix from world to L_AAA coordinates:
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
mat_trans_size(Alink[L_IN_HEEL],N_CART,N_CART,R);
// transform forces
for (i=1; i<=N_CART; ++i)
v[i] = lambda[N_CART*2+i];
mat_vec_mult(R,v,v);
if (debug_print) {
printf("LEFT Force: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[L_CFx]);
printf("LEFT Force: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[L_CFy]);
printf("LEFT Force: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[L_CFz]);
}
misc_sensor[L_CFx] = v[1];
misc_sensor[L_CFy] = v[2];
misc_sensor[L_CFz] = v[3];
// transform torques
for (i=1; i<=N_CART; ++i)
v[i] = lambda[N_CART*2+N_CART+i];
mat_vec_mult(R,v,v);
if (debug_print) {
printf("LEFT Torque: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[L_CTa]);
printf("LEFT Torque: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[L_CTb]);
printf("LEFT Torque: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[L_CTg]);
}
misc_sensor[L_CTa] = v[1];
misc_sensor[L_CTb] = v[2];
misc_sensor[L_CTg] = v[3];
/* rotation matrix from world to R_AAA coordinates :
we can borrow this matrix from the toes, which have the same
rotation, but just a different offset vector, which is not
needed here */
mat_trans_size(Alink[R_IN_HEEL],N_CART,N_CART,R);
// transform forces
for (i=1; i<=N_CART; ++i)
v[i] = lambda[i];
mat_vec_mult(R,v,v);
if (debug_print) {
printf("RIGHT Force: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[R_CFx]);
printf("RIGHT Force: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[R_CFy]);
printf("RIGHT Force: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[R_CFz]);
}
misc_sensor[R_CFx] = v[1];
misc_sensor[R_CFy] = v[2];
misc_sensor[R_CFz] = v[3];
// transform torques
for (i=1; i<=N_CART; ++i)
v[i] = lambda[N_CART+i];
mat_vec_mult(R,v,v);
if (debug_print) {
printf("RIGHT Torque: lx=% 7.5f sx=% 7.5f\n",v[1],misc_sim_sensor[R_CTa]);
printf("RIGHT Torque: ly=% 7.5f sy=% 7.5f\n",v[2],misc_sim_sensor[R_CTb]);
printf("RIGHT Torque: lz=% 7.5f sz=% 7.5f\n",v[3],misc_sim_sensor[R_CTg]);
}
misc_sensor[R_CTa] = v[1];
misc_sensor[R_CTb] = v[2];
misc_sensor[R_CTg] = v[3];
if (debug_print)
getchar();
}
/**
* 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);
}
/**
* 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;
}
/*!*****************************************************************************
*******************************************************************************
\note parm_opt
\date 10/20/91
\remarks
this is the major optimzation program
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] tol : error tolernance to be achieved
\param[in] n_parm : number of parameters to be optimzed
\param[in] n_con : number of contraints to be taken into account
\param[in] f_dLda : function which calculates the derivative of the
optimziation criterion with respect to the parameters;
must return vector
\param[in] f_dMda : function which calculates the derivate of the
constraints with respect to parameters
must return matrix
\param[in] f_M : constraints function, must always be formulted to
return 0 for properly fulfilled constraints
\param[in] f_L : function to calculate simple cost (i.e., constraint
cost NOT included), the constraint costs are added
by this program automatically, the function returns
a double scalar
\param[in] f_dLdada : second derivative of L with respect to the parameters,
must be a matrix of dim n_parm x n_parm
\param[in] f_dMdada : second derivative of M with respect to parameters,
must be a matrix n_con*n_parm x n_parm
\param[in] use_newton: TRUE or FALSE to indicate that second derivatives
are given and can be used for Newton algorithm
\param[in,out] a : initial setting of parameters and also return of
optimal value (must be a vector, even if scalar)
\param[out] final_cost: the final cost
\param[out] err : the sqrt of the squared error of all constraints
NOTE: - program returns TRUE if everything correct, otherwise FALSE
- always minimizes the cost!!!
- algorithms come from Dyer McReynolds
NOTE: besides the possiblity of a bug, the Newton method seems to
sacrifice the validity of the constraint a little up to
quite a bit and should be used prudently
******************************************************************************/
int
parm_opt(double *a,int n_parm, int n_con, double *tol, void (*f_dLda)(),
void (*f_dMda)(), void (*f_M)(), double (*f_L)(), void (*f_dMdada)(),
void (*f_dLdada)(), int use_newton, double *final_cost, double *err)
{
register int i,j,n;
double cost= 999.e30;
double last_cost = 0.0;
double *mult=NULL, *new_mult=NULL; /* this is the vector of Lagrange mulitplier */
double **dMda=NULL, **dMda_t=NULL;
double *dLda;
double *K=NULL; /* the error in the constraints */
double eps = 0.025; /* the learning rate */
double **aux_mat=NULL; /* needed for inversion of matrix */
double *aux_vec=NULL;
double *new_a;
double **dMdada=NULL;
double **dLdada=NULL;
double **A=NULL; /* big matrix, a combination of several other matrices */
double *B=NULL; /* a big vector */
double **A_inv=NULL;
int rc=TRUE;
long count = 0;
int last_sign = 1;
int pending1 = FALSE, pending2 = FALSE;
int firsttime = TRUE;
int newton_active = FALSE;
dLda = my_vector(1,n_parm);
new_a = my_vector(1,n_parm);
if (n_con > 0) {
mult = my_vector(1,n_con);
dMda = my_matrix(1,n_con,1,n_parm);
dMda_t = my_matrix(1,n_parm,1,n_con);
K = my_vector(1,n_con);
aux_mat = my_matrix(1,n_con,1,n_con);
aux_vec = my_vector(1,n_con);
}
if (use_newton) {
dLdada = my_matrix(1,n_parm,1,n_parm);
A = my_matrix(1,n_parm+n_con,1,n_parm+n_con);
A_inv = my_matrix(1,n_parm+n_con,1,n_parm+n_con);
B = my_vector(1,n_parm+n_con);
if (n_con > 0) {
dMdada = my_matrix(1,n_con*n_parm,1,n_parm);
new_mult = my_vector(1,n_con);
}
for (i=1+n_parm; i<=n_con+n_parm; ++i) {
for (j=1+n_parm; j<=n_con+n_parm; ++j) {
A[i][j] = 0.0;
}
}
}
while (fabs(cost-last_cost) > *tol) {
++count;
pending1 = FALSE;
pending2 = FALSE;
AGAIN:
/* calculate the current Lagrange multipliers */
if (n_con > 0) {
(*f_M)(a,K); /* takes the parameters, returns residuals */
(*f_dMda)(a,dMda); /* takes the parameters, returns the Jacobian */
}
(*f_dLda)(a,dLda); /* takes the parameters, returns the gradient */
if (n_con > 0) {
mat_trans(dMda,dMda_t);
}
if (newton_active) {
if (n_con > 0) {
(*f_dMdada)(a,dMdada);
}
(*f_dLdada)(a,dLdada);
}
/* the first step is always a gradient step */
if (newton_active) {
if (firsttime) {
firsttime = FALSE;
eps = 0.1;
}
/* build the A matrix */
for (i=1; i<=n_parm; ++i) {
for (j=1; j<=n_parm; ++j) {
A[i][j] = dLdada[i][j];
for (n=1; n<=n_con; ++n) {
A[i][j] += mult[n]*dMdada[n+(i-1)*n_con][j];
}
}
}
for (i=1+n_parm; i<=n_con+n_parm; ++i) {
for (j=1; j<=n_parm; ++j) {
A[j][i] = A[i][j] = dMda[i-n_parm][j];
}
}
/* build the B vector */
if (n_con > 0) {
mat_vec_mult(dMda_t,mult,B);
}
for (i=1; i<=n_con; ++i) {
B[i+n_parm] = K[i];
}
/* invert the A matrix */
if (!my_inv_ludcmp(A, n_con+n_parm, A_inv)) {
rc = FALSE;
break;
}
mat_vec_mult(A_inv,B,B);
vec_mult_scalar(B,eps,B);
for (i=1; i<=n_parm; ++i) {
new_a[i] = a[i] + B[i];
}
for (i=1; i<=n_con; ++i) {
new_mult[i] = mult[i] + B[n_parm+i];
}
} else {
if (n_con > 0) {
/* the mulitpliers are updated according:
mult = (dMda dMda_t)^(-1) (K/esp - dMda dLda_t) */
mat_mult(dMda,dMda_t,aux_mat);
if (!my_inv_ludcmp(aux_mat, n_con, aux_mat)) {
rc = FALSE;
break;
}
mat_vec_mult(dMda,dLda,aux_vec);
vec_mult_scalar(K,1./eps,K);
vec_sub(K,aux_vec,aux_vec);
mat_vec_mult(aux_mat,aux_vec,mult);
}
/* the update step looks the following:
a_new = a - eps * (dLda + mult_t * dMda)_t */
if (n_con > 0) {
vec_mat_mult(mult,dMda,new_a);
vec_add(dLda,new_a,new_a);
} else {
vec_equal(dLda,new_a);
}
vec_mult_scalar(new_a,eps,new_a);
vec_sub(a,new_a,new_a);
}
if (count == 1 && !pending1) {
last_cost = (*f_L)(a);
if (n_con > 0) {
(*f_M)(a,K);
last_cost += vec_mult_inner(K,mult);
}
} else {
last_cost = cost;
}
/* calculate the updated cost */
cost = (*f_L)(new_a);
/*printf(" %f\n",cost);*/
if (n_con > 0) {
(*f_M)(new_a,K);
if (newton_active) {
cost += vec_mult_inner(K,new_mult);
} else {
cost += vec_mult_inner(K,mult);
}
}
/* printf("last=%f new=%f\n",last_cost,cost); */
/* check out whether we reduced the cost */
if (cost > last_cost && fabs(cost-last_cost) > *tol) {
/* reduce the gradient climbing rate: sometimes a reduction of eps
causes an increase in cost, thus leave an option to increase
eps */
cost = last_cost; /* reset last_cost */
if (pending1 && pending2) {
/* this means that either increase nor decrease
of eps helps, ==> leave the program */
rc = TRUE;
break;
} else if (pending1) {
eps *= 4.0; /* the last cutting by half did not help, thus
multiply by 2 to get to previous value, and
one more time by 2 to get new value */
pending2 = TRUE;
} else {
eps /= 2.0;
pending1 = TRUE;
}
goto AGAIN;
} else {
vec_equal(new_a,a);
if (newton_active && n_con > 0) {
vec_equal(new_mult,mult);
}
if (use_newton && fabs(cost-last_cost) < NEWTON_THRESHOLD)
newton_active = TRUE;
}
}
my_free_vector(dLda,1,n_parm);
my_free_vector(new_a,1,n_parm);
if (n_con > 0) {
my_free_vector(mult,1,n_con);
my_free_matrix(dMda,1,n_con,1,n_parm);
my_free_matrix(dMda_t,1,n_parm,1,n_con);
my_free_vector(K,1,n_con);
my_free_matrix(aux_mat,1,n_con,1,n_con);
my_free_vector(aux_vec,1,n_con);
}
if (use_newton) {
my_free_matrix(dLdada,1,n_parm,1,n_parm);
my_free_matrix(A,1,n_parm+n_con,1,n_parm+n_con);
my_free_matrix(A_inv,1,n_parm+n_con,1,n_parm+n_con);
my_free_vector(B,1,n_parm+n_con);
if (n_con > 0) {
my_free_matrix(dMdada,1,n_con*n_parm,1,n_parm);
my_free_vector(new_mult,1,n_con);
}
}
*final_cost = cost;
*tol = fabs(cost-last_cost);
if (n_con > 0) {
*err = sqrt(vec_mult_inner(K,K));
} else {
*err = 0.0;
}
/*
printf("count=%ld rc=%d\n",count,rc);
*/
return rc;
}
mat mars(Agraph_t* g, struct marsopts opts)
{
int i, j, n = agnnodes(g), k = MIN(n, MAX(opts.k, 2)), iter = 0;
mat dij, u, u_trans, q, r, q_t, tmp, tmp2, z;
double* s = (double*) malloc(sizeof(double)*k);
double* ones = (double*) malloc(sizeof(double)*n);
double* d;
int* anchors = (int*) malloc(sizeof(int)*k);
int* clusters = NULL;
double change = 1, old_stress = -1;
dij = mat_new(k, n);
u = mat_new(n,k);
tmp = mat_new(n,k);
darrset(ones,n,-1);
select_anchors(g, dij, anchors, k);
if(opts.color) {
for(i = 0; i < k; i++) {
Agnode_t* anchor = get_node(anchors[i]);
agset(anchor, "color", "red");
}
}
if(opts.power != 1) {
clusters = graph_cluster(g,dij,anchors);
}
singular_vectors(g, dij, opts.power, u, s);
vec_scalar_mult(s, k, -1);
u_trans = mat_trans(u);
d = mat_mult_for_d(u, s, u_trans, ones);
for(i = 0; i < u->c; i++) {
double* col = mat_col(u,i);
double* b = inv_mul_ax(d,col,u->r);
for(j = 0; j < u->r; j++) {
tmp->m[mindex(j,i,tmp)] = b[j];
}
free(b);
free(col);
}
tmp2 = mat_mult(u_trans,tmp);
for(i = 0; i < k; i++) {
tmp2->m[mindex(i,i,tmp2)] += (1.0/s[i]);
}
q = mat_new(tmp2->r, tmp2->c);
r = mat_new(tmp2->c, tmp2->c);
qr_factorize(tmp2,q,r);
q_t = mat_trans(q);
if(opts.given) {
z = get_positions(g, opts.dim);
} else {
z = mat_rand(n, opts.dim);
}
translate_by_centroid(z);
if(opts.viewer) {
init_viewer(g, opts.max_iter);
append_layout(z);
}
old_stress = stress(z, dij, anchors, opts.power);
while(change > EPSILON && iter < opts.max_iter) {
mat right_side;
double new_stress;
if(opts.power == 1) {
right_side = barnes_hut(z);
} else {
right_side = barnes_hut_cluster(z, dij, clusters, opts.power);
}
for(i = 0; i < opts.dim; i++) {
double sum = 0;
double* x;
double* b = mat_col(right_side,i);
for(j = 0; j < right_side->r; j++) {
sum += b[j];
}
x = inv_mul_full(d, b, right_side->r, u, u_trans, q_t, r);
for(j = 0; j < z->r; j++) {
z->m[mindex(j,i,z)] = x[j] - sum/right_side->r;
}
free(x);
free(b);
}
adjust_anchors(g, anchors, k, z);
update_anchors(z, dij, anchors, opts.power);
translate_by_centroid(z);
if(opts.viewer) {
append_layout(z);
}
new_stress = stress(z, dij, anchors, opts.power);
change = fabs(new_stress-old_stress)/old_stress;
old_stress = new_stress;
mat_free(right_side);
iter++;
}
mat_free(dij);
mat_free(u);
mat_free(u_trans);
mat_free(q);
mat_free(r);
mat_free(q_t);
mat_free(tmp);
mat_free(tmp2);
free(s);
free(ones);
free(d);
free(anchors);
free(clusters);
return z;
}
/*
* This is the constraint that makes sure we hit the ball
*/
void kinematics_eq_constr(unsigned m, double *result, unsigned n, const double *x, double *grad, void *data) {
static double ballPred[CART];
static double racketDesVel[CART];
static double racketDesNormal[CART];
static Matrix link_pos_des;
static Matrix joint_cog_mpos_des;
static Matrix joint_origin_pos_des;
static Matrix joint_axis_pos_des;
static Matrix Alink_des[N_LINKS+1];
static Matrix racketTransform;
static Matrix Jacobi;
static Vector qfdot;
static Vector xfdot;
static Vector normal;
static int firsttime = TRUE;
double T = x[2*DOF];
int N = T/TSTEP;
int i;
/* initialization of static variables */
if (firsttime) {
firsttime = FALSE;
link_pos_des = my_matrix(1,N_LINKS,1,3);
joint_cog_mpos_des = my_matrix(1,N_DOFS,1,3);
joint_origin_pos_des = my_matrix(1,N_DOFS,1,3);
joint_axis_pos_des = my_matrix(1,N_DOFS,1,3);
Jacobi = my_matrix(1,2*CART,1,N_DOFS);
racketTransform = my_matrix(1,4,1,4);
qfdot = my_vector(1,DOF);
xfdot = my_vector(1,2*CART);
normal = my_vector(1,CART);
for (i = 0; i <= N_LINKS; ++i)
Alink_des[i] = my_matrix(1,4,1,4);
// initialize the base variables
//taken from ParameterPool.cf
bzero((void *) &base_state, sizeof(base_state));
bzero((void *) &base_orient, sizeof(base_orient));
base_orient.q[_Q1_] = 1.0;
// the the default endeffector parameters
setDefaultEndeffector();
// homogeneous transform instead of using quaternions
racketTransform[1][1] = 1;
racketTransform[2][3] = 1;
racketTransform[3][2] = -1;
racketTransform[4][4] = 1;
}
if (grad) {
// compute gradient of kinematics = jacobian
//TODO:
grad[0] = 0.0;
grad[1] = 0.0;
grad[2] = 0.0;
}
// interpolate at time T to get the desired racket parameters
first_order_hold(ballPred,racketDesVel,racketDesNormal,T);
// extract state information from array to joint_des_state structure
for (i = 1; i <= DOF; i++) {
joint_des_state[i].th = x[i-1];
joint_des_state[i].thd = qfdot[i] = x[i-1+DOF];
}
/* compute the desired link positions */
kinematics(joint_des_state, &base_state, &base_orient, endeff,
joint_cog_mpos_des, joint_axis_pos_des, joint_origin_pos_des,
link_pos_des, Alink_des);
/* compute the racket normal */
mat_mult(Alink_des[6],racketTransform,Alink_des[6]);
for (i = 1; i <= CART; i++) {
normal[i] = Alink_des[6][i][3];
}
/* compute the jacobian */
jacobian(link_pos_des, joint_origin_pos_des, joint_axis_pos_des, Jacobi);
mat_vec_mult(Jacobi, qfdot, xfdot);
/* deviations from the desired racket frame */
for (i = 1; i <= CART; i++) {
//printf("xfdot[%d] = %.4f, racketDesVel[%d] = %.4f\n",i,xfdot[i],i,racketDesVel[i-1]);
//printf("normal[%d] = %.4f, racketDesNormal[%d] = %.4f\n",i,normal[i],i,racketDesNormal[i-1]);
result[i-1] = link_pos_des[6][i] - ballPred[i-1];
result[i-1 + CART] = xfdot[i] - racketDesVel[i-1];
result[i-1 + 2*CART] = normal[i] - racketDesNormal[i-1];
}
}
/* main program
*/
int main(int argc,char *argv[])
{
// double **a=allocm_mat(2,2),**b=allocm_mat(2,2),**c=allocm_mat(2,2);
// a[0][0]=1.0; a[0][1]=2.0; /* initialize a */
// a[1][0]=2.0; a[1][1]=1.0;
// b[0][0]=1.0; b[0][1]=2.0; /* initialize b */
//b[1][0]=3.0; b[1][1]=4.0;
// c[0][0]=0.0; c[0][1]=0.0; /* initialize b */
// c[1][0]=0.0; c[1][1]=0.0;
char * filenamea;
char * filenameb;
char * filenamec;
/* read in size of matrix, number of processors */
if (argc!=5) {
filenamea = "1000-A.mat";
filenameb = "1000-B.mat";
NUM_THREADS = 8;
filenamec= "1000-C.mat";
}
else
{
filenamea = argv[1];
filenameb = argv[2];
NUM_THREADS = atoi(argv[3]);
filenamec= argv[4];
}
read_matrix(filenamea,filenameb);
pthread_mutex_init(&mc_mutex,NULL); /* initialize the lock */
double wall0 = get_wall_time();
mat_mult(); /* calculate A*B */
// printf("%8.4f%8.4f\n",c[0][0],c[0][1]); /* 7.0000 10.0000 */
// printf("%8.4f%8.4f\n",c[1][0],c[1][1]); /* 5.0000 8.0000 */
double wall1 = get_wall_time();
print_time(wall1-wall0);
print_matrix(filenamec, mat_c, m_dim, p_dim);
pthread_mutex_destroy(&mc_mutex);
return(0);
}
void BaseStateEstimation::update(SL_quat& base_orientation, SL_Cstate& base_position)
{
SL_quat orient;
memcpy(&(orient.q[1]), imu_quaternion_, 4*sizeof(double));
MY_MATRIX(quat_to_rot_helper, 1,3,1,3);
quatToRotMat(&orient, quat_to_rot_helper);
for(int r=1; r<=3; r++)
for(int c=1; c<=3; c++)
O_rot_I_[r][c] = quat_to_rot_helper[c][r];
memcpy(&(O_angrate_I_[1]), imu_angrate_, 3*sizeof(double));
// memcpy(&(O_linvel_I_[1]), imu_linvel_, 3*sizeof(double));
memcpy(&(O_linacc_I_[1]), imu_linacc_, 3*sizeof(double));
// compensate for gravity
// for(int i=1; i<=3; i++)
// O_linacc_I_[i] -= gravity_[i];
// do numerical integration
for(int i=1; i<=3; i++)
{
//integrate
O_angacc_I_[i] = (double)update_rate_*(O_angrate_I_[i] - prev_O_angrate_I_[i]);
// O_linpos_I_[i] += (1.0/(double)update_rate_)*O_linvel_I_[i];
//apply leakage term
// O_linvel_I_[i] *= leakage_factor_;
// O_linvel_I_[i] += (1.0/(double)update_rate_)*O_linacc_I_[i];
}
// do coordinate transformation
MY_MATRIX(S_angrate, 1,3,1,3);
vectorToSkewMatrix(O_angrate_I_, S_angrate);
MY_MATRIX(S_angacc, 1,3,1,3);
vectorToSkewMatrix(O_angacc_I_, S_angacc);
MY_MATRIX(S_helper, 1,3,1,3);
mat_mult(S_angrate, S_angrate, S_helper);
mat_add(S_angacc, S_helper, S_helper);
mat_mult(O_rot_I_, I_rot_B_, O_rot_B_);
// mat_vec_mult(O_rot_I_, I_linpos_B_, O_linpos_B_);
// vec_add(O_linpos_I_, O_linpos_B_, O_linpos_B_);
//
// mat_vec_mult(O_rot_I_, I_linpos_B_, O_linvel_B_);
// mat_vec_mult(S_angrate, O_linvel_B_, O_linvel_B_);
// vec_add(O_linvel_I_, O_linvel_B_, O_linvel_B_);
mat_vec_mult(O_rot_I_, I_linpos_B_, O_linacc_B_);
mat_vec_mult(S_helper, O_linacc_B_, O_linacc_B_);
vec_add(O_linacc_I_, O_linacc_B_, O_linacc_B_);
// integrate base state
for(int i=1; i<=3; i++)
{
//integrate
O_linpos_B_[i] += (1.0/(double)update_rate_)*O_linvel_B_[i];
//apply leakage term
O_linvel_B_[i] *= leakage_factor_;
O_linvel_B_[i] += (1.0/(double)update_rate_)*O_linacc_B_[i];
}
// write data back
linkQuat(O_rot_B_, &O_orient_B_);
memcpy(&(O_orient_B_.ad[1]), &(O_angrate_I_[1]), 3*sizeof(double));
memcpy(&(O_orient_B_.add[1]), &(O_angacc_I_[1]), 3*sizeof(double));
memcpy(&(O_trans_B_.x[1]), &(O_linpos_B_[1]),3*sizeof(double));
memcpy(&(O_trans_B_.xd[1]), &(O_linvel_B_[1]),3*sizeof(double));
memcpy(&(O_trans_B_.xdd[1]), &(O_linacc_B_[1]),3*sizeof(double));
// linkQuat(O_rot_I_, &O_orient_B_);
// memcpy(&(O_orient_B_.ad[1]), &(O_angrate_I_[1]), 3*sizeof(double));
// memcpy(&(O_orient_B_.add[1]), &(O_angacc_I_[1]), 3*sizeof(double));
// memcpy(&(O_trans_B_.x[1]), &(O_linpos_I_[1]),3*sizeof(double));
// memcpy(&(O_trans_B_.xd[1]), &(O_linvel_I_[1]),3*sizeof(double));
// memcpy(&(O_trans_B_.xdd[1]), &(O_linacc_I_[1]),3*sizeof(double));
// quatDerivatives(&O_orient_B_);
base_orientation = O_orient_B_;
base_position = O_trans_B_;
// update buffers
memcpy(&(prev_O_angrate_I_[1]), &(O_angrate_I_[1]), 3*sizeof(double));
}
int main(int argc, char* argv[])
{
TEST_PARAS myparas = parse_test_paras(argc, argv, testfile, embeddingfile, trainfile);
printf("Predicting...\n");
if(!myparas.allow_self_transition)
printf("Do not allow self-transtion.\n");
if (!myparas.underflow_correction)
printf("Underflow correction disabled\n");
int new_test_song_exp = (myparas.train_test_hash_file[0] != '\0');
if(myparas.tagfile[0] == '\0' && new_test_song_exp)
{
printf("Have to support with a tag file if you want to test on unseen songs.\n");
exit(1);
}
int d;
int m;
int l;
int i;
int j;
int s;
int fr;
int to;
double* bias_terms = 0;
double** X = read_matrix_file(embeddingfile, &l, &d, &bias_terms);
double** realX;
PDATA pd = read_playlists_data(testfile);
//int k = pd.num_songs;
int k;
double llhood = 0.0;
double uniform_llhood = 0.0;
double realn = 0.0;
double not_realn= 0.0;
int* train_test_hash;
int k_train;
int k_test;
TDATA td;
if(!new_test_song_exp)
{
k = pd.num_songs;
if(myparas.tagfile[0] != '\0')
{
td = read_tag_data(myparas.tagfile);
m = td.num_tags;
myparas.num_points = l / (k + m);
realX = zerosarray(k * myparas.num_points, d);
calculate_realX(X, realX, td, k, m, d, myparas.num_points);
free_tag_data(td);
if(myparas.tag_ebd_filename[0] != '\0')
write_embedding_to_file(X + k * myparas.num_points, m * myparas.num_points, d, myparas.tag_ebd_filename, 0);
}
else
{
myparas.num_points = l / k;
realX = zerosarray(k * myparas.num_points, d);
Array2Dcopy(X, realX, l, d);
}
Array2Dfree(X, l, d);
}
else
{
printf("Prediction on unseen songs.\n");
td = read_tag_data(myparas.tagfile);
m = td.num_tags;
k = td.num_songs;
train_test_hash = read_hash(myparas.train_test_hash_file, &k_train);
k_test = k - k_train;
printf("Number of new songs %d.\n", k_test);
myparas.num_points = l / (k_train + m);
realX = zerosarray(k * myparas.num_points, d);
calculate_realX_with_hash(X, realX, td, k, m, d, myparas.num_points, k_train, train_test_hash);
free_tag_data(td);
Array2Dfree(X, l, d);
}
if(myparas.song_ebd_filename[0] != '\0')
write_embedding_to_file(realX, k * myparas.num_points, d, myparas.song_ebd_filename, 0);
if(myparas.bias_ebd_filename[0] != '\0')
{
FILE* fp = fopen(myparas.bias_ebd_filename, "w");
for( i = 0; i < k ;i++)
{
fprintf(fp, "%f", bias_terms[i]);
if ( i != k - 1)
fputc('\n', fp);
}
fclose(fp);
}
double** square_dist;
if(myparas.square_dist_filename[0] != '\0')
square_dist = zerosarray(k, k);
int n = 0;
for(i = 0; i < pd.num_playlists; i ++)
if(pd.playlists_length[i] > 0)
n += pd.playlists_length[i] - 1;
printf("Altogether %d transitions.\n", n);fflush(stdout);
PHASH* tcount;
PHASH* tcount_train;
double** tcount_full;
double** tcount_full_train;
if(myparas.use_hash_TTable)
tcount = create_empty_hash(2 * n);
else
tcount_full = zerosarray(k, k);
HELEM temp_elem;
TPAIR temp_pair;
int idx;
double temp_val;
for(i = 0; i < pd.num_playlists; i ++)
{
if(pd.playlists_length[i] > myparas.range)
{
for(j = 0; j < pd.playlists_length[i] - 1; j++)
{
temp_pair.fr = pd.playlists[i][j];
temp_pair.to = pd.playlists[i][j + myparas.range];
//printf("(%d, %d)\n", temp_pair.fr, temp_pair.to);
if(temp_pair.fr >= 0 && temp_pair.to >= 0)
{
if(myparas.use_hash_TTable)
{
idx = exist_in_hash(tcount, temp_pair);
if(idx < 0)
{
temp_elem.key = temp_pair;
temp_elem.val = 1.0;
add_entry(tcount, temp_elem);
}
else
update_with(tcount, idx, 1.0);
}
else
tcount_full[temp_pair.fr][temp_pair.to] += 1.0;
}
}
}
}
TRANSITIONTABLE ttable;
TRANSITIONTABLE BFStable;
//Need to use the training file
if(myparas.output_distr)
{
PDATA pd_train = read_playlists_data(trainfile);
if(myparas.use_hash_TTable)
tcount_train = create_empty_hash(2 * n);
else
tcount_full_train = zerosarray(k, k);
for(i = 0; i < pd_train.num_playlists; i ++)
{
if(pd_train.playlists_length[i] > 1)
{
for(j = 0; j < pd_train.playlists_length[i] - 1; j++)
{
temp_pair.fr = pd_train.playlists[i][j];
temp_pair.to = pd_train.playlists[i][j + 1];
if(myparas.use_hash_TTable)
{
idx = exist_in_hash(tcount_train, temp_pair);
if(idx < 0)
{
temp_elem.key = temp_pair;
temp_elem.val = 1.0;
add_entry(tcount_train, temp_elem);
}
else
update_with(tcount_train, idx, 1.0);
}
else
tcount_full_train[temp_pair.fr][temp_pair.to] += 1.0;
}
}
}
}
FILE* song_distr_file;
FILE* trans_distr_file;
double* song_sep_ll;
if(myparas.output_distr)
{
printf("Output likelihood distribution file turned on.\n");
if(myparas.output_distr)
{
song_distr_file = fopen(songdistrfile, "w");
trans_distr_file = fopen(transdistrfile, "w");
song_sep_ll = (double*)calloc(k, sizeof(double));
}
}
int* test_ids_for_new_songs;
if(new_test_song_exp)
test_ids_for_new_songs = get_test_ids(k, k_train, train_test_hash);
for(fr = 0; fr < k; fr++)
{
int collection_size;
int* collection_idx;
if(myparas.fast_collection)
{
collection_size = (BFStable.parray)[fr].length;
if (collection_size == 0)
continue;
collection_idx = (int*)malloc(collection_size * sizeof(int));
LINKEDELEM* tempp = (BFStable.parray)[fr].head;
for(i = 0; i < collection_size; i++)
{
collection_idx[i] = tempp -> idx;
tempp = tempp -> pnext;
}
}
else if(new_test_song_exp)
{
collection_size = k_test;
collection_idx = (int*)malloc(collection_size * sizeof(int));
int_list_copy(test_ids_for_new_songs, collection_idx, k_test);
}
else
collection_size = k;
double** delta = zerosarray(collection_size, d);
double* p = (double*)calloc(collection_size, sizeof(double));
double** tempkd = zerosarray(collection_size, d);
double* tempk = (double*)calloc(collection_size, sizeof(double));
double** mid_delta = 0;
double* mid_p = 0;
double** mid_tempkd = 0;
// I get a seg fault when these get freed. Don't understand.
if (myparas.num_points == 3) {
mid_delta = zerosarray(collection_size, d);
mid_p = (double*)calloc(collection_size, sizeof(double));
mid_tempkd = zerosarray(collection_size, d);
}
for(j = 0; j < collection_size; j++)
{
for(i = 0; i < d; i++)
{
if(myparas.fast_collection || new_test_song_exp)
delta[j][i] = realX[fr][i] - realX[(myparas.num_points - 1) * k + collection_idx[j]][i];
else
delta[j][i] = realX[fr][i] - realX[(myparas.num_points - 1) * k + j][i];
}
if(myparas.num_points == 3) {
if(myparas.fast_collection || new_test_song_exp)
mid_delta[j][i] =
realX[k + fr][i] - realX[k + collection_idx[j]][i];
else
mid_delta[j][i] = realX[k + fr][i] - realX[k + j][i];
}
}
mat_mult(delta, delta, tempkd, collection_size, d);
scale_mat(tempkd, collection_size, d, -1.0);
sum_along_direct(tempkd, p, collection_size, d, 1);
if(myparas.square_dist_filename[0] != '\0')
for(i = 0; i < k; i++)
square_dist[fr][i] = -p[i];
if (bias_terms != 0)
add_vec(p, bias_terms, collection_size, 1.0);
if (myparas.num_points == 3) {
// Just use the mid_deltas (midpoint differences): square them,
// then sum and add to the p vector directly, then the midpoint
// probability is incorporated
mat_mult(mid_delta, mid_delta, mid_tempkd, collection_size, d);
scale_mat(mid_tempkd, collection_size, d, -1.0);
sum_along_direct(mid_tempkd, mid_p, collection_size, d, 1);
add_vec(p, mid_p, collection_size, 1.0);
}
if (myparas.underflow_correction == 1) {
double max_val = p[0];
for(i = 0; i < collection_size; i++)
max_val = p[i] > max_val? p[i] : max_val;
vec_scalar_sum(p, -max_val, collection_size);
}
Veccopy(p, tempk, collection_size);
exp_on_vec(tempk, collection_size);
//exp_on_vec(p, collection_size);
// underflow checking:
// for (i = 0; i < collection_size; i++)
// if (p[i] < 0.000001)
// p[i] = 0.000001;
double temp_sum;
if(myparas.allow_self_transition)
temp_sum = sum_vec(tempk, collection_size);
else
{
temp_sum = 0.0;
for(i = 0; i < collection_size; i++)
if(!myparas.fast_collection || new_test_song_exp)
temp_sum += (i != fr)? tempk[i] : 0.0;
else
temp_sum += (collection_idx[i] != fr)? tempk[i] : 0.0;
}
vec_scalar_sum(p, -log(temp_sum), collection_size);
//scale_vec(p, collection_size, 1.0 / temp_sum);
//printf("done...\n");
for(to = 0; to < k; to++)
{
if(myparas.allow_self_transition || (!myparas.allow_self_transition && fr != to))
{
temp_pair.fr = fr;
temp_pair.to = to;
//printf("(%d, %d)\n", fr, to);
if(myparas.use_hash_TTable)
idx = exist_in_hash(tcount, temp_pair);
else
idx = tcount_full[fr][to] > 0.0? 1 : -1;
//printf("%d\n", idx);fflush(stdout);
int idx_train;
//printf("done...\n");fflush(stdout);
if(myparas.output_distr)
{
if(myparas.use_hash_TTable)
idx_train = exist_in_hash(tcount_train, temp_pair);
else
idx_train = tcount_full_train[fr][to] > 0.0? 1 : -1;
}
if(idx >= 0)
{
if(myparas.fast_collection || new_test_song_exp)
{
s = -1;
for(i = 0; i < collection_size; i++)
{
if(collection_idx[i] == to)
{
s = i;
break;
}
}
}
else
s = to;
//printf("%d\n", idx);fflush(stdout);
if(myparas.use_hash_TTable)
temp_val = retrieve_value_with_idx(tcount, idx);
else
temp_val = tcount_full[fr][to];
if(s < 0)
not_realn += temp_val;
else
{
//printf("s = %d\n", s);
llhood += temp_val * p[s];
if(new_test_song_exp)
uniform_llhood += temp_val * log(1.0 / (double) k_test);
realn += temp_val;
if(myparas.output_distr)
{
//double temp_val_train = idx_train >= 0? retrieve_value_with_idx(tcount_train, idx_train): 0.0;
double temp_val_train;
if(idx_train < 0)
temp_val_train = 0.0;
else
temp_val_train = myparas.use_hash_TTable ? retrieve_value_with_idx(tcount_train, idx_train) : tcount_full_train[fr][to];
song_sep_ll[fr] += temp_val * p[s];
song_sep_ll[to] += temp_val * p[s];
fprintf(trans_distr_file, "%d %d %f\n", (int)temp_val_train, (int)temp_val, temp_val * p[s]);
}
}
}
}
}
Array2Dfree(delta, collection_size, d);
free(p);
Array2Dfree(tempkd, collection_size, d);
free(tempk);
if (myparas.num_points == 3) {
Array2Dfree(mid_delta, collection_size, d);
free(mid_p);
Array2Dfree(mid_tempkd, collection_size, d);
}
if(myparas.fast_collection || new_test_song_exp)
free(collection_idx);
}
if(myparas.output_distr)
{
printf("Writing song distr.\n");
for(i = 0; i < k; i++)
fprintf(song_distr_file, "%d %f\n", (int)(pd.id_counts[i]), song_sep_ll[i]);
fclose(song_distr_file);
fclose(trans_distr_file);
free(song_sep_ll);
}
llhood /= realn;
printf("Avg log-likelihood on test: %f\n", llhood);
if(myparas.fast_collection)
printf("Ratio of transitions that do not appear in the training set: %f\n", not_realn / (realn + not_realn));
if(new_test_song_exp)
{
uniform_llhood /= realn;
printf("Avg log-likelihood for uniform baseline: %f\n", uniform_llhood);
}
if(myparas.use_hash_TTable)
free_hash(tcount);
else
Array2Dfree(tcount_full, k, k);
free_playlists_data(pd);
if(myparas.output_distr)
{
if(myparas.use_hash_TTable)
free_hash(tcount_train);
else
Array2Dfree(tcount_full_train, k, k);
}
Array2Dfree(realX, k * myparas.num_points, d);
if(new_test_song_exp)
{
free(train_test_hash);
free(test_ids_for_new_songs);
}
if(myparas.square_dist_filename[0] != '\0')
{
write_embedding_to_file(square_dist, k, k, myparas.square_dist_filename, 0);
Array2Dfree(square_dist, k, k);
}
}