| 1 | #include "EMGLLF.h" |
| 2 | #include <gsl/gsl_linalg.h> |
| 3 | |
| 4 | // TODO: don't recompute indexes every time...... |
| 5 | void EMGLLF( |
| 6 | // IN parameters |
| 7 | const double* phiInit, // parametre initial de moyenne renormalisé |
| 8 | const double* rhoInit, // parametre initial de variance renormalisé |
| 9 | const double* piInit, // parametre initial des proportions |
| 10 | const double* gamInit, // paramètre initial des probabilités a posteriori de chaque échantillon |
| 11 | int mini, // nombre minimal d'itérations dans l'algorithme EM |
| 12 | int maxi, // nombre maximal d'itérations dans l'algorithme EM |
| 13 | double gamma, // valeur de gamma : puissance des proportions dans la pénalisation pour un Lasso adaptatif |
| 14 | double lambda, // valeur du paramètre de régularisation du Lasso |
| 15 | const double* X, // régresseurs |
| 16 | const double* Y, // réponse |
| 17 | double tau, // seuil pour accepter la convergence |
| 18 | // OUT parameters (all pointers, to be modified) |
| 19 | double* phi, // parametre de moyenne renormalisé, calculé par l'EM |
| 20 | double* rho, // parametre de variance renormalisé, calculé par l'EM |
| 21 | double* pi, // parametre des proportions renormalisé, calculé par l'EM |
| 22 | double* LLF, // log vraisemblance associé à cet échantillon, pour les valeurs estimées des paramètres |
| 23 | double* S, |
| 24 | // additional size parameters |
| 25 | int n, // nombre d'echantillons |
| 26 | int p, // nombre de covariables |
| 27 | int m, // taille de Y (multivarié) |
| 28 | int k) // nombre de composantes dans le mélange |
| 29 | { |
| 30 | //Initialize outputs |
| 31 | copyArray(phiInit, phi, p*m*k); |
| 32 | copyArray(rhoInit, rho, m*m*k); |
| 33 | copyArray(piInit, pi, k); |
| 34 | zeroArray(LLF, maxi); |
| 35 | //S is already allocated, and doesn't need to be 'zeroed' |
| 36 | |
| 37 | //Other local variables |
| 38 | //NOTE: variables order is always [maxi],n,p,m,k |
| 39 | double* gam = (double*)malloc(n*k*sizeof(double)); |
| 40 | copyArray(gamInit, gam, n*k); |
| 41 | double* b = (double*)malloc(k*sizeof(double)); |
| 42 | double* Phi = (double*)malloc(p*m*k*sizeof(double)); |
| 43 | double* Rho = (double*)malloc(m*m*k*sizeof(double)); |
| 44 | double* Pi = (double*)malloc(k*sizeof(double)); |
| 45 | double* gam2 = (double*)malloc(k*sizeof(double)); |
| 46 | double* pi2 = (double*)malloc(k*sizeof(double)); |
| 47 | double* Gram2 = (double*)malloc(p*p*k*sizeof(double)); |
| 48 | double* ps = (double*)malloc(m*k*sizeof(double)); |
| 49 | double* nY2 = (double*)malloc(m*k*sizeof(double)); |
| 50 | double* ps1 = (double*)malloc(n*m*k*sizeof(double)); |
| 51 | double* ps2 = (double*)malloc(p*m*k*sizeof(double)); |
| 52 | double* nY21 = (double*)malloc(n*m*k*sizeof(double)); |
| 53 | double* Gam = (double*)malloc(n*k*sizeof(double)); |
| 54 | double* X2 = (double*)malloc(n*p*k*sizeof(double)); |
| 55 | double* Y2 = (double*)malloc(n*m*k*sizeof(double)); |
| 56 | gsl_matrix* matrix = gsl_matrix_alloc(m, m); |
| 57 | gsl_permutation* permutation = gsl_permutation_alloc(m); |
| 58 | double* YiRhoR = (double*)malloc(m*sizeof(double)); |
| 59 | double* XiPhiR = (double*)malloc(m*sizeof(double)); |
| 60 | double dist = 0.; |
| 61 | double dist2 = 0.; |
| 62 | int ite = 0; |
| 63 | double EPS = 1e-15; |
| 64 | double* dotProducts = (double*)malloc(k*sizeof(double)); |
| 65 | |
| 66 | while (ite < mini || (ite < maxi && (dist >= tau || dist2 >= sqrt(tau)))) |
| 67 | { |
| 68 | copyArray(phi, Phi, p*m*k); |
| 69 | copyArray(rho, Rho, m*m*k); |
| 70 | copyArray(pi, Pi, k); |
| 71 | |
| 72 | // Calculs associés a Y et X |
| 73 | for (int r=0; r<k; r++) |
| 74 | { |
| 75 | for (int mm=0; mm<m; mm++) |
| 76 | { |
| 77 | //Y2(:,mm,r)=sqrt(gam(:,r)).*transpose(Y(mm,:)); |
| 78 | for (int u=0; u<n; u++) |
| 79 | Y2[ai(u,mm,r,n,m,k)] = sqrt(gam[mi(u,r,n,k)]) * Y[mi(u,mm,m,n)]; |
| 80 | } |
| 81 | for (int i=0; i<n; i++) |
| 82 | { |
| 83 | //X2(i,:,r)=X(i,:).*sqrt(gam(i,r)); |
| 84 | for (int u=0; u<p; u++) |
| 85 | X2[ai(i,u,r,n,m,k)] = sqrt(gam[mi(i,r,n,k)]) * X[mi(i,u,n,p)]; |
| 86 | } |
| 87 | for (int mm=0; mm<m; mm++) |
| 88 | { |
| 89 | //ps2(:,mm,r)=transpose(X2(:,:,r))*Y2(:,mm,r); |
| 90 | for (int u=0; u<p; u++) |
| 91 | { |
| 92 | double dotProduct = 0.; |
| 93 | for (int v=0; v<n; v++) |
| 94 | dotProduct += X2[ai(v,u,r,n,m,k)] * Y2[ai(v,mm,r,n,m,k)]; |
| 95 | ps2[ai(u,mm,r,n,m,k)] = dotProduct; |
| 96 | } |
| 97 | } |
| 98 | for (int j=0; j<p; j++) |
| 99 | { |
| 100 | for (int s=0; s<p; s++) |
| 101 | { |
| 102 | //Gram2(j,s,r)=transpose(X2(:,j,r))*(X2(:,s,r)); |
| 103 | double dotProduct = 0.; |
| 104 | for (int u=0; u<n; u++) |
| 105 | dotProduct += X2[ai(u,j,r,n,p,k)] * X2[ai(u,s,r,n,p,k)]; |
| 106 | Gram2[ai(j,s,r,p,p,k)] = dotProduct; |
| 107 | } |
| 108 | } |
| 109 | } |
| 110 | |
| 111 | ///////////// |
| 112 | // Etape M // |
| 113 | ///////////// |
| 114 | |
| 115 | // Pour pi |
| 116 | for (int r=0; r<k; r++) |
| 117 | { |
| 118 | //b(r) = sum(sum(abs(phi(:,:,r)))); |
| 119 | double sumAbsPhi = 0.; |
| 120 | for (int u=0; u<p; u++) |
| 121 | for (int v=0; v<m; v++) |
| 122 | sumAbsPhi += fabs(phi[ai(u,v,r,p,m,k)]); |
| 123 | b[r] = sumAbsPhi; |
| 124 | } |
| 125 | //gam2 = sum(gam,1); |
| 126 | for (int u=0; u<k; u++) |
| 127 | { |
| 128 | double sumOnColumn = 0.; |
| 129 | for (int v=0; v<n; v++) |
| 130 | sumOnColumn += gam[mi(v,u,n,k)]; |
| 131 | gam2[u] = sumOnColumn; |
| 132 | } |
| 133 | //a=sum(gam*transpose(log(pi))); |
| 134 | double a = 0.; |
| 135 | for (int u=0; u<n; u++) |
| 136 | { |
| 137 | double dotProduct = 0.; |
| 138 | for (int v=0; v<k; v++) |
| 139 | dotProduct += gam[mi(u,v,n,k)] * log(pi[v]); |
| 140 | a += dotProduct; |
| 141 | } |
| 142 | |
| 143 | //tant que les proportions sont negatives |
| 144 | int kk = 0; |
| 145 | int pi2AllPositive = 0; |
| 146 | double invN = 1./n; |
| 147 | while (!pi2AllPositive) |
| 148 | { |
| 149 | //pi2(:)=pi(:)+0.1^kk*(1/n*gam2(:)-pi(:)); |
| 150 | for (int r=0; r<k; r++) |
| 151 | pi2[r] = pi[r] + pow(0.1,kk) * (invN*gam2[r] - pi[r]); |
| 152 | pi2AllPositive = 1; |
| 153 | for (int r=0; r<k; r++) |
| 154 | { |
| 155 | if (pi2[r] < 0) |
| 156 | { |
| 157 | pi2AllPositive = 0; |
| 158 | break; |
| 159 | } |
| 160 | } |
| 161 | kk++; |
| 162 | } |
| 163 | |
| 164 | //t(m) la plus grande valeur dans la grille O.1^k tel que ce soit décroissante ou constante |
| 165 | //(pi.^gamma)*b |
| 166 | double piPowGammaDotB = 0.; |
| 167 | for (int v=0; v<k; v++) |
| 168 | piPowGammaDotB += pow(pi[v],gamma) * b[v]; |
| 169 | //(pi2.^gamma)*b |
| 170 | double pi2PowGammaDotB = 0.; |
| 171 | for (int v=0; v<k; v++) |
| 172 | pi2PowGammaDotB += pow(pi2[v],gamma) * b[v]; |
| 173 | //transpose(gam2)*log(pi2) |
| 174 | double prodGam2logPi2 = 0.; |
| 175 | for (int v=0; v<k; v++) |
| 176 | prodGam2logPi2 += gam2[v] * log(pi2[v]); |
| 177 | while (-invN*a + lambda*piPowGammaDotB < -invN*prodGam2logPi2 + lambda*pi2PowGammaDotB && kk<1000) |
| 178 | { |
| 179 | //pi2=pi+0.1^kk*(1/n*gam2-pi); |
| 180 | for (int v=0; v<k; v++) |
| 181 | pi2[v] = pi[v] + pow(0.1,kk) * (invN*gam2[v] - pi[v]); |
| 182 | //pi2 was updated, so we recompute pi2PowGammaDotB and prodGam2logPi2 |
| 183 | pi2PowGammaDotB = 0.; |
| 184 | for (int v=0; v<k; v++) |
| 185 | pi2PowGammaDotB += pow(pi2[v],gamma) * b[v]; |
| 186 | prodGam2logPi2 = 0.; |
| 187 | for (int v=0; v<k; v++) |
| 188 | prodGam2logPi2 += gam2[v] * log(pi2[v]); |
| 189 | kk++; |
| 190 | } |
| 191 | double t = pow(0.1,kk); |
| 192 | //sum(pi+t*(pi2-pi)) |
| 193 | double sumPiPlusTbyDiff = 0.; |
| 194 | for (int v=0; v<k; v++) |
| 195 | sumPiPlusTbyDiff += (pi[v] + t*(pi2[v] - pi[v])); |
| 196 | //pi=(pi+t*(pi2-pi))/sum(pi+t*(pi2-pi)); |
| 197 | for (int v=0; v<k; v++) |
| 198 | pi[v] = (pi[v] + t*(pi2[v] - pi[v])) / sumPiPlusTbyDiff; |
| 199 | |
| 200 | //Pour phi et rho |
| 201 | for (int r=0; r<k; r++) |
| 202 | { |
| 203 | for (int mm=0; mm<m; mm++) |
| 204 | { |
| 205 | for (int i=0; i<n; i++) |
| 206 | { |
| 207 | //< X2(i,:,r) , phi(:,mm,r) > |
| 208 | double dotProduct = 0.0; |
| 209 | for (int u=0; u<p; u++) |
| 210 | dotProduct += X2[ai(i,u,r,n,p,k)] * phi[ai(u,mm,r,n,m,k)]; |
| 211 | //ps1(i,mm,r)=Y2(i,mm,r)*dot(X2(i,:,r),phi(:,mm,r)); |
| 212 | ps1[ai(i,mm,r,n,m,k)] = Y2[ai(i,mm,r,n,m,k)] * dotProduct; |
| 213 | nY21[ai(i,mm,r,n,m,k)] = Y2[ai(i,mm,r,n,m,k)] * Y2[ai(i,mm,r,n,m,k)]; |
| 214 | } |
| 215 | //ps(mm,r)=sum(ps1(:,mm,r)); |
| 216 | double sumPs1 = 0.0; |
| 217 | for (int u=0; u<n; u++) |
| 218 | sumPs1 += ps1[ai(u,mm,r,n,m,k)]; |
| 219 | ps[mi(mm,r,m,k)] = sumPs1; |
| 220 | //nY2(mm,r)=sum(nY21(:,mm,r)); |
| 221 | double sumNy21 = 0.0; |
| 222 | for (int u=0; u<n; u++) |
| 223 | sumNy21 += nY21[ai(u,mm,r,n,m,k)]; |
| 224 | nY2[mi(mm,r,m,k)] = sumNy21; |
| 225 | //rho(mm,mm,r)=((ps(mm,r)+sqrt(ps(mm,r)^2+4*nY2(mm,r)*(gam2(r))))/(2*nY2(mm,r))); |
| 226 | rho[ai(mm,mm,k,m,m,k)] = ( ps[mi(mm,r,m,k)] + sqrt( ps[mi(mm,r,m,k)]*ps[mi(mm,r,m,k)] |
| 227 | + 4*nY2[mi(mm,r,m,k)] * (gam2[r]) ) ) / (2*nY2[mi(mm,r,m,k)]); |
| 228 | } |
| 229 | } |
| 230 | for (int r=0; r<k; r++) |
| 231 | { |
| 232 | for (int j=0; j<p; j++) |
| 233 | { |
| 234 | for (int mm=0; mm<m; mm++) |
| 235 | { |
| 236 | //sum(phi(1:j-1,mm,r).*transpose(Gram2(j,1:j-1,r)))+sum(phi(j+1:p,mm,r).*transpose(Gram2(j,j+1:p,r))) |
| 237 | double dotPhiGram2 = 0.0; |
| 238 | for (int u=0; u<j; u++) |
| 239 | dotPhiGram2 += phi[ai(u,mm,r,p,m,k)] * Gram2[ai(j,u,r,p,p,k)]; |
| 240 | for (int u=j+1; u<p; u++) |
| 241 | dotPhiGram2 += phi[ai(u,mm,r,p,m,k)] * Gram2[ai(j,u,r,p,p,k)]; |
| 242 | //S(j,r,mm)=-rho(mm,mm,r)*ps2(j,mm,r)+sum(phi(1:j-1,mm,r).*transpose(Gram2(j,1:j-1,r))) |
| 243 | // +sum(phi(j+1:p,mm,r).*transpose(Gram2(j,j+1:p,r))); |
| 244 | S[ai(j,mm,r,p,m,k)] = -rho[ai(mm,mm,r,m,m,k)] * ps2[ai(j,mm,r,p,m,k)] + dotPhiGram2; |
| 245 | if (fabs(S[ai(j,mm,r,p,m,k)]) <= n*lambda*pow(pi[r],gamma)) |
| 246 | phi[ai(j,mm,r,p,m,k)] = 0; |
| 247 | else if (S[ai(j,mm,r,p,m,k)] > n*lambda*pow(pi[r],gamma)) |
| 248 | phi[ai(j,mm,r,p,m,k)] = (n*lambda*pow(pi[r],gamma) - S[ai(j,mm,r,p,m,k)]) |
| 249 | / Gram2[ai(j,j,r,p,p,k)]; |
| 250 | else |
| 251 | phi[ai(j,mm,r,p,m,k)] = -(n*lambda*pow(pi[r],gamma) + S[ai(j,mm,r,p,m,k)]) |
| 252 | / Gram2[ai(j,j,r,p,p,k)]; |
| 253 | } |
| 254 | } |
| 255 | } |
| 256 | |
| 257 | ///////////// |
| 258 | // Etape E // |
| 259 | ///////////// |
| 260 | |
| 261 | int signum; |
| 262 | double sumLogLLF2 = 0.0; |
| 263 | for (int i=0; i<n; i++) |
| 264 | { |
| 265 | double sumLLF1 = 0.0; |
| 266 | double sumGamI = 0.0; |
| 267 | double minDotProduct = INFINITY; |
| 268 | |
| 269 | for (int r=0; r<k; r++) |
| 270 | { |
| 271 | //Compute |
| 272 | //Gam(i,r) = Pi(r) * det(Rho(:,:,r)) * exp( -1/2 * (Y(i,:)*Rho(:,:,r) - X(i,:)... |
| 273 | // *phi(:,:,r)) * transpose( Y(i,:)*Rho(:,:,r) - X(i,:)*phi(:,:,r) ) ); |
| 274 | //split in several sub-steps |
| 275 | |
| 276 | //compute Y(i,:)*rho(:,:,r) |
| 277 | for (int u=0; u<m; u++) |
| 278 | { |
| 279 | YiRhoR[u] = 0.0; |
| 280 | for (int v=0; v<m; v++) |
| 281 | YiRhoR[u] += Y[imi(i,v,n,m)] * rho[ai(v,u,r,m,m,k)]; |
| 282 | } |
| 283 | |
| 284 | //compute X(i,:)*phi(:,:,r) |
| 285 | for (int u=0; u<m; u++) |
| 286 | { |
| 287 | XiPhiR[u] = 0.0; |
| 288 | for (int v=0; v<p; v++) |
| 289 | XiPhiR[u] += X[mi(i,v,n,p)] * phi[ai(v,u,r,p,m,k)]; |
| 290 | } |
| 291 | |
| 292 | // compute dotProduct < Y(:,i)*rho(:,:,r)-X(i,:)*phi(:,:,r) . Y(:,i)*rho(:,:,r)-X(i,:)*phi(:,:,r) > |
| 293 | dotProducts[r] = 0.0; |
| 294 | for (int u=0; u<m; u++) |
| 295 | dotProducts[r] += (YiRhoR[u]-XiPhiR[u]) * (YiRhoR[u]-XiPhiR[u]); |
| 296 | if (dotProducts[r] < minDotProduct) |
| 297 | minDotProduct = dotProducts[r]; |
| 298 | } |
| 299 | double shift = 0.5*minDotProduct; |
| 300 | for (int r=0; r<k; r++) |
| 301 | { |
| 302 | //compute det(rho(:,:,r)) [TODO: avoid re-computations] |
| 303 | for (int u=0; u<m; u++) |
| 304 | { |
| 305 | for (int v=0; v<m; v++) |
| 306 | matrix->data[u*m+v] = rho[ai(u,v,r,m,m,k)]; |
| 307 | } |
| 308 | gsl_linalg_LU_decomp(matrix, permutation, &signum); |
| 309 | double detRhoR = gsl_linalg_LU_det(matrix, signum); |
| 310 | |
| 311 | Gam[mi(i,r,n,k)] = pi[r] * detRhoR * exp(-0.5*dotProducts[r] + shift); |
| 312 | sumLLF1 += Gam[mi(i,r,n,k)] / pow(2*M_PI,m/2.0); |
| 313 | sumGamI += Gam[mi(i,r,n,k)]; |
| 314 | } |
| 315 | sumLogLLF2 += log(sumLLF1); |
| 316 | for (int r=0; r<k; r++) |
| 317 | { |
| 318 | //gam(i,r)=Gam(i,r)/sum(Gam(i,:)); |
| 319 | gam[mi(i,r,n,k)] = sumGamI > EPS |
| 320 | ? Gam[mi(i,r,n,k)] / sumGamI |
| 321 | : 0.0; |
| 322 | } |
| 323 | } |
| 324 | |
| 325 | //sum(pen(ite,:)) |
| 326 | double sumPen = 0.0; |
| 327 | for (int r=0; r<k; r++) |
| 328 | sumPen += pow(pi[r],gamma) * b[r]; |
| 329 | //LLF(ite)=-1/n*sum(log(LLF2(ite,:)))+lambda*sum(pen(ite,:)); |
| 330 | LLF[ite] = -invN * sumLogLLF2 + lambda * sumPen; |
| 331 | if (ite == 0) |
| 332 | dist = LLF[ite]; |
| 333 | else |
| 334 | dist = (LLF[ite] - LLF[ite-1]) / (1.0 + fabs(LLF[ite])); |
| 335 | |
| 336 | //Dist1=max(max((abs(phi-Phi))./(1+abs(phi)))); |
| 337 | double Dist1 = 0.0; |
| 338 | for (int u=0; u<p; u++) |
| 339 | { |
| 340 | for (int v=0; v<m; v++) |
| 341 | { |
| 342 | for (int w=0; w<k; w++) |
| 343 | { |
| 344 | double tmpDist = fabs(phi[ai(u,v,w,p,m,k)]-Phi[ai(u,v,w,p,m,k)]) |
| 345 | / (1.0+fabs(phi[ai(u,v,w,p,m,k)])); |
| 346 | if (tmpDist > Dist1) |
| 347 | Dist1 = tmpDist; |
| 348 | } |
| 349 | } |
| 350 | } |
| 351 | //Dist2=max(max((abs(rho-Rho))./(1+abs(rho)))); |
| 352 | double Dist2 = 0.0; |
| 353 | for (int u=0; u<m; u++) |
| 354 | { |
| 355 | for (int v=0; v<m; v++) |
| 356 | { |
| 357 | for (int w=0; w<k; w++) |
| 358 | { |
| 359 | double tmpDist = fabs(rho[ai(u,v,w,m,m,k)]-Rho[ai(u,v,w,m,m,k)]) |
| 360 | / (1.0+fabs(rho[ai(u,v,w,m,m,k)])); |
| 361 | if (tmpDist > Dist2) |
| 362 | Dist2 = tmpDist; |
| 363 | } |
| 364 | } |
| 365 | } |
| 366 | //Dist3=max(max((abs(pi-Pi))./(1+abs(Pi)))); |
| 367 | double Dist3 = 0.0; |
| 368 | for (int u=0; u<n; u++) |
| 369 | { |
| 370 | for (int v=0; v<k; v++) |
| 371 | { |
| 372 | double tmpDist = fabs(pi[v]-Pi[v]) / (1.0+fabs(pi[v])); |
| 373 | if (tmpDist > Dist3) |
| 374 | Dist3 = tmpDist; |
| 375 | } |
| 376 | } |
| 377 | //dist2=max([max(Dist1),max(Dist2),max(Dist3)]); |
| 378 | dist2 = Dist1; |
| 379 | if (Dist2 > dist2) |
| 380 | dist2 = Dist2; |
| 381 | if (Dist3 > dist2) |
| 382 | dist2 = Dist3; |
| 383 | |
| 384 | ite++; |
| 385 | } |
| 386 | |
| 387 | //free memory |
| 388 | free(b); |
| 389 | free(gam); |
| 390 | free(Gam); |
| 391 | free(Phi); |
| 392 | free(Rho); |
| 393 | free(Pi); |
| 394 | free(ps); |
| 395 | free(nY2); |
| 396 | free(ps1); |
| 397 | free(nY21); |
| 398 | free(Gram2); |
| 399 | free(ps2); |
| 400 | gsl_matrix_free(matrix); |
| 401 | gsl_permutation_free(permutation); |
| 402 | free(XiPhiR); |
| 403 | free(YiRhoR); |
| 404 | free(gam2); |
| 405 | free(pi2); |
| 406 | free(X2); |
| 407 | free(Y2); |
| 408 | free(dotProducts); |
| 409 | } |