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