/*note assuming you are using inverse W matrix*/ double dLogWishartB(gsl_matrix *ptInvW, int nD, double dNu, int bInv) { int i = 0; double dRet = 0.0, dT = 0.0; double dLogDet = 0.0, dD = (double) nD; gsl_matrix* ptTemp = gsl_matrix_alloc(nD,nD); gsl_matrix_memcpy(ptTemp, ptInvW); dLogDet = decomposeMatrix(ptTemp, nD); if(bInv == TRUE){ dRet = 0.5*dNu*dLogDet; } else{ dRet = -0.5*dNu*dLogDet; } dT = 0.5*dNu*dD*log(2.0); dT += 0.25*dD*(dD - 1.0)*log(M_PI); for(i = 0; i < nD; i++){ dT += gsl_sf_lngamma(0.5*(dNu - (double) i)); } gsl_matrix_free(ptTemp); return dRet - dT; }
double dWishartExpectLogDet(gsl_matrix *ptW, double dNu, int nD) { int i = 0; double dRet = 0.0, dLogDet = 0.0, dD = (double) nD; gsl_matrix* ptTemp = gsl_matrix_alloc(nD,nD); gsl_matrix_memcpy(ptTemp, ptW); dLogDet = decomposeMatrix(ptW, nD); dRet = dD*log(2.0) + dLogDet; for(i = 0; i < nD; i++){ dRet += gsl_sf_psi(0.5*(dNu - (double) i)); } gsl_matrix_free(ptTemp); return dRet; }
static void __DecomposeMatrices(float *matrices, size_t count, std::vector< shared_ptr <GLTFBufferView> > &TRSBufferViews) { size_t translationBufferSize = sizeof(float) * 3 * count; size_t rotationBufferSize = sizeof(float) * 4 * count; size_t scaleBufferSize = sizeof(float) * 3 * count; float *translationData = (float*)malloc(translationBufferSize); float *rotationData = (float*)malloc(rotationBufferSize); float *scaleData = (float*)malloc(scaleBufferSize); shared_ptr <GLTF::GLTFBufferView> translationBufferView = createBufferViewWithAllocatedBuffer(translationData, 0, translationBufferSize, true); shared_ptr <GLTF::GLTFBufferView> rotationBufferView = createBufferViewWithAllocatedBuffer(rotationData, 0, rotationBufferSize, true); shared_ptr <GLTF::GLTFBufferView> scaleBufferView = createBufferViewWithAllocatedBuffer(scaleData, 0, scaleBufferSize, true); float *previousRotation = 0; for (size_t i = 0 ; i < count ; i++) { float *m = matrices; COLLADABU::Math::Matrix4 mat; mat.setAllElements(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8], m[9], m[10], m[11], m[12], m[13], m[14], m[15] ); decomposeMatrix(mat, translationData, rotationData, scaleData); //make sure we export the short path from orientations if (0 != previousRotation) { COLLADABU::Math::Vector3 axis1(previousRotation[0], previousRotation[1], previousRotation[2]); COLLADABU::Math::Vector3 axis2(rotationData[0], rotationData[1], rotationData[2]); COLLADABU::Math::Quaternion key1; COLLADABU::Math::Quaternion key2; key1.fromAngleAxis(previousRotation[3], axis1); key2.fromAngleAxis(rotationData[3], axis2); COLLADABU::Math::Real cosHalfTheta = key1.dot(key2); if (cosHalfTheta < 0) { key2.x = -key2.x; key2.y = -key2.y; key2.z = -key2.z; key2.w = -key2.w; COLLADABU::Math::Real angle; key2.toAngleAxis(angle, axis2); rotationData[3] = (float)angle; rotationData[0] = (float)axis2.x; rotationData[1] = (float)axis2.y; rotationData[2] = (float)axis2.z; key2.fromAngleAxis(rotationData[3], axis2); //FIXME: this needs to be refined, we ensure continuity here, but assume in clockwise order cosHalfTheta = key1.dot(key2); if (cosHalfTheta < 0) { rotationData[3] += (float)(2. * 3.14159265359); key2.fromAngleAxis(rotationData[3], axis2); } } } previousRotation = rotationData; translationData += 3; rotationData += 4; scaleData += 3; matrices += 16; } /* rotationData = (float*)rotationBufferView->getBufferDataByApplyingOffset(); for (size_t i = 0 ; i < count ; i++) { printf("rotation at:%d %f %f %f %f\n", i, rotationData[0],rotationData[1],rotationData[2],rotationData[3]); rotationData += 4; } */ TRSBufferViews.push_back(translationBufferView); TRSBufferViews.push_back(rotationBufferView); TRSBufferViews.push_back(scaleBufferView); }
void calcCovarMatricesVB(t_Cluster *ptCluster, t_Data *ptData){ int i = 0, j = 0, k = 0, l = 0, m = 0; int nN = ptData->nN, nK = ptCluster->nK, nD = ptData->nD; double **aadZ = ptCluster->aadZ,**aadX = ptData->aadX; double *adLDet = ptCluster->adLDet, *adPi = ptCluster->adPi; double **aadCovar = NULL, **aadInvWK = NULL; t_VBParams *ptVBParams = ptCluster->ptVBParams; aadCovar = (double **) malloc(nD*sizeof(double*)); if(!aadCovar) goto memoryError; for(i = 0; i < nD; i++){ aadCovar[i] = (double *) malloc(nD*sizeof(double)); if(!aadCovar[i]) goto memoryError; } aadInvWK = (double **) malloc(nD*sizeof(double*)); if(!aadInvWK) goto memoryError; for(i = 0; i < nD; i++){ aadInvWK[i] = (double *) malloc(nD*sizeof(double)); if(!aadInvWK[i]) goto memoryError; } /*perform M step*/ for(k = 0; k < nK; k++){ /*loop components*/ double* adMu = ptCluster->aadMu[k]; gsl_matrix *ptSigmaMatrix = ptCluster->aptSigma[k]; double dF = 0.0; /*recompute mixture weights and means*/ for(j = 0; j < nD; j++){ adMu[j] = 0.0; for(l = 0; l < nD; l++){ aadCovar[j][l] = 0.0; } } /* compute weight associated with component k*/ adPi[k] = 0.0; for(i = 0; i < nN; i++){ adPi[k] += aadZ[i][k]; for(j = 0; j < nD; j++){ adMu[j] += aadZ[i][k]*aadX[i][j]; } } /*normalise means*/ if(adPi[k] > MIN_PI){ /*Equation 10.60*/ ptCluster->adBeta[k] = ptVBParams->dBeta0 + adPi[k]; for(j = 0; j < nD; j++){ /*Equation 10.61*/ ptCluster->aadM[k][j] = adMu[j]/ptCluster->adBeta[k]; adMu[j] /= adPi[k]; } ptCluster->adNu[k] = ptVBParams->dNu0 + adPi[k]; /*calculate covariance matrices*/ for(i = 0; i < nN; i++){ double adDiff[nD]; for(j = 0; j < nD; j++){ adDiff[j] = aadX[i][j] - adMu[j]; } for(l = 0; l < nD; l++){ for(m = 0; m <=l ; m++){ aadCovar[l][m] += aadZ[i][k]*adDiff[l]*adDiff[m]; } } } for(l = 0; l < nD; l++){ for(m = l + 1; m < nD; m++){ aadCovar[l][m] = aadCovar[m][l]; } } /*save sample covariances for later use*/ for(l = 0; l < nD; l++){ for(m = 0; m < nD; m++){ double dC = aadCovar[l][m] / adPi[k]; gsl_matrix_set(ptCluster->aptCovar[k],l,m,dC); } } /*Now perform equation 10.62*/ dF = (ptVBParams->dBeta0*adPi[k])/ptCluster->adBeta[k]; for(l = 0; l < nD; l++){ for(m = 0; m <= l; m++){ aadInvWK[l][m] = gsl_matrix_get(ptVBParams->ptInvW0, l,m) + aadCovar[l][m] + dF*adMu[l]*adMu[m]; } } for(l = 0; l < nD; l++){ for(m = 0; m <= l ; m++){ //aadCovar[l][m] /= adPi[k]; gsl_matrix_set(ptSigmaMatrix, l, m, aadInvWK[l][m]); gsl_matrix_set(ptSigmaMatrix, m, l, aadInvWK[l][m]); } } } else{ /*Equation 10.60*/ adPi[k] = 0.0; ptCluster->adBeta[k] = ptVBParams->dBeta0; for(j = 0; j < nD; j++){ /*Equation 10.61*/ ptCluster->aadM[k][j] = 0.0; adMu[j] = 0.0; } ptCluster->adNu[k] = ptVBParams->dNu0; for(l = 0; l < nD; l++){ for(m = 0; m <= l; m++){ aadInvWK[l][m] = gsl_matrix_get(ptVBParams->ptInvW0, l,m); } } for(l = 0; l < nD; l++){ for(m = 0; m <= l ; m++){ //aadCovar[l][m] /= adPi[k]; gsl_matrix_set(ptSigmaMatrix, l, m, aadInvWK[l][m]); gsl_matrix_set(ptSigmaMatrix, m, l, aadInvWK[l][m]); } } /*Implement Equation 10.65*/ adLDet[k] = ((double) nD)*log(2.0); for(l = 0; l < nD; l++){ double dX = 0.5*(ptCluster->adNu[k] - (double) l); adLDet[k] += gsl_sf_psi (dX); } adLDet[k] -= decomposeMatrix(ptSigmaMatrix,nD); } } /*Normalise pi*/ if(1){ double dNP = 0.0; for(k = 0; k < nK; k++){ dNP += adPi[k]; } for(k = 0; k < nK; k++){ adPi[k] /= dNP; } } /*free up memory*/ for(i = 0; i < nD; i++){ free(aadCovar[i]); free(aadInvWK[i]); } //gsl_matrix_free(ptSigmaMatrix); free(aadCovar); free(aadInvWK); return; memoryError: fprintf(stderr, "Failed allocating memory in performMStep\n"); fflush(stderr); exit(EXIT_FAILURE); }