/** * Adapted from: Multivariate Normal density function and random * number generator Using GSL from Ralph dos Santos Silva * Copyright (C) 2006 * * multivariate normal density function * * @param n dimension of the random vetor * @param mean vector of means of size n * @param var variance matrix of dimension n x n */ double ssm_dmvnorm(const int n, const gsl_vector *x, const gsl_vector *mean, const gsl_matrix *var, double sd_fac) { int s; double ax,ay; gsl_vector *ym, *xm; gsl_matrix *work = gsl_matrix_alloc(n,n), *winv = gsl_matrix_alloc(n,n); gsl_permutation *p = gsl_permutation_alloc(n); gsl_matrix_memcpy( work, var ); //scale var with sd_fac^2 gsl_matrix_scale(work, sd_fac*sd_fac); gsl_linalg_LU_decomp( work, p, &s ); gsl_linalg_LU_invert( work, p, winv ); ax = gsl_linalg_LU_det( work, s ); gsl_matrix_free( work ); gsl_permutation_free( p ); xm = gsl_vector_alloc(n); gsl_vector_memcpy( xm, x); gsl_vector_sub( xm, mean ); ym = gsl_vector_alloc(n); gsl_blas_dsymv(CblasUpper,1.0,winv,xm,0.0,ym); gsl_matrix_free( winv ); gsl_blas_ddot( xm, ym, &ay); gsl_vector_free(xm); gsl_vector_free(ym); ay = exp(-0.5*ay)/sqrt( pow((2*M_PI),n)*ax ); return ay; }
static REAL8 fContact (REAL8 x, void *params) { fContactWorkSpace *p = (fContactWorkSpace *)params; gsl_permutation *p1 = p->p1; gsl_vector *tmpV = p->tmpV; gsl_matrix *C = p->C; gsl_matrix *A = p->tmpA; gsl_matrix *B = p->tmpB; INT4 s1; REAL8 result; gsl_matrix_memcpy ( A, p->invQ1); gsl_matrix_memcpy ( B, p->invQ2); gsl_matrix_scale (B, x); gsl_matrix_scale (A, (1.0L-x)); gsl_matrix_add (A, B); gsl_linalg_LU_decomp( A, p1, &s1 ); gsl_linalg_LU_invert( A, p1, C ); /* Evaluate the product C x r_AB */ gsl_blas_dsymv (CblasUpper, 1.0, C, p->r_AB, 0.0, tmpV); /* Now evaluate transpose(r_AB) x (C x r_AB) */ gsl_blas_ddot (p->r_AB, tmpV, &result); result *= (x*(1.0L-x)); return (-result); }
double dmvt(const unsigned int n, const gsl_vector *x, const gsl_vector *location, const gsl_matrix *scale, const unsigned int dof) { int s; double ax,ay,az=0.5*(dof + n); gsl_vector *ym, *xm; gsl_matrix *work = gsl_matrix_alloc(n,n), *winv = gsl_matrix_alloc(n,n); gsl_permutation *p = gsl_permutation_alloc(n); gsl_matrix_memcpy( work, scale ); gsl_linalg_LU_decomp( work, p, &s ); gsl_linalg_LU_invert( work, p, winv ); ax = gsl_linalg_LU_det( work, s ); gsl_matrix_free( work ); gsl_permutation_free( p ); xm = gsl_vector_alloc(n); gsl_vector_memcpy( xm, x); gsl_vector_sub( xm, location ); ym = gsl_vector_alloc(n); gsl_blas_dsymv(CblasUpper,1.0,winv,xm,0.0,ym); gsl_matrix_free( winv ); gsl_blas_ddot( xm, ym, &ay); gsl_vector_free(xm); gsl_vector_free(ym); ay = pow((1+ay/dof),-az)*gsl_sf_gamma(az)/(gsl_sf_gamma(0.5*dof)*sqrt( pow((dof*M_PI),double(n))*ax )); return ay; }
int lls_lcurve(gsl_vector *reg_param, gsl_vector *rho, gsl_vector *eta, lls_workspace *w) { const size_t N = rho->size; /* number of points on L-curve */ if (N != reg_param->size) { GSL_ERROR("size of reg_param and rho do not match", GSL_EBADLEN); } else if (N != eta->size) { GSL_ERROR("size of eta and rho do not match", GSL_EBADLEN); } else { int s; double smax, smin; size_t i; /* compute eigenvalues of A^T W A */ gsl_matrix_transpose_memcpy(w->work_A, w->ATA); s = gsl_eigen_symm(w->work_A, w->eval, w->eigen_p); if (s) return s; /* find largest and smallest eigenvalues */ gsl_vector_minmax(w->eval, &smin, &smax); /* singular values are square roots of eigenvalues */ smax = sqrt(smax); if (smin > GSL_DBL_EPSILON) smin = sqrt(fabs(smin)); gsl_multifit_linear_lreg(smin, smax, reg_param); for (i = 0; i < N; ++i) { double r2; double lambda = gsl_vector_get(reg_param, i); lls_solve(lambda, w->c, w); /* store ||c|| */ gsl_vector_set(eta, i, gsl_blas_dnrm2(w->c)); /* compute: A^T A c - 2 A^T y */ gsl_vector_memcpy(w->work_b, w->ATb); gsl_blas_dsymv(CblasUpper, 1.0, w->ATA, w->c, -2.0, w->work_b); /* compute: c^T A^T A c - 2 c^T A^T y */ gsl_blas_ddot(w->c, w->work_b, &r2); r2 += w->bTb; gsl_vector_set(rho, i, sqrt(r2)); } return GSL_SUCCESS; } } /* lls_lcurve() */
gsl_vector* linear_ols_beta(gsl_vector *v_y, gsl_matrix *m_X){ size_t i_k = m_X->size2; gsl_vector *v_XTy = gsl_vector_alloc(i_k); gsl_vector *v_betahat = gsl_vector_alloc(i_k); gsl_matrix *m_XTX = gsl_matrix_alloc(i_k,i_k); gsl_matrix *m_invXTX = gsl_matrix_alloc(i_k,i_k); gsl_vector_set_all(v_XTy,0); gsl_vector_set_all(v_betahat,0); gsl_matrix_set_all(m_XTX,0); olsg(v_y,m_X,v_XTy,m_XTX); gsl_linalg_cholesky_decomp (m_XTX); gsl_matrix_set_identity(m_invXTX); gsl_blas_dtrsm (CblasLeft, CblasLower,CblasNoTrans,CblasNonUnit,1.0,m_XTX,m_invXTX); gsl_blas_dtrsm (CblasLeft, CblasLower,CblasTrans,CblasNonUnit,1.0,m_XTX,m_invXTX); gsl_vector_set_all(v_betahat,0.0); gsl_blas_dsymv (CblasLower,1.0,m_invXTX,v_XTy,0.0,v_betahat); gsl_vector_free(v_XTy); gsl_matrix_free(m_XTX); gsl_matrix_free(m_invXTX); return v_betahat; }
int gsl_linalg_symmtd_decomp (gsl_matrix * A, gsl_vector * tau) { if (A->size1 != A->size2) { GSL_ERROR ("symmetric tridiagonal decomposition requires square matrix", GSL_ENOTSQR); } else if (tau->size + 1 != A->size1) { GSL_ERROR ("size of tau must be (matrix size - 1)", GSL_EBADLEN); } else { const size_t N = A->size1; size_t i; for (i = 0 ; i < N - 2; i++) { gsl_vector_view c = gsl_matrix_column (A, i); gsl_vector_view v = gsl_vector_subvector (&c.vector, i + 1, N - (i + 1)); double tau_i = gsl_linalg_householder_transform (&v.vector); /* Apply the transformation H^T A H to the remaining columns */ if (tau_i != 0.0) { gsl_matrix_view m = gsl_matrix_submatrix (A, i + 1, i + 1, N - (i+1), N - (i+1)); double ei = gsl_vector_get(&v.vector, 0); gsl_vector_view x = gsl_vector_subvector (tau, i, N-(i+1)); gsl_vector_set (&v.vector, 0, 1.0); /* x = tau * A * v */ gsl_blas_dsymv (CblasLower, tau_i, &m.matrix, &v.vector, 0.0, &x.vector); /* w = x - (1/2) tau * (x' * v) * v */ { double xv, alpha; gsl_blas_ddot(&x.vector, &v.vector, &xv); alpha = - (tau_i / 2.0) * xv; gsl_blas_daxpy(alpha, &v.vector, &x.vector); } /* apply the transformation A = A - v w' - w v' */ gsl_blas_dsyr2(CblasLower, -1.0, &v.vector, &x.vector, &m.matrix); gsl_vector_set (&v.vector, 0, ei); } gsl_vector_set (tau, i, tau_i); } return GSL_SUCCESS; } }
double argOfExp(particle_t *oldPa, particle_t *newPa, gsl_matrix *invCov) { gsl_vector *Delta_param = oldPa->param_gsl; gsl_vector *intermediate = newPa->param_gsl; double value; int i; for (i=0; i<newPa->d; i++) gsl_vector_set(Delta_param, i, newPa->param[i] - oldPa->param[i]); gsl_blas_dsymv(CblasUpper, 1.0, invCov, Delta_param, 0.0, intermediate); //-- intermediate = invCov * Delta_param gsl_blas_ddot(Delta_param, intermediate, &value); value *= -0.5; return value; }
double ldmvnorm(const int n, const gsl_vector *x, const gsl_vector *mean, const gsl_matrix *var){ /* log-multivariate normal density function */ /* * n dimension of the random vetor * mean vector of means of size n * var variance matrix of dimension n x n */ int s; double ax,ay; gsl_vector *ym, *xm; gsl_matrix *work = gsl_matrix_alloc(n,n), *winv = gsl_matrix_alloc(n,n); #ifdef PRINTSTIFF /* Print Stiffness indicator S=max(eigen)/min(eigen)*/ gsl_vector *eval = gsl_vector_alloc (n); gsl_matrix *evec = gsl_matrix_alloc (n, n); gsl_matrix_memcpy(work,var); gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc(n); gsl_eigen_symmv (work, eval, evec, w); gsl_eigen_symmv_free (w); gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC); printf("%f ", gsl_vector_get(eval,n-1) / gsl_vector_get(eval,0)); gsl_vector_free(eval); gsl_matrix_free(evec); #endif gsl_permutation *p = gsl_permutation_alloc(n); gsl_matrix_memcpy( work, var ); gsl_linalg_LU_decomp( work, p, &s ); gsl_linalg_LU_invert( work, p, winv ); ax = gsl_linalg_LU_det( work, s ); gsl_matrix_free( work ); gsl_permutation_free( p ); xm = gsl_vector_alloc(n); gsl_vector_memcpy( xm, x); gsl_vector_sub( xm, mean ); ym = gsl_vector_alloc(n); gsl_blas_dsymv(CblasUpper,1.0,winv,xm,0.0,ym); gsl_matrix_free( winv ); gsl_blas_ddot( xm, ym, &ay); gsl_vector_free(xm); gsl_vector_free(ym); /* * ay = exp(-0.5*ay)/sqrt( pow((2*M_PI),n)*ax ); */ ay = -0.5*( ay + n*log(2*M_PI) + log(ax) ); return ay; }
/* V = tau*XTX + T0, M = V^{-1}*(tau*XTy + T0*b0) */ void linear_gibbs_beta(const gsl_matrix *XTX, const gsl_vector *XTy, const gsl_vector *b0, const double s2, const gsl_matrix *T0, gsl_vector *draw) { size_t k=XTy->size; double tau=1./s2; gsl_matrix *V=gsl_matrix_alloc(k,k); gsl_matrix *VI=gsl_matrix_alloc(k,k); gsl_vector *tmp=gsl_vector_alloc(k); gsl_vector *M=gsl_vector_alloc(k); /* compute V = tau*XTX + T0 */ gsl_matrix_memcpy(V,XTX); gsl_matrix_scale(V,tau); gsl_matrix_add(V,T0); /* compute V inverse = VI */ gsl_linalg_cholesky_decomp (V); gsl_matrix_set_identity(VI); gsl_blas_dtrsm (CblasLeft, CblasLower,CblasNoTrans,CblasNonUnit,1.0,V,VI); gsl_blas_dtrsm (CblasLeft, CblasLower,CblasTrans,CblasNonUnit,1.0,V,VI); /* form T0*b0 + tau*XTy */ gsl_vector_memcpy(tmp,XTy); gsl_blas_dsymv (CblasLower,1.0,T0,b0,tau,tmp); /* form V^{-1}*( T0*b0 + tau*XTy ) */ gsl_vector_set_all(M,0.0); gsl_blas_dsymv (CblasLower,1.0,VI,tmp,0.0,M); ran_mvn(M,VI,draw); gsl_matrix_free(V); gsl_matrix_free(VI); gsl_vector_free(tmp); gsl_vector_free(M); }
double execute_chi2_t(chi2_t *chichi) { //-- Let Delta X = X_model - X_obs, //-- L = 1 / sqrt[(2 pi)^d * det(Cov)] * exp[-0.5 *(Delta X)^T * Cov^-1 * (Delta X)] //-- -2 ln L = -2 * [ -0.5 * ln (2 pi)^d - 0.5 * ln det(Cov) - 0.5 * (Delta X)^T * Cov^-1 * (Delta X) ] //-- = cst + ln det(Cov) + (Delta X)^T * Cov^-1 * (Delta X) //-- We set chi2 = ln det(Cov) + (Delta X)^T * Cov^-1 * (Delta X) //-- data should be N*d matrix int N = chichi->N; int d = chichi->d; gsl_vector *X_model = chichi->X_model; double value; gsl_vector_sub(X_model, chichi->X_obs); //-- X_model -= X_obs gsl_blas_dsymv(CblasUpper, 1.0, chichi->invCov, X_model, 0.0, chichi->intermediate); //-- intermediate = invCov * (X_model - X_obs) gsl_blas_ddot(X_model, chichi->intermediate, &value); value += gsl_linalg_LU_lndet(chichi->cov); return value; }
void GetMVNpdf(const gsl_matrix * mat, const double * mu, const gsl_matrix * sigmaInv, const gsl_matrix * sigmaChol, const size_t nPoints, const size_t nDim, double * returnVal) { double normConst = - log(2*M_PI)*nDim/2.0; for(size_t j = 0; j < nDim; j++) normConst -= log(gsl_matrix_get(sigmaChol, j, j)); gsl_vector_const_view vecMu = gsl_vector_const_view_array(mu, nDim); #pragma omp parallel for for(size_t i = 0; i < nPoints; i++){ gsl_vector * x1 = gsl_vector_alloc(nDim); // Note: allocating and freeing these every loop is not ideal, but needed for threadsafe. There might be a better way. gsl_vector * x2 = gsl_vector_alloc(nDim); gsl_matrix_get_row(x1, mat, i); gsl_vector_sub(x1, &vecMu.vector); gsl_blas_dsymv(CblasUpper, 1.0, sigmaInv, x1, 0.0, x2); gsl_blas_ddot(x1, x2, &returnVal[i]); returnVal[i] = exp(normConst - 0.5*returnVal[i]); gsl_vector_free(x1); gsl_vector_free(x2); } return; }
/* matrices are linearized */ double ighmm_rand_multivariate_normal_density(int length, const double *x, double *mean, double *sigmainv, double det) { # define CUR_PROC "ighmm_rand_multivariate_normal_density" /* multivariate normal density function */ /* * length dimension of the random vetor * x point at which to evaluate the pdf * mean vector of means of size n * sigmainv inverse variance matrix of dimension n x n * det determinant of covariance matrix */ #ifdef DO_WITH_GSL int i, j; double ax,ay; gsl_vector *ym, *xm, *gmean; gsl_matrix *inv = gsl_matrix_alloc(length, length); for (i=0; i<length; ++i) { for (j=0; j<length; ++j) { gsl_matrix_set(inv, i, j, sigmainv[i*length+j]); } } xm = gsl_vector_alloc(length); gmean = gsl_vector_alloc(length); /*gsl_vector_memcpy(xm, x);*/ for (i=0; i<length; ++i) { gsl_vector_set(xm, i, x[i]); gsl_vector_set(gmean, i, mean[i]); } gsl_vector_sub(xm, gmean); ym = gsl_vector_alloc(length); gsl_blas_dsymv(CblasUpper, 1.0, inv, xm, 0.0, ym); gsl_matrix_free(inv); gsl_blas_ddot(xm, ym, &ay); gsl_vector_free(xm); gsl_vector_free(ym); ay = exp(-0.5*ay) / sqrt(pow((2*M_PI), length) * det); return ay; #else /* do without GSL */ int i, j; double ay, tempv; ay = 0; for (i=0; i<length; ++i) { tempv = 0; for (j=0; j<length; ++j) { tempv += (x[j]-mean[j])*sigmainv[j*length+i]; } ay += tempv*(x[i]-mean[i]); } ay = exp(-0.5*ay) / sqrt(pow((2*PI), length) * det); return ay; #endif # undef CUR_PROC } /* double ighmm_rand_multivariate_normal_density */
void calcZ(t_Cluster* ptCluster, t_Data *ptData){ double **aadX = ptData->aadX, **aadZ = ptCluster->aadZ; int i = 0, k = 0, l = 0; int nK = ptCluster->nK, nD = ptCluster->nD, nN = ptData->nN; gsl_vector *ptDiff = gsl_vector_alloc(nD); gsl_vector *ptRes = gsl_vector_alloc(nD); double adDist[nK], dD = (double) nD; double** aadM = ptCluster->aadM, *adPi = ptCluster->adPi; for(i = 0; i < nN; i++){ double dMinDist = DBL_MAX; double dTotalZ = 0.0; double dNTotalZ = 0.0; for(k = 0; k < nK; k++){ if(adPi[k] > 0.){ /*set vector to data point*/ for(l = 0; l < nD; l++){ gsl_vector_set(ptDiff,l,aadX[i][l] - aadM[k][l]); } gsl_blas_dsymv (CblasLower, 1.0, ptCluster->aptSigma[k], ptDiff, 0.0, ptRes); gsl_blas_ddot (ptDiff, ptRes, &adDist[k]); adDist[k] *= ptCluster->adNu[k]; adDist[k] -= ptCluster->adLDet[k]; adDist[k] += dD/ptCluster->adBeta[k]; if(adDist[k] < dMinDist){ dMinDist = adDist[k]; } } } for(k = 0; k < nK; k++){ if(adPi[k] > 0.){ aadZ[i][k] = adPi[k]*exp(-0.5*(adDist[k]-dMinDist)); dTotalZ += aadZ[i][k]; } else{ aadZ[i][k] = 0.0; } } for(k = 0; k < nK; k++){ double dF = aadZ[i][k] / dTotalZ; if(dF < MIN_Z){ aadZ[i][k] = 0.0; } dNTotalZ += aadZ[i][k]; } if(dNTotalZ > 0.){ for(k = 0; k < nK; k++){ aadZ[i][k] /= dNTotalZ; } } } gsl_vector_free(ptRes); gsl_vector_free(ptDiff); return; }
void GICPOptimizer::fdf(const gsl_vector *x, void *params, double * f, gsl_vector *g) { std::cout << ">>> fdf" << std::endl; GICPOptData *opt_data = (GICPOptData *)params; double pt1[3]; double pt2[3]; double res[3]; // residual double temp[3]; // temp local vector double temp_mat[9]; // temp matrix used for accumulating the rotation gradient gsl_vector_view gsl_pt1 = gsl_vector_view_array(pt1, 3); gsl_vector_view gsl_pt2 = gsl_vector_view_array(pt2, 3); gsl_vector_view gsl_res = gsl_vector_view_array(res, 3); gsl_vector_view gsl_temp = gsl_vector_view_array(temp, 3); gsl_vector_view gsl_gradient_t = gsl_vector_subvector(g, 0, 3); // translation comp. of gradient gsl_vector_view gsl_gradient_r = gsl_vector_subvector(g, 3, 3); // rotation comp. of gradient gsl_matrix_view gsl_temp_mat_r = gsl_matrix_view_array(temp_mat, 3, 3); gsl_matrix_view gsl_M; dgc_transform_t t; double temp_double; // take the base transformation dgc_transform_copy(t, opt_data->base_t); // apply the current state apply_state(t, x); // zero all accumulator variables *f = 0; gsl_vector_set_zero(g); gsl_vector_set_zero(&gsl_temp.vector); gsl_matrix_set_zero(&gsl_temp_mat_r.matrix); for(int i = 0; i < opt_data->p1->Size(); i++) { int j = opt_data->nn_indecies[i]; if(j != -1) { // get point 1 pt1[0] = (*opt_data->p1)[i].x; pt1[1] = (*opt_data->p1)[i].y; pt1[2] = (*opt_data->p1)[i].z; // get point 2 pt2[0] = (*opt_data->p2)[j].x; pt2[1] = (*opt_data->p2)[j].y; pt2[2] = (*opt_data->p2)[j].z; //cout << "accessing " << i << " of " << opt_data->p1->Size() << ", " << opt_data->p2->Size() << endl; //get M-matrix gsl_M = gsl_matrix_view_array(&opt_data->M[i][0][0], 3, 3); print_gsl_matrix(&gsl_M.matrix, "M"); //transform point 1 dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], t); std::cout << "pt1 " << pt1[0] << "," << pt1[1] << "," << pt1[2] << std::endl; res[0] = pt1[0] - pt2[0]; res[1] = pt1[1] - pt2[1]; res[2] = pt1[2] - pt2[2]; std::cout << "res " << res[0] << "," << res[1] << "," << res[2] << std::endl; // compute the transformed residual // temp := M*res //print_gsl_matrix(&gsl_M.matrix, "gsl_m"); gsl_blas_dsymv(CblasLower, 1., &gsl_M.matrix, &gsl_res.vector, 0., &gsl_temp.vector); print_gsl_vector(&gsl_temp.vector, "temp"); // compute M-norm of the residual // temp_double := res'*temp = temp'*M*res gsl_blas_ddot(&gsl_res.vector, &gsl_temp.vector, &temp_double); // accumulate total error: f += res'*M*res *f += temp_double/(double)opt_data->num_matches; std::cout << "f " << *f << std::endl; // accumulate translation gradient: // gsl_gradient_t += 2*M*res gsl_blas_dsymv(CblasLower, 2./(double)opt_data->num_matches, &gsl_M.matrix, &gsl_res.vector, 1., &gsl_gradient_t.vector); if(opt_data->solve_rotation) { // accumulate the rotation gradient matrix // get back the original untransformed point to compute the rotation gradient pt1[0] = (*opt_data->p1)[i].x; pt1[1] = (*opt_data->p1)[i].y; pt1[2] = (*opt_data->p1)[i].z; dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], opt_data->base_t); // gsl_temp_mat_r += 2*(gsl_temp).(gsl_pt1)' [ = (2*M*residual).(gsl_pt1)' ] gsl_blas_dger(2./(double)opt_data->num_matches, &gsl_pt1.vector, &gsl_temp.vector, &gsl_temp_mat_r.matrix); } } } print_gsl_vector(g, "gradient"); // the above loop sets up the gradient with respect to the translation, and the matrix derivative w.r.t. the rotation matrix // this code sets up the matrix derivatives dR/dPhi, dR/dPsi, dR/dTheta. i.e. the derivatives of the whole rotation matrix with respect to the euler angles // note that this code assumes the XYZ order of euler angles, with the Z rotation corresponding to bearing. This means the Z angle is negative of what it would be // in the regular XYZ euler-angle convention. if(opt_data->solve_rotation) { // now use the d/dR matrix to compute the derivative with respect to euler angles and put it directly into g[3], g[4], g[5]; compute_dr(x, &gsl_temp_mat_r.matrix, g); } print_gsl_matrix(&gsl_temp_mat_r.matrix, "R"); print_gsl_vector(g, "gradient"); std::cout << "<<< fdf" << std::endl; }
/** * C++ version of gsl_blas_dsymv(). * @param Uplo Upper or lower triangular * @param alpha A constant * @param A A matrix * @param X A vector * @param beta Another constant * @param Y A vector * @return Error code on failure */ int dsymv( CBLAS_UPLO_t Uplo, double alpha, matrix const& A, vector const& X, double beta, vector& Y ){ return gsl_blas_dsymv( Uplo, alpha, A.get(), X.get(), beta, Y.get() ); }
double* paramTransQMC_LG(double *theta, int dx, int dy) { int k,s, npx=pow(dx,2), mpy=dy*dx; int start1=dx+npx, start2=start1+(dx-1)*dx*0.5; double var; double *param=(double*)malloc(sizeof(double)*(start2+dx)); gsl_matrix *SIGMA=gsl_matrix_alloc(dx,dx); gsl_matrix *winv=NULL, *work=NULL; gsl_vector *v1=NULL, *v2=NULL; gsl_permutation *p=NULL; //Store mux vector for(k=0;k<dx;k++) { param[k]=theta[k]; } //store A matrix for(k=0;k<dx;k++) { for(s=0;s<dx;s++) { param[dx+dx*k+s]=theta[dx+dy+dx*k+s]; } } //store parameters for the variance for(k=0;k<dx;k++) { for(s=0;s<dx;s++) { gsl_matrix_set(SIGMA,k,s,theta[dx+dy+npx+mpy+dx*k+s]); } } param[start2]=pow(gsl_matrix_get(SIGMA,0,0),0.5); for(k=1;k<dx;k++) { v1=gsl_vector_alloc(k); v2=gsl_vector_alloc(k); p=gsl_permutation_alloc(k); winv=gsl_matrix_alloc(k,k); work=gsl_matrix_alloc(k,k); for(s=0;s<k;s++) { gsl_vector_set(v1,s,gsl_matrix_get(SIGMA,k,s)); //vector of covariance } gsl_matrix_view cov= gsl_matrix_submatrix(SIGMA, 0, 0, k,k); //variance-covariance matrix gsl_matrix_memcpy(work,&cov.matrix); gsl_linalg_LU_decomp(work, p, &s); gsl_linalg_LU_invert(work, p, winv); //inverse of the variance-covariance matrix gsl_blas_dsymv (CblasUpper, 1, winv, v1, 0, v2); //Sima_{i,i}^{-1}Sigma_{i,j} gsl_blas_ddot (v2, v1, &var); //Sigma_{j,i}Sima_{i,i}^{-1}Sigma_{i,j} param[start2+k]=pow(gsl_matrix_get(SIGMA,k,k)-var,0.5); //variance of x_k|x_{1:k-1} for(s=0;s<k;s++) { param[start1+((k-1)*k)/2+s]=gsl_vector_get(v2,s); } gsl_vector_free(v1); v1=NULL; gsl_vector_free(v2); v2=NULL; gsl_matrix_free(winv); winv=NULL; gsl_matrix_free(work); work=NULL; gsl_permutation_free(p); p=NULL; } gsl_matrix_free(SIGMA); SIGMA=NULL; return(param); }
void simLinearGaussianQMC(double* qseq, int dx, int start1, int start2, int N, double *theta, double *x) { int i, s,k; double var, var1,muI; gsl_matrix *SIGMA=gsl_matrix_alloc(dx,dx); gsl_matrix *winv=NULL, *work1=NULL; gsl_vector *v1=NULL, *v2=NULL; gsl_permutation *p=NULL; //Sigma for(k=0;k<dx;k++) { for(s=0;s<dx;s++) { gsl_matrix_set(SIGMA,k,s,theta[start2+dx*k+s]); } } var=pow(gsl_matrix_get(SIGMA,0,0),0.5); for(i=0;i<N;i++) //sample first dimension { x[dx*i]=var*gsl_cdf_ugaussian_Pinv(qseq[dx*i])+theta[start1]; } for(k=1;k<dx;k++) //sample other dimensions { v1=gsl_vector_alloc(k); v2=gsl_vector_alloc(k); p=gsl_permutation_alloc(k); winv=gsl_matrix_alloc(k,k); work1=gsl_matrix_alloc(k,k); for(s=0;s<k;s++) { gsl_vector_set(v1,s,gsl_matrix_get(SIGMA,k,s)); //vector of covariance } gsl_matrix_view cov= gsl_matrix_submatrix(SIGMA, 0, 0, k,k); //variance-covariance matrix gsl_matrix_memcpy(work1,&cov.matrix); gsl_linalg_LU_decomp(work1, p, &s); gsl_linalg_LU_invert(work1, p, winv); //inverse of the variance-covariance matrix gsl_blas_dsymv (CblasUpper, 1, winv, v1, 0, v2); //Sima_{i,i}^{-1}Sigma_{i,j} gsl_blas_ddot (v2, v1, &var1); //Sigma_{j,i}Sima_{i,i}^{-1}Sigma_{i,j} var=pow(gsl_matrix_get(SIGMA,k,k)-var1,0.5); //variance of x_k|x_{1:k-1} for(i=0;i<N;i++) { muI=theta[start1+k]; for(s=0;s<k;s++) { muI+=gsl_vector_get(v2,s)*(x[dx*i+s]-theta[start1+s]); } x[dx*i+k]=var*gsl_cdf_ugaussian_Pinv(qseq[dx*i+k])+muI; } gsl_vector_free(v1); v1=NULL; gsl_vector_free(v2); v2=NULL; gsl_matrix_free(winv); winv=NULL; gsl_matrix_free(work1); work1=NULL; gsl_permutation_free(p); p=NULL; } gsl_matrix_free(SIGMA); SIGMA=NULL; }
// GICP cost function double GICPOptimizer::f(const gsl_vector *x, void *params) { GICPOptData *opt_data = (GICPOptData *)params; double pt1[3]; double pt2[3]; double res[3]; // residual double temp[3]; gsl_vector_view gsl_pt1 = gsl_vector_view_array(pt1, 3); gsl_vector_view gsl_pt2 = gsl_vector_view_array(pt2, 3); gsl_vector_view gsl_res = gsl_vector_view_array(res, 3); gsl_vector_view gsl_temp = gsl_vector_view_array(temp, 3); gsl_matrix_view gsl_M; dgc_transform_t t; // initialize the temp variable; if it happens to be NaN at start, bad things will happen in blas routines below temp[0] = 0; temp[1] = 0; temp[2] = 0; // take the base transformation dgc_transform_copy(t, opt_data->base_t); // apply the current state apply_state(t, x); double f = 0; double temp_double = 0; int N = opt_data->p1->Size(); int counter = 0; for(int i = 0; i < N; i++) { int j = opt_data->nn_indecies[i]; if(j != -1) { // get point 1 pt1[0] = (*opt_data->p1)[i].x; pt1[1] = (*opt_data->p1)[i].y; pt1[2] = (*opt_data->p1)[i].z; // get point 2 pt2[0] = (*opt_data->p2)[j].x; pt2[1] = (*opt_data->p2)[j].y; pt2[2] = (*opt_data->p2)[j].z; //get M-matrix gsl_M = gsl_matrix_view_array(&opt_data->M[i][0][0], 3, 3); //transform point 1 dgc_transform_point(&pt1[0], &pt1[1], &pt1[2], t); res[0] = pt1[0] - pt2[0]; res[1] = pt1[1] - pt2[1]; res[2] = pt1[2] - pt2[2]; //cout << "res: (" << res[0] << ", " <<res[1] <<", " << res[2] << ")" << endl; // temp := M*res gsl_blas_dsymv(CblasLower, 1., &gsl_M.matrix, &gsl_res.vector, 0., &gsl_temp.vector); // temp_double := res'*temp = temp'*M*temp gsl_blas_ddot(&gsl_res.vector, &gsl_temp.vector, &temp_double); // increment total error f += temp_double/(double)opt_data->num_matches; //cout << "temp: " << temp_double << endl; //cout << "f: " << f << "\t (" << opt_data->num_matches << ")" << endl; //print_gsl_matrix(&gsl_M.matrix, "M"); counter++; } } printf("counter %d\n",counter); return f; }
double calcVBL(t_Cluster* ptCluster) { int i = 0, k = 0, l = 0, nN = ptCluster->nN; int nK = ptCluster->nK, nD = ptCluster->nD; double dBishop1 = 0.0, dBishop2 = 0.0, dBishop3 = 0.0, dBishop4 = 0.0, dBishop5 = 0.0; /*Bishop equations 10.71...*/ gsl_matrix *ptRes = gsl_matrix_alloc(nD,nD); gsl_vector *ptDiff = gsl_vector_alloc(nD); gsl_vector *ptR = gsl_vector_alloc(nD); double dD = (double) nD; double** aadMu = ptCluster->aadMu, **aadM = ptCluster->aadM, **aadZ = ptCluster->aadZ; double* adBeta = ptCluster->adBeta, *adNu = ptCluster->adNu, *adLDet = ptCluster->adLDet, *adPi = ptCluster->adPi; double adNK[nK]; double d2Pi = 2.0*M_PI, dBeta0 = ptCluster->ptVBParams->dBeta0, dNu0 = ptCluster->ptVBParams->dNu0, dRet = 0.0; double dK = 0.0; for(k = 0; k < nK; k++){ adNK[k] = 0.0; } /*Equation 10.72*/ for(i = 0; i < nN; i++){ for(k = 0; k < nK; k++){ adNK[k] += aadZ[i][k]; if(adPi[k] > 0.0){ dBishop2 += aadZ[i][k]*log(adPi[k]); } } } for(k = 0; k < nK; k++){ if(adNK[k] > 0.0){ dK++; } } /*Equation 10.71*/ for(k = 0; k < nK; k++){ if(adNK[k] > 0.0){ double dT1 = 0.0, dT2 = 0.0, dF = 0.0; gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0,ptCluster->aptCovar[k],ptCluster->aptSigma[k],0.0,ptRes); for(l = 0; l < nD; l++){ dT1 += gsl_matrix_get(ptRes,l,l); } for(l = 0; l < nD; l++){ gsl_vector_set(ptDiff,l,aadMu[k][l] - aadM[k][l]); } gsl_blas_dsymv (CblasLower, 1.0, ptCluster->aptSigma[k], ptDiff, 0.0, ptR); gsl_blas_ddot (ptDiff, ptR, &dT2); dF = adLDet[k] - adNu[k]*(dT1 + dT2) - dD*(log(d2Pi) + (1.0/adBeta[k])); dBishop1 += 0.5*adNK[k]*dF; } } /*Equation 10.74*/ for(k = 0; k < nK; k++){ if(adNK[k] > 0.0){ double dT1 = 0.0, dT2 = 0.0, dF = 0.0; gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0,ptCluster->ptVBParams->ptInvW0,ptCluster->aptSigma[k],0.0,ptRes); for(l = 0; l < nD; l++){ dT1 += gsl_matrix_get(ptRes,l,l); } for(l = 0; l < nD; l++){ gsl_vector_set(ptDiff,l,aadM[k][l]); } gsl_blas_dsymv (CblasLower, 1.0, ptCluster->aptSigma[k], ptDiff, 0.0, ptR); gsl_blas_ddot (ptDiff, ptR, &dT2); dF = dD*log(dBeta0/d2Pi) + adLDet[k] - ((dD*dBeta0)/adBeta[k]) - dBeta0*adNu[k]*dT2 - adNu[k]*dT1; dBishop3 += 0.5*(dF + (dNu0 - dD - 1.0)*adLDet[k]); } } dBishop3 += dK*ptCluster->ptVBParams->dLogWishartB; /*Equation 10.75*/ for(i = 0; i < nN; i++){ for(k = 0; k < nK; k++){ if(aadZ[i][k] > 0.0){ dBishop4 += aadZ[i][k]*log(aadZ[i][k]); } } } /*Equation 10.77*/ for(k = 0; k < nK; k++){ if(adNK[k] > 0.0){ dBishop5 += 0.5*adLDet[k] + 0.5*dD*log(adBeta[k]/d2Pi) - 0.5*dD - dWishartExpectLogDet(ptCluster->aptSigma[k], adNu[k], nD); } } gsl_matrix_free(ptRes); gsl_vector_free(ptDiff); gsl_vector_free(ptR); dRet = dBishop1 + dBishop2 + dBishop3 - dBishop4 - dBishop5; return dRet; }