// ------------------------------- mexFunction ---------------------------------------//
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[]) {
/* Check for proper number of arguments */
if(nrhs!=5) {
Usage();
mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nrhs",
"Five inputs required.");
}
if(nlhs!=4) {
Usage();
mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nlhs",
"Four output required.");
}
// Input argument and dimensions
const mwSize DataR = mxGetM(prhs[0]),
DataC = mxGetN(prhs[0]),
MeanIR = mxGetM(prhs[1]),
MeanIC = mxGetN(prhs[1]);
const double *Data = mxGetPr(prhs[0]),
*MeanI = mxGetPr(prhs[1]);
const double MaxIter = mxGetScalar(prhs[2]),
Thresh = mxGetScalar(prhs[3]);
const int Verbose = (int) mxGetScalar(prhs[4]);
// Set the number of means elements, dimension and number of data points
int NM = MeanIC,
ND = DataC,
Dim = DataR;
// Check appropriate sizes and modes
if( Dim != MeanIR ) {
Usage();
mexErrMsgIdAndTxt("KMeans:Means","Means matrix dimension");
}
// Data elements
double *Prior = NULL,
*Mean = NULL,
*Cov = NULL,
*Assign;
// ** KM parameters
double *SqrTemp = new double[Dim];
double *DistTemp = new double[NM];
double alpha = 1,
beta = 1,
Scale = 0;
double Term, TermP = 0;
int AssignTemp;
int *AssignNum; AssignNum = new int[NM];
mwSize CSize[] = {Dim,Dim,NM};
int CNumSize = 3;
int i,k,j;
int inc = 1;
int index = 0;
Assign = new double[ND];
Prior = new double[NM];
Mean = new double[Dim*NM];
Cov = new double[Dim*Dim*NM];
memcpy(Mean, MeanI, Dim*NM*sizeof(double));
for(int E = 0; E < MaxIter; E++){
// ** Assign features to each center
#pragma omp parallel for shared(Data, Mean, Dim, inc, Assign) private(i,j, SqrTemp, DistTemp, AssignTemp)
for(i = 0; i < ND; i++){
for(j = 0; j < NM; j++){
// Calculate elements of Data-Mean
vdSub( Dim, &Data[i*Dim], &Mean[j*Dim], SqrTemp );
// Square elements
vdSqr( Dim, SqrTemp, SqrTemp );
// Sum distances
DistTemp[j] = dasum( &Dim, SqrTemp, &inc);
}
// Find minimum distance index
AssignTemp = idamin(&NM, DistTemp, &inc);
Assign[i] = double(AssignTemp-1);
}
// ** Calculate new center values
// Zero out centers
memset(Mean, 0, Dim*NM*sizeof(double));
memset(AssignNum, 0, NM*sizeof(int));
for(i = 0; i < ND; i++){
index = Assign[i];
daxpy(&Dim, &alpha, &Data[i*Dim], &inc, &Mean[index*Dim], &inc);
AssignNum[index] = AssignNum[index] + 1;
}
for(i = 0; i < NM; i++){
// If no point was assigned to an average, assign a different point
if(AssignNum[i] == 0){
memcpy(&Mean[i*Dim], &Data[(rand()%ND)*Dim], Dim*sizeof(double));
Scale = 1;
dscal(&Dim, &Scale, &Mean[i*Dim], &inc);
}
else{
Scale = 1 / double(AssignNum[i]);
dscal(&Dim, &Scale, &Mean[i*Dim], &inc);
}
}
// Termination conditions
k = Dim*NM;
Term = dasum(&k, Mean, &inc);
if(Verbose == 1){
printf("Iteration %d - Shift %3.9f\n",E, fabs(Term-TermP));
mexEvalString("pause(.000001);");
}
if(fabs(Term-TermP) <= Thresh)
break;
TermP = Term;
}
// ** Create and assign output variables
double *Out;
plhs[0] = mxCreateDoubleMatrix(1, ND ,mxREAL );
plhs[1] = mxCreateDoubleMatrix(Dim, NM, mxREAL );
plhs[2] = mxCreateDoubleMatrix(1, NM, mxREAL );
plhs[3] = mxCreateNumericArray( CNumSize, CSize,
mxDOUBLE_CLASS, mxREAL);
// Priors
k = 0;
for(i = 0; i < NM; i++ )
k = k + AssignNum[i];
for(i = 0; i < NM; i++){
Prior[i] = double(AssignNum[i])/k;
}
// Covariances
memset(Cov, 0, Dim*Dim*NM*sizeof(double));
char trans = 'N';
k = 1;
for(i = 0; i < ND; i++){
index = Assign[i];
Scale = 1/(Prior[index]*ND);
// Calculate elements of Data-Mean
vdSub( Dim, &Data[i*Dim], &Mean[index*Dim], SqrTemp );
// covariance
dgemm(&trans, &trans, &Dim, &Dim, &k, &Scale, SqrTemp, &Dim, SqrTemp,
&k, &beta, &Cov[index*Dim*Dim], &Dim);
}
Out = mxGetPr(plhs[0]);
memcpy(Out, Assign, ND*sizeof(double));
Out = mxGetPr(plhs[1]);
memcpy(Out, Mean, Dim*NM*sizeof(double));
Out = mxGetPr(plhs[2]);
memcpy(Out, Prior, NM*sizeof(double));
Out = mxGetPr(plhs[3]);
memcpy(Out, Cov, Dim*Dim*NM*sizeof(double));
//** Delete dynamic variables
delete [] Prior;
delete [] Mean;
delete [] Cov;
delete [] Assign;
delete [] AssignNum;
}
double KNITRO_EXPORT KTR_dasum (const int n,
const double * const x,
const int incx)
{
return( dasum (n, x, incx) );
}
/* Function: mdlOutputs =======================================================
* do the main optimization routine here
* no discrete states are considered
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int_T i, j, Result, qinfeas=0;
real_T *z = ssGetOutputPortRealSignal(S,0);
real_T *w = ssGetOutputPortRealSignal(S,1);
real_T *I = ssGetOutputPortRealSignal(S,2);
real_T *exitflag = ssGetOutputPortRealSignal(S,3);
real_T *pivots = ssGetOutputPortRealSignal(S,4);
real_T *time = ssGetOutputPortRealSignal(S,5);
InputRealPtrsType M = ssGetInputPortRealSignalPtrs(S,0);
InputRealPtrsType q = ssGetInputPortRealSignalPtrs(S,1);
ptrdiff_t pivs, info, m, n, inc=1;
char_T T='N';
double total_time, alpha=1.0, tmp=-1.0, *x, *Mn, *qn, *r, *c, s=0.0, sn=0.0;
PT_Matrix pA; /* Problem data A = [M -1 q] */
PT_Basis pB; /* The basis */
T_Options options; /* options structure defined in lcp_matrix.h */
/* for RTW we do not need these variables */
#ifdef MATLAB_MEX_FILE
const char *fname;
mxArray *fval;
int_T nfields;
clock_t t1,t2;
#endif
UNUSED_ARG(tid); /* not used in single tasking mode */
/* default options */
options.zerotol = 1e-10; /* zero tolerance */
options.lextol = 1e-10; /* lexicographic tolerance - a small treshold to determine if values are equal */
options.maxpiv = INT_MAX; /* maximum number of pivots */
/* if LUMOD routine is chosen, this options refactorizes the basis after n steps using DGETRF
routine from lapack to avoid numerical problems */
options.nstepf = 50;
options.clock = 0; /* 0 or 1 - to print computational time */
options.verbose = 0; /* 0 or 1 - verbose output */
/* which routine in Basis_solve should solve a set of linear equations:
0 - corresponds to LUmod package that performs factorization in the form L*A = U. Depending on
the change in A factors L, U are updated. This is the fastest method.
1 - corresponds to DGESV simple driver
which solves the system AX = B by factorizing A and overwriting B with the solution X
2 - corresponds to DGELS simple driver
which solves overdetermined or underdetermined real linear systems min ||b - Ax||_2
involving an M-by-N matrix A, or its transpose, using a QR or LQ factorization of A. */
options.routine = 0;
options.timelimit = 3600; /* time limit in seconds to interrupt iterations */
options.normalize = 1; /* 0 or 1 - perform scaling of input matrices M, q */
options.normalizethres = 1e6;
/* If the normalize option is on, then the matrix scaling is performed
only if 1 norm of matrix M (maximum absolute column sum) is above this threshold.
This enforce additional control over normalization since it seems to be more
aggressive also for well-conditioned problems. */
#ifdef MATLAB_MEX_FILE
/* overwriting default options by the user */
if (ssGetSFcnParamsCount(S)==3) {
nfields = mxGetNumberOfFields(ssGetSFcnParam(S,2));
for(i=0; i<nfields; i++){
fname = mxGetFieldNameByNumber(ssGetSFcnParam(S,2), i);
fval = mxGetField(ssGetSFcnParam(S,2), 0, fname);
if ( strcmp(fname,"zerotol")==0 )
options.zerotol = mxGetScalar(fval);
if ( strcmp(fname,"lextol")==0 )
options.lextol = mxGetScalar(fval);
if ( strcmp(fname,"maxpiv")==0 ) {
if (mxGetScalar(fval)>=(double)INT_MAX)
options.maxpiv = INT_MAX;
else
options.maxpiv = (int_T)mxGetScalar(fval);
}
if ( strcmp(fname,"nstepf")==0 )
options.nstepf = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"timelimit")==0 )
options.timelimit = mxGetScalar(fval);
if ( strcmp(fname,"clock")==0 )
options.clock = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"verbose")==0 )
options.verbose = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"routine")==0 )
options.routine = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"normalize")==0 )
options.normalize = (int_T)mxGetScalar(fval);
if ( strcmp(fname, "normalizethres")==0 )
options.normalizethres = mxGetScalar(fval);
}
}
#endif
/* Normalize M, q to avoid numerical problems if possible
Mn = diag(r)*M*diag(c) , qn = diag(r)*q */
/* initialize Mn, qn */
Mn = (double *)ssGetPWork(S)[0];
qn = (double *)ssGetPWork(S)[1];
/* initialize vectors r, c */
r = (double *)ssGetPWork(S)[2];
c = (double *)ssGetPWork(S)[3];
/* initialize auxiliary vector x */
x = (double *)ssGetPWork(S)[4];
/* initialize to ones */
for (i=0; i<NSTATES(S); i++) {
r[i] = 1.0;
c[i] = 1.0;
}
m = NSTATES(S);
n = m*m;
/* write data to Mn = M */
memcpy(Mn, *M, n*sizeof(double));
/* write data to qn = q */
memcpy(qn, *q, m*sizeof(double));
/* check out the 1-norm of matrix M (maximum column sum) */
for (i=0; i<m; i++) {
sn = dasum(&m, &Mn[i*m], &inc);
if (sn>s) {
s = sn;
}
}
/* scale matrix M, q and write scaling factors to r (rows) and c (columns) */
if (options.normalize && s>options.normalizethres) {
NormalizeMatrix (m, m, Mn, qn, r, c, options);
}
/* Setup the problem */
pA = ssGetPWork(S)[5];
/* A(:,1:m) = M */
memcpy(pMAT(pA), Mn, n*sizeof(double));
/* A(:,1:m) = -A(:,1:m) */
dscal(&n, &tmp, pMAT(pA), &inc);
/* A(:,m+1) = -1 */
for(i=0;i<m;i++)
C_SEL(pA,i,m) = -1.0;
/* A(:,m+2) = q */
memcpy(&(C_SEL(pA,0,m+1)),qn,m*sizeof(double));
/* initialize basis */
pB = ssGetPWork(S)[6];
/* check if the problem is not feasible at the beginning */
for (i=0; i<m; i++) {
if (qn[i]<-options.zerotol) {
qinfeas = 1;
break;
}
}
/* Solve the LCP */
if (qinfeas) {
#ifdef MATLAB_MEX_FILE
t1 = clock();
#endif
/* main LCP rouinte */
Result = lcp(pB, pA, &pivs, options);
#ifdef MATLAB_MEX_FILE
t2 = clock();
total_time = ((double)(t2-t1))/CLOCKS_PER_SEC;
#else
total_time = -1;
#endif
} else {
pivs = 0;
total_time = 0;
Result = LCP_FEASIBLE;
}
#ifdef MATLAB_MEX_FILE
if (options.clock) {
printf("Time needed to perform pivoting:\n time= %i (%lf seconds)\n",
t2-t1,total_time);
printf("Pivots: %ld\n", pivs);
printf("CLOCKS_PER_SEC = %i\n",CLOCKS_PER_SEC);
}
#endif
/* initialize values to 0 */
for(i=0;i<NSTATES(S);i++)
{
w[i] = 0.0;
z[i] = 0.0;
I[i] = 0.0;
}
/* for a feasible basis, compute the solution */
if ( Result == LCP_FEASIBLE || Result == LCP_PRETERMINATED )
{
#ifdef MATLAB_MEX_FILE
t1 = clock();
#endif
info = Basis_Solve(pB, &(C_SEL(pA,0,m+1)), x, options);
for (j=0,i=0;i<Index_Length(pB->pW);i++,j++)
{
w[Index_Get(pB->pW,i)] = x[j];
/* add 1 due to matlab 1-indexing */
I[j] = Index_Get(pB->pW,i)+1;
}
for(i=0;i<Index_Length(pB->pZ);i++,j++)
{
/* take only positive values */
if (x[j] > options.zerotol ) {
z[Index_Get(pB->pZ,i)] = x[j];
}
/* add 1 due to matlab 1-indexing */
I[j] = Index_Get(pB->pZ, i)+m+1;
}
#ifdef MATLAB_MEX_FILE
t2 = clock();
total_time+=(double)(t2-t1)/(double)CLOCKS_PER_SEC;
if (options.clock) {
printf("Time in total needed to solve LCP: %lf seconds\n",
total_time + (double)(t2-t1)/(double)CLOCKS_PER_SEC);
}
#endif
if (options.normalize) {
/* do the backward normalization */
/* z = diag(c)*zn */
for (i=0; i<m; i++) {
z[i] = c[i]*z[i];
}
/* since the normalization does not compute w properly, we recalculate it from
* recovered z */
/* write data to Mn = M */
memcpy(Mn, *M, n*sizeof(double));
/* write data to qn = q */
memcpy(qn, *q, m*sizeof(double));
/* copy w <- q; */
dcopy(&m, qn, &inc, w, &inc);
/* compute w = M*z + q */
dgemv(&T, &m, &m, &alpha, Mn, &m, z, &inc, &alpha, w, &inc);
/* if w is less than eps, consider it as zero */
for (i=0; i<m; i++) {
if (w[i]<options.zerotol) {
w[i] = 0.0;
}
}
}
}
/* outputs */
*exitflag = (real_T )Result;
*pivots =(real_T)pivs;
*time = (real_T)total_time;
/* reset dimensions and values in basis for a recursive call */
Reinitialize_Basis(m, pB);
}
/* normalize input matrix M */
void NormalizeMatrix (ptrdiff_t m, ptrdiff_t n, double *A, double *b, double *r, double *c, T_Options options)
{
/* scales matrix A by finding D1= diag(r) and D2=diag(c) in An = D1*A*D2
such that infinity norm of each row and column approaches 1
find D1, D2
s.t.
An = D1*A*D2
Scaling matrix is used in solving A*x=b for badly scaled matrix A as
follows:
A*x = b
D1*A*x = D1*b / multiply from left by D1
D1*A*D2*inv(D2)*x = D1*b / insert D2*inv(D2)
D1*A*D2*y = D1*b / substitute y = inv(D2)*x
An*y = bn / substitue An = D1*A*D2, v = D1*b
First solve An*y = bn, then obtain x = D2*y
Details of the method are in file drRAL2001034.ps.gz at
http://www.numerical.rl.ac.uk/reports/reports.html.
arguments:
n - (input), ptrdiff_t (INTEGER)
Number of rows for matrix A and dimension of vector r
m - (input), ptrdiff_t (INTEGER)
Number of cols for matrix A and dimension of vector c
A - (input/output) (pointer to DOUBLE)
Input matrix is changed at the output with the normalized matrix.
It is assumed that matrix A does not contain row or column with all elements
zeros. If this not the case, no scaling is performed.
b - (inptu/output) (pointer to DOUBLE)
Input vector is normalized at the output
r - (output) (pointer to DOUBLE)
Vector with scaling factors for each row, D1 = diag(r)
c - (output) (pointer to DOUBLE)
Vector with scaling factors for each column, D2 = diag(c)
*/
ptrdiff_t i, j, kmax, inc=1,k=0;
int flag=1;
double row_norm = 1.0, col_norm = 1.0, nrm;
double *rn, *rs, *cn, *cs;
/* prepare vectors */
rn = (double *)mxCalloc(n,sizeof(double));
rs = (double *)mxCalloc(n,sizeof(double));
cn = (double *)mxCalloc(m,sizeof(double));
cs = (double *)mxCalloc(m,sizeof(double));
/* check if A contains row or cols with all zeros */
/* initialize output vectors */
for (i=0; i<m; i++) {
nrm = dasum(&n, &A[i], &m);
if (nrm<options.zerotol) {
flag = 0;
}
r[i] = 1.0;
}
for (i=0; i<n; i++) {
nrm = dasum(&m, &A[i*n], &inc);
if (nrm<options.zerotol) {
flag = 0;
}
c[i] = 1.0;
}
if (flag)
{
while ((row_norm>options.zerotol) && (col_norm>options.zerotol))
{
/* loop thru rows */
for (i=0; i<m; i++) {
/* compute infinity norm of each row */
/* find index of a maximum value in this row (1-indexed) */
kmax = idamax(&n, &A[i], &m);
/* evaluate the norm */
nrm = fabs(A[i+(kmax-1)*m]);
rs[i] = 1.0/sqrt(nrm);
/* printf("rows: kmax=%ld, nrm=%f\n",kmax,nrm); */
/* to compute row_norm*/
rn[i] = 1 - nrm;
}
/* loop thru cols */
for (i=0; i<n; i++) {
/* compute infinity norm of each column */
/* find maximum value in this column */
kmax = idamax(&m, &A[i*n], &inc);
/* evaluate the norm */
nrm = fabs(A[i*m+kmax-1]);
cs[i] = 1.0/sqrt(nrm);
/* printf("cols: kmax=%ld, nrm=%f\n",kmax,nrm); */
/* set the col_norm */
cn[i] = 1 - nrm;
}
/* scale matrix A and vector b */
for (i=0; i<m; i++) {
for (j=0; j<n; j++) {
A[i+j*m] = rs[i]*A[i+j*m]*cs[j];
}
b[i] = rs[i]*b[i];
/* update output vector r */
r[i] = r[i]*rs[i];
}
/* update output vectors c*/
for (i=0; i<n; i++) {
c[i] = c[i]*cs[i];
}
/* compute the norm and column norms */
kmax = idamax(&m, rn, &inc);
row_norm = fabs(rn[kmax-1]);
kmax = idamax(&n, cn, &inc);
col_norm = fabs(cn[kmax-1]);
if (++k>=1000 ) {
/* limit the number of iterations */
break;
}
}
}
/* freeing vectors */
mxFree(rn);
mxFree(rs);
mxFree(cn);
mxFree(cs);
}
int lmT(double * X, int n, int p,
double * y,
double nu,
int maxIter, double tol, char method,
double * logLikelihood,
double * coef,
double * tau)
{
// Initialize workspace variables
int iter, i;
double logLikelihood_tm1, delta;
/*
* Allocate workspace arrays
*/
// Allocate workspace matrices
double * sqwX, * XTX;
sqwX = malloc(n * p * sizeof(double));
if (sqwX==NULL)
{
fprintf(stderr, "Error -- out of memory\n");
exit(1);
}
XTX = malloc(p * p * sizeof(double));
if (XTX==NULL)
{
fprintf(stderr, "Error -- out of memory\n");
exit(1);
}
// Allocate workspace vectors
double * w, * sqw, * sqwy, * resid;
w = calloc(n, sizeof(double));
if (w==NULL)
{
fprintf(stderr, "Error -- out of memory\n");
exit(1);
}
sqw = calloc(n, sizeof(double));
if (sqw==NULL)
{
fprintf(stderr, "Error -- out of memory\n");
exit(1);
}
sqwy = calloc(n, sizeof(double));
if (sqwy==NULL)
{
fprintf(stderr, "Error -- out of memory\n");
exit(1);
}
resid = calloc(n, sizeof(double));
if (resid==NULL)
{
fprintf(stderr, "Error -- out of memory\n");
exit(1);
}
/*
* Initialize quantities before EM iterations
*/
// Initialize weights for observations
for (i=0; i<n; i++)
{
w[i] = 1;
}
/*
* First iteration
*/
// Run regression to obtain initial coefficients
wls(X, n, p, y, w, method, XTX, sqw, sqwX, sqwy, coef);
// Calculate residuals
calcResid(X, n, p, y, coef, resid);
// Calculate tau
(*tau) = 0;
for (i=0; i<n; i++)
{
(*tau) += resid[i] * resid[i] * w[i];
}
(*tau) /= dasum(n, w, 1);
// Calculate initial log-posterior
(*logLikelihood) = dt_log(resid, n, nu, 0, sqrt(*tau));
logLikelihood_tm1 = (*logLikelihood);
/*
* EM loop
*/
for (iter=0; iter<maxIter; iter++)
{
// E step: Update u | beta, tau
for (i=0; i<n; i++)
{
w[i] = (nu + 1) / (nu + resid[i]*resid[i]/(*tau));
}
// M step: Update beta, tau | u
// Run regression to obtain coefficients
wls(X, n, p, y, w, method, XTX, sqw, sqwX, sqwy, coef);
// Calculate residuals
calcResid(X, n, p, y, coef, resid);
// Calculate tau
(*tau) = 0;
for (i=0; i<n; i++)
{
(*tau) += resid[i] * resid[i] * w[i];
}
(*tau) /= dasum(n, w, 1);
// Calculate log-posterior
(*logLikelihood) = dt_log(resid, n, nu, 0, sqrt(*tau));
// Check convergence
delta = ((*logLikelihood) - logLikelihood_tm1) /
fabs((*logLikelihood) + logLikelihood_tm1) * 2;
if (delta < tol)
{
logLikelihood_tm1 = (*logLikelihood);
break;
}
logLikelihood_tm1 = (*logLikelihood);
}
/*
* Free allocated memory
*/
// Free workspace matrices
free(sqwX);
sqwX = NULL;
free(XTX);
XTX = NULL;
// Free workspace vectors
free(w);
w=NULL;
free(sqw);
sqw=NULL;
free(sqwy);
sqw=NULL;
free(resid);
resid=NULL;
return iter;
}
