/**
* 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;
}
