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