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