[valse.git] / pkg / src / sources / EMGrank.c

#include <stdlib.h>
#include <gsl/gsl_linalg.h>
#include "utils.h"

// Compute pseudo-inverse of a square matrix
static Real* pinv(const Real* matrix, int dim)
{
	gsl_matrix* U = gsl_matrix_alloc(dim,dim);
	gsl_matrix* V = gsl_matrix_alloc(dim,dim);
	gsl_vector* S = gsl_vector_alloc(dim);
	gsl_vector* work = gsl_vector_alloc(dim);
	Real EPS = 1e-10; //threshold for singular value "== 0"
	
	//copy matrix into U
	copyArray(matrix, U->data, dim*dim);

	//U,S,V = SVD of matrix
	gsl_linalg_SV_decomp(U, V, S, work);
	gsl_vector_free(work);

	// Obtain pseudo-inverse by V*S^{-1}*t(U)
	Real* inverse = (Real*)malloc(dim*dim*sizeof(Real));
	for (int i=0; i<dim; i++)
	{
		for (int ii=0; ii<dim; ii++)
		{
			Real dotProduct = 0.0;
			for (int j=0; j<dim; j++)
				dotProduct += V->data[i*dim+j] * (S->data[j] > EPS ? 1.0/S->data[j] : 0.0) * U->data[ii*dim+j];
			inverse[i*dim+ii] = dotProduct;
		}
	}
	
	gsl_matrix_free(U);
	gsl_matrix_free(V);
	gsl_vector_free(S);
	return inverse;
}

// TODO: comment EMGrank purpose
void EMGrank_core(
	// IN parameters
	const Real* Pi, // parametre de proportion
	const Real* Rho, // parametre initial de variance renormalisé
	int mini, // nombre minimal d'itérations dans l'algorithme EM
	int maxi, // nombre maximal d'itérations dans l'algorithme EM
	const Real* X, // régresseurs
	const Real* Y, // réponse
	Real tau, // seuil pour accepter la convergence
	const int* rank, // vecteur des rangs possibles
	// OUT parameters
	Real* phi, // parametre de moyenne renormalisé, calculé par l'EM
	Real* LLF, // log vraisemblance associé à cet échantillon, pour les valeurs estimées des paramètres
	// additional size parameters
	int n, // taille de l'echantillon
	int p, // nombre de covariables
	int m, // taille de Y (multivarié)
	int k) // nombre de composantes
{
	// Allocations, initializations
	Real* Phi = (Real*)calloc(p*m*k,sizeof(Real));
	Real* hatBetaR = (Real*)malloc(p*m*sizeof(Real));
	int signum;
	Real invN = 1.0/n;
	int deltaPhiBufferSize = 20;
	Real* deltaPhi = (Real*)malloc(deltaPhiBufferSize*sizeof(Real));
	int ite = 0;
	Real sumDeltaPhi = 0.0;
	Real* YiRhoR = (Real*)malloc(m*sizeof(Real));
	Real* XiPhiR = (Real*)malloc(m*sizeof(Real));
	Real* Xr = (Real*)malloc(n*p*sizeof(Real));
	Real* Yr = (Real*)malloc(n*m*sizeof(Real));
	Real* tXrXr = (Real*)malloc(p*p*sizeof(Real));
	Real* tXrYr = (Real*)malloc(p*m*sizeof(Real));
	gsl_matrix* matrixM = gsl_matrix_alloc(p, m);
	gsl_matrix* matrixE = gsl_matrix_alloc(m, m);
	gsl_permutation* permutation = gsl_permutation_alloc(m);
	gsl_matrix* V = gsl_matrix_alloc(m,m);
	gsl_vector* S = gsl_vector_alloc(m);
	gsl_vector* work = gsl_vector_alloc(m);

	//Initialize class memberships (all elements in class 0; TODO: randomize ?)
	int* Z = (int*)calloc(n, sizeof(int));

	//Initialize phi to zero, because some M loops might exit before phi affectation
	zeroArray(phi, p*m*k);

	while (ite<mini || (ite<maxi && sumDeltaPhi>tau))
	{
		/////////////
		// Etape M //
		/////////////
		
		//M step: Mise à jour de Beta (et donc phi)
		for (int r=0; r<k; r++)
		{
			//Compute Xr = X(Z==r,:) and Yr = Y(Z==r,:)
			int cardClustR=0;
			for (int i=0; i<n; i++)
			{
				if (Z[i] == r)
				{
					for (int j=0; j<p; j++)
						Xr[mi(cardClustR,j,n,p)] = X[mi(i,j,n,p)];
					for (int j=0; j<m; j++)
						Yr[mi(cardClustR,j,n,m)] = Y[mi(i,j,n,m)];
					cardClustR++;
				}
			}
			if (cardClustR == 0)
				continue;

			//Compute tXrXr = t(Xr) * Xr
			for (int j=0; j<p; j++)
			{
				for (int jj=0; jj<p; jj++)
				{
					Real dotProduct = 0.0;
					for (int u=0; u<cardClustR; u++)
						dotProduct += Xr[mi(u,j,n,p)] * Xr[mi(u,jj,n,p)];
					tXrXr[mi(j,jj,p,p)] = dotProduct;
				}
			}

			//Get pseudo inverse = (t(Xr)*Xr)^{-1}
			Real* invTXrXr = pinv(tXrXr, p);
			
			// Compute tXrYr = t(Xr) * Yr
			for (int j=0; j<p; j++)
			{
				for (int jj=0; jj<m; jj++)
				{
					Real dotProduct = 0.0;
					for (int u=0; u<cardClustR; u++)
						dotProduct += Xr[mi(u,j,n,p)] * Yr[mi(u,jj,n,m)];
					tXrYr[mi(j,jj,p,m)] = dotProduct;
				}
			}

			//Fill matrixM with inverse * tXrYr = (t(Xr)*Xr)^{-1} * t(Xr) * Yr
			for (int j=0; j<p; j++)
			{
				for (int jj=0; jj<m; jj++)
				{
					Real dotProduct = 0.0;
					for (int u=0; u<p; u++)
						dotProduct += invTXrXr[mi(j,u,p,p)] * tXrYr[mi(u,jj,p,m)];
					matrixM->data[j*m+jj] = dotProduct;
				}
			}
			free(invTXrXr);

			//U,S,V = SVD of (t(Xr)Xr)^{-1} * t(Xr) * Yr
			gsl_linalg_SV_decomp(matrixM, V, S, work);

			//Set m-rank(r) singular values to zero, and recompose
			//best rank(r) approximation of the initial product
			for (int j=rank[r]; j<m; j++)
				S->data[j] = 0.0;
			
			//[intermediate step] Compute hatBetaR = U * S * t(V)
			double* U = matrixM->data; //GSL require double precision
			for (int j=0; j<p; j++)
			{
				for (int jj=0; jj<m; jj++)
				{
					Real dotProduct = 0.0;
					for (int u=0; u<m; u++)
						dotProduct += U[j*m+u] * S->data[u] * V->data[jj*m+u];
					hatBetaR[mi(j,jj,p,m)] = dotProduct;
				}
			}

			//Compute phi(:,:,r) = hatBetaR * Rho(:,:,r)
			for (int j=0; j<p; j++)
			{
				for (int jj=0; jj<m; jj++)
				{
					Real dotProduct=0.0;
					for (int u=0; u<m; u++)
						dotProduct += hatBetaR[mi(j,u,p,m)] * Rho[ai(u,jj,r,m,m,k)];
					phi[ai(j,jj,r,p,m,k)] = dotProduct;
				}
		}
		}

		/////////////
		// Etape E //
		/////////////
		
		Real sumLogLLF2 = 0.0;
		for (int i=0; i<n; i++)
		{
			Real sumLLF1 = 0.0;
			Real maxLogGamIR = -INFINITY;
			for (int r=0; r<k; r++)
			{
				//Compute
				//Gam(i,r) = Pi(r) * det(Rho(:,:,r)) * exp( -1/2 * (Y(i,:)*Rho(:,:,r) - X(i,:)...
				//*phi(:,:,r)) * transpose( Y(i,:)*Rho(:,:,r) - X(i,:)*phi(:,:,r) ) );
				//split in several sub-steps
				
				//compute det(Rho(:,:,r)) [TODO: avoid re-computations]
				for (int j=0; j<m; j++)
				{
					for (int jj=0; jj<m; jj++)
						matrixE->data[j*m+jj] = Rho[ai(j,jj,r,m,m,k)];
				}
				gsl_linalg_LU_decomp(matrixE, permutation, &signum);
				Real detRhoR = gsl_linalg_LU_det(matrixE, signum);

				//compute Y(i,:)*Rho(:,:,r)
				for (int j=0; j<m; j++)
				{
					YiRhoR[j] = 0.0;
					for (int u=0; u<m; u++)
						YiRhoR[j] += Y[mi(i,u,n,m)] * Rho[ai(u,j,r,m,m,k)];
				}

				//compute X(i,:)*phi(:,:,r)
				for (int j=0; j<m; j++)
				{
					XiPhiR[j] = 0.0;
					for (int u=0; u<p; u++)
						XiPhiR[j] += X[mi(i,u,n,p)] * phi[ai(u,j,r,p,m,k)];
				}

				//compute dotProduct < Y(:,i)*rho(:,:,r)-X(i,:)*phi(:,:,r) . Y(:,i)*rho(:,:,r)-X(i,:)*phi(:,:,r) >
				Real dotProduct = 0.0;
				for (int u=0; u<m; u++)
					dotProduct += (YiRhoR[u]-XiPhiR[u]) * (YiRhoR[u]-XiPhiR[u]);
				Real logGamIR = log(Pi[r]) + log(detRhoR) - 0.5*dotProduct;

				//Z(i) = index of max (gam(i,:))
				if (logGamIR > maxLogGamIR)
				{
					Z[i] = r;
					maxLogGamIR = logGamIR;
				}
				sumLLF1 += exp(logGamIR) / pow(2*M_PI,m/2.0);
			}

			sumLogLLF2 += log(sumLLF1);
		}

		// Assign output variable LLF
		*LLF = -invN * sumLogLLF2;

		//newDeltaPhi = max(max((abs(phi-Phi))./(1+abs(phi))));
		Real newDeltaPhi = 0.0;
		for (int j=0; j<p; j++)
		{
			for (int jj=0; jj<m; jj++)
			{
				for (int r=0; r<k; r++)
				{
					Real tmpDist = fabs(phi[ai(j,jj,r,p,m,k)]-Phi[ai(j,jj,r,p,m,k)])
						/ (1.0+fabs(phi[ai(j,jj,r,p,m,k)]));
					if (tmpDist > newDeltaPhi)
						newDeltaPhi = tmpDist;
				}
			}
		}

		//update distance parameter to check algorithm convergence (delta(phi, Phi))
		//TODO: deltaPhi should be a linked list for perf.
		if (ite < deltaPhiBufferSize)
			deltaPhi[ite] = newDeltaPhi;
		else
		{
			sumDeltaPhi -= deltaPhi[0];
			for (int u=0; u<deltaPhiBufferSize-1; u++)
				deltaPhi[u] = deltaPhi[u+1];
			deltaPhi[deltaPhiBufferSize-1] = newDeltaPhi;
		}
		sumDeltaPhi += newDeltaPhi;

		// update other local variables
		for (int j=0; j<m; j++)
		{
			for (int jj=0; jj<p; jj++)
			{
				for (int r=0; r<k; r++)
					Phi[ai(jj,j,r,p,m,k)] = phi[ai(jj,j,r,p,m,k)];
			}
		}
		ite++;
	}

	//free memory
	free(hatBetaR);
	free(deltaPhi);
	free(Phi);
	gsl_matrix_free(matrixE);
	gsl_matrix_free(matrixM);
	gsl_permutation_free(permutation);
	gsl_vector_free(work);
	gsl_matrix_free(V);
	gsl_vector_free(S);
	free(XiPhiR);
	free(YiRhoR);
	free(Xr);
	free(Yr);
	free(tXrXr);
	free(tXrYr);
	free(Z);
}
Commit	Line	Data
3453829e BA	1	#include <stdlib.h>
	2	#include <gsl/gsl_linalg.h>
	3	#include "utils.h"
	4
	5	// Compute pseudo-inverse of a square matrix
	6	static Real* pinv(const Real* matrix, int dim)
	7	{
	8	gsl_matrix* U = gsl_matrix_alloc(dim,dim);
	9	gsl_matrix* V = gsl_matrix_alloc(dim,dim);
	10	gsl_vector* S = gsl_vector_alloc(dim);
	11	gsl_vector* work = gsl_vector_alloc(dim);
	12	Real EPS = 1e-10; //threshold for singular value "== 0"
	13
	14	//copy matrix into U
	15	copyArray(matrix, U->data, dim*dim);
	16
	17	//U,S,V = SVD of matrix
	18	gsl_linalg_SV_decomp(U, V, S, work);
	19	gsl_vector_free(work);
	20
	21	// Obtain pseudo-inverse by VS^{-1}t(U)
	22	Real* inverse = (Real)malloc(dimdim*sizeof(Real));
	23	for (int i=0; i<dim; i++)
	24	{
	25	for (int ii=0; ii<dim; ii++)
	26	{
	27	Real dotProduct = 0.0;
	28	for (int j=0; j<dim; j++)
	29	dotProduct += V->data[idim+j] (S->data[j] > EPS ? 1.0/S->data[j] : 0.0) * U->data[ii*dim+j];
	30	inverse[i*dim+ii] = dotProduct;
	31	}
	32	}
	33
	34	gsl_matrix_free(U);
	35	gsl_matrix_free(V);
	36	gsl_vector_free(S);
	37	return inverse;
	38	}
	39
	40	// TODO: comment EMGrank purpose
	41	void EMGrank_core(
	42	// IN parameters
	43	const Real* Pi, // parametre de proportion
	44	const Real* Rho, // parametre initial de variance renormalisé
	45	int mini, // nombre minimal d'itérations dans l'algorithme EM
	46	int maxi, // nombre maximal d'itérations dans l'algorithme EM
	47	const Real* X, // régresseurs
	48	const Real* Y, // réponse
	49	Real tau, // seuil pour accepter la convergence
	50	const int* rank, // vecteur des rangs possibles
	51	// OUT parameters
	52	Real* phi, // parametre de moyenne renormalisé, calculé par l'EM
	53	Real* LLF, // log vraisemblance associé à cet échantillon, pour les valeurs estimées des paramètres
	54	// additional size parameters
	55	int n, // taille de l'echantillon
	56	int p, // nombre de covariables
	57	int m, // taille de Y (multivarié)
	58	int k) // nombre de composantes
	59	{
	60	// Allocations, initializations
	61	Real* Phi = (Real)calloc(pm*k,sizeof(Real));
	62	Real* hatBetaR = (Real)malloc(pm*sizeof(Real));
	63	int signum;
	64	Real invN = 1.0/n;
65	int deltaPhiBufferSize = 20;
66	Real* deltaPhi = (Real)malloc(deltaPhiBufferSizesizeof(Real));
67	int ite = 0;
68	Real sumDeltaPhi = 0.0;
69	Real* YiRhoR = (Real)malloc(msizeof(Real));
70	Real* XiPhiR = (Real)malloc(msizeof(Real));
71	Real* Xr = (Real)malloc(np*sizeof(Real));
72	Real* Yr = (Real)malloc(nm*sizeof(Real));
73	Real* tXrXr = (Real)malloc(pp*sizeof(Real));
74	Real* tXrYr = (Real)malloc(pm*sizeof(Real));
75	gsl_matrix* matrixM = gsl_matrix_alloc(p, m);
76	gsl_matrix* matrixE = gsl_matrix_alloc(m, m);
77	gsl_permutation* permutation = gsl_permutation_alloc(m);
78	gsl_matrix* V = gsl_matrix_alloc(m,m);
79	gsl_vector* S = gsl_vector_alloc(m);
80	gsl_vector* work = gsl_vector_alloc(m);
81
82	//Initialize class memberships (all elements in class 0; TODO: randomize ?)
83	int* Z = (int*)calloc(n, sizeof(int));
84
85	//Initialize phi to zero, because some M loops might exit before phi affectation
86	zeroArray(phi, pmk);
87
88	while (ite<mini \|\| (ite<maxi && sumDeltaPhi>tau))
89	{
90	/////////////
91	// Etape M //
92	/////////////
93
94	//M step: Mise à jour de Beta (et donc phi)
95	for (int r=0; r<k; r++)
96	{
97	//Compute Xr = X(Z==r,:) and Yr = Y(Z==r,:)
98	int cardClustR=0;
99	for (int i=0; i<n; i++)
100	{
101	if (Z[i] == r)
102	{
103	for (int j=0; j<p; j++)
104	Xr[mi(cardClustR,j,n,p)] = X[mi(i,j,n,p)];
105	for (int j=0; j<m; j++)
106	Yr[mi(cardClustR,j,n,m)] = Y[mi(i,j,n,m)];
107	cardClustR++;
108	}
109	}
110	if (cardClustR == 0)
111	continue;
112
113	//Compute tXrXr = t(Xr) * Xr
114	for (int j=0; j<p; j++)
115	{
116	for (int jj=0; jj<p; jj++)
117	{
118	Real dotProduct = 0.0;
119	for (int u=0; u<cardClustR; u++)
120	dotProduct += Xr[mi(u,j,n,p)] * Xr[mi(u,jj,n,p)];
121	tXrXr[mi(j,jj,p,p)] = dotProduct;
122	}
123	}
124
125	//Get pseudo inverse = (t(Xr)*Xr)^{-1}
126	Real* invTXrXr = pinv(tXrXr, p);
127
128	// Compute tXrYr = t(Xr) * Yr
129	for (int j=0; j<p; j++)
130	{
131	for (int jj=0; jj<m; jj++)
132	{
133	Real dotProduct = 0.0;
134	for (int u=0; u<cardClustR; u++)
135	dotProduct += Xr[mi(u,j,n,p)] * Yr[mi(u,jj,n,m)];
136	tXrYr[mi(j,jj,p,m)] = dotProduct;
137	}
138	}
139
140	//Fill matrixM with inverse * tXrYr = (t(Xr)Xr)^{-1} t(Xr) * Yr
141	for (int j=0; j<p; j++)
142	{
143	for (int jj=0; jj<m; jj++)
144	{
145	Real dotProduct = 0.0;
146	for (int u=0; u<p; u++)
147	dotProduct += invTXrXr[mi(j,u,p,p)] * tXrYr[mi(u,jj,p,m)];
148	matrixM->data[j*m+jj] = dotProduct;
149	}
150	}
151	free(invTXrXr);
152
153	//U,S,V = SVD of (t(Xr)Xr)^{-1} * t(Xr) * Yr
154	gsl_linalg_SV_decomp(matrixM, V, S, work);
155
156	//Set m-rank(r) singular values to zero, and recompose
157	//best rank(r) approximation of the initial product
158	for (int j=rank[r]; j<m; j++)
159	S->data[j] = 0.0;
160
161	//[intermediate step] Compute hatBetaR = U * S * t(V)
162	double* U = matrixM->data; //GSL require double precision
163	for (int j=0; j<p; j++)
164	{
165	for (int jj=0; jj<m; jj++)
166	{
167	Real dotProduct = 0.0;
168	for (int u=0; u<m; u++)
169	dotProduct += U[jm+u] S->data[u] * V->data[jj*m+u];
170	hatBetaR[mi(j,jj,p,m)] = dotProduct;
171	}
172	}
173
174	//Compute phi(:,:,r) = hatBetaR * Rho(:,:,r)
175	for (int j=0; j<p; j++)
176	{
177	for (int jj=0; jj<m; jj++)
178	{
179	Real dotProduct=0.0;
180	for (int u=0; u<m; u++)
181	dotProduct += hatBetaR[mi(j,u,p,m)] * Rho[ai(u,jj,r,m,m,k)];
182	phi[ai(j,jj,r,p,m,k)] = dotProduct;
183	}
184	}
185	}
186
187	/////////////
188	// Etape E //
189	/////////////
190
191	Real sumLogLLF2 = 0.0;
192	for (int i=0; i<n; i++)
193	{
194	Real sumLLF1 = 0.0;
195	Real maxLogGamIR = -INFINITY;
196	for (int r=0; r<k; r++)
197	{
198	//Compute
199	//Gam(i,r) = Pi(r) * det(Rho(:,:,r)) * exp( -1/2 * (Y(i,:)*Rho(:,:,r) - X(i,:)...
200	//phi(:,:,r)) transpose( Y(i,:)Rho(:,:,r) - X(i,:)phi(:,:,r) ) );
201	//split in several sub-steps
202
203	//compute det(Rho(:,:,r)) [TODO: avoid re-computations]
204	for (int j=0; j<m; j++)
205	{
206	for (int jj=0; jj<m; jj++)
207	matrixE->data[j*m+jj] = Rho[ai(j,jj,r,m,m,k)];
208	}
209	gsl_linalg_LU_decomp(matrixE, permutation, &signum);
210	Real detRhoR = gsl_linalg_LU_det(matrixE, signum);
211
212	//compute Y(i,:)*Rho(:,:,r)
213	for (int j=0; j<m; j++)
214	{
215	YiRhoR[j] = 0.0;
216	for (int u=0; u<m; u++)
217	YiRhoR[j] += Y[mi(i,u,n,m)] * Rho[ai(u,j,r,m,m,k)];
218	}
219
220	//compute X(i,:)*phi(:,:,r)
221	for (int j=0; j<m; j++)
222	{
223	XiPhiR[j] = 0.0;
224	for (int u=0; u<p; u++)
225	XiPhiR[j] += X[mi(i,u,n,p)] * phi[ai(u,j,r,p,m,k)];
226	}
227
228	//compute dotProduct < Y(:,i)rho(:,:,r)-X(i,:)phi(:,:,r) . Y(:,i)rho(:,:,r)-X(i,:)phi(:,:,r) >
229	Real dotProduct = 0.0;
230	for (int u=0; u<m; u++)
231	dotProduct += (YiRhoR[u]-XiPhiR[u]) * (YiRhoR[u]-XiPhiR[u]);
232	Real logGamIR = log(Pi[r]) + log(detRhoR) - 0.5*dotProduct;
233
234	//Z(i) = index of max (gam(i,:))
235	if (logGamIR > maxLogGamIR)
236	{
237	Z[i] = r;
238	maxLogGamIR = logGamIR;
239	}
240	sumLLF1 += exp(logGamIR) / pow(2*M_PI,m/2.0);
241	}
242
243	sumLogLLF2 += log(sumLLF1);
244	}
245
246	// Assign output variable LLF
247	LLF = -invN sumLogLLF2;
248
249	//newDeltaPhi = max(max((abs(phi-Phi))./(1+abs(phi))));
250	Real newDeltaPhi = 0.0;
251	for (int j=0; j<p; j++)
252	{
253	for (int jj=0; jj<m; jj++)
254	{
255	for (int r=0; r<k; r++)
256	{
257	Real tmpDist = fabs(phi[ai(j,jj,r,p,m,k)]-Phi[ai(j,jj,r,p,m,k)])
258	/ (1.0+fabs(phi[ai(j,jj,r,p,m,k)]));
259	if (tmpDist > newDeltaPhi)
260	newDeltaPhi = tmpDist;
261	}
262	}
263	}
264
265	//update distance parameter to check algorithm convergence (delta(phi, Phi))
266	//TODO: deltaPhi should be a linked list for perf.
267	if (ite < deltaPhiBufferSize)
268	deltaPhi[ite] = newDeltaPhi;
269	else
270	{
271	sumDeltaPhi -= deltaPhi[0];
272	for (int u=0; u<deltaPhiBufferSize-1; u++)
273	deltaPhi[u] = deltaPhi[u+1];
274	deltaPhi[deltaPhiBufferSize-1] = newDeltaPhi;
275	}
276	sumDeltaPhi += newDeltaPhi;
277
278	// update other local variables
279	for (int j=0; j<m; j++)
280	{
281	for (int jj=0; jj<p; jj++)
282	{
283	for (int r=0; r<k; r++)
284	Phi[ai(jj,j,r,p,m,k)] = phi[ai(jj,j,r,p,m,k)];
285	}
286	}
287	ite++;
288	}
289
290	//free memory
291	free(hatBetaR);
292	free(deltaPhi);
293	free(Phi);
294	gsl_matrix_free(matrixE);
295	gsl_matrix_free(matrixM);
296	gsl_permutation_free(permutation);
297	gsl_vector_free(work);
298	gsl_matrix_free(V);
299	gsl_vector_free(S);
300	free(XiPhiR);
301	free(YiRhoR);
302	free(Xr);
303	free(Yr);
304	free(tXrXr);
305	free(tXrYr);
306	free(Z);
307	}