/* FILENAME : matrixArrayCalcs.cpp * AUTHOR : Debbie Meduna * DATE : 01/01/08 * * LAST MODIFIED : 03/30/10 * MODIFIED BY : Debbie Meduna * ----------------------------------------------------------------------------- * Modification History * ----------------------------------------------------------------------------- ******************************************************************************/ #include "matrixArrayCalcs.h" #ifdef USE_MATLAB Engine* matlabEng; #endif static Real cubWeights[] = {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0 ,0,-0.5,0,0,0,0,0,0,0,0.5,0,0,0,0,0,0 ,0,1.0,0,0,0,-2.5,0,0,0,2.0,0,0,0,-0.5,0,0 ,0,-0.5,0,0,0,1.5,0,0,0,-1.5,0,0,0,0.5,0,0 ,0,0,0,0,-0.5,0,0.5,0,0,0,0,0,0,0,0,0 ,0.25,0,-0.25,0,0,0,0,0,-0.25,0,0.25,0,0,0,0,0 ,-0.5,0,0.5,0,1.25,0,-1.25,0,-1,0,1,0,0.25,0,-0.25,0 ,0.25,0,-0.25,0,-0.75,0,0.75,0,0.75,0,-0.75,0, -0.25,0,0.25,0 ,0,0,0,0,1,-2.5,2,-0.5,0,0,0,0,0,0,0,0 ,-0.5,1.25,-1,0.25,0,0,0,0,0.5,-1.25,1,-0.25,0,0,0,0 ,1,-2.5,2,-0.5,-2.5,6.25,-5,1.25,2,-5,4,-1,-0.5, 1.25,-1,0.25 ,-0.5,1.25,-1,0.25,1.5,-3.75,3,-0.75,-1.5,3.75,-3 ,0.75,0.5,-1.25,1,-0.25 ,0,0,0,0,-0.5,1.5,-1.5,0.5,0,0,0,0,0,0,0,0 ,0.25,-0.75,0.75,-0.25,0,0,0,0,-0.25,0.75,-0.75, 0.25,0,0,0,0 ,-0.5,1.5,-1.5,0.5,1.25,-3.75,3.75,-1.25,-1,3,-3, 1,0.25,-0.75,0.75,-0.25 ,0.25,-0.75,0.75,-0.25,-0.75,2.25,-2.25,0.75,0.75 ,-2.25,2.25,-0.75,-0.25,0.75,-0.75,0.25}; #ifdef _QNX int round(const double &num) { double rem = num - floor(num); if(rem >= 0.5) return int(ceil(num)); else return int(floor(num)); } #endif int minVal(const int* values, const int numValues) { int minVal = 10000; for(int i = 0; i < numValues; i++) { if(values[i] < minVal) minVal = values[i]; } return minVal; } int maxVal(const int* values, const int numValues) { int maxVal = -10000; for(int i = 0; i < numValues; i++) { if(values[i] > maxVal) maxVal = values[i]; } return maxVal; } Matrix conv2(const Matrix &A, const Matrix &H) { int filterSizeX, filterSizeY; Matrix tempMAT = H; Matrix newMAT = A; //filterSize corresponds to the number of rows/cols in the filter H filterSizeX = 2*(int(floor(H.Nrows()/2.0))); filterSizeY = 2*(int(floor(H.Ncols()/2.0))); #if 0 //slower method of convolution, not currently being used int subIndices[4]; //Compute convolution by applying filter H over all points in A for(int row = 1; row <= A.Nrows(); row++) { for(int col = 1; col <= A.Ncols(); col++) { //subIndices describes portion of H which has related values in A. subIndices[0] = 2-min(1,row-filterSize/2); subIndices[1] = filterSize/2+1+min(filterSize/2,A.Nrows()- row); subIndices[2] = 2-min(1,col-filterSize/2); subIndices[3] = filterSize/2+1+min(filterSize/2,A.Ncols() - col); tempMAT = 0.0; tempMAT.SubMatrix (subIndices[0], subIndices[1], subIndices[2], subIndices[3]) = SP(H.SubMatrix(subIndices[0],subIndices[1],subIndices[2], subIndices[3]), A.SubMatrix(max(row-filterSize/2,1),min(row+filterSize/2, A.Nrows()), max(col-filterSize/2,1),min(col+filterSize/2, A.Ncols()))); newMAT(row, col) = tempMAT.Sum(); } } #endif //Alternative method: apply A to all points in H tempMAT.ReSize(A.Nrows()+filterSizeX, A.Ncols()+filterSizeY); tempMAT = 0.0; tempMAT.SubMatrix(filterSizeX/2+1,A.Nrows()+filterSizeX/2,filterSizeY/2+1, A.Ncols()+filterSizeY/2) = A; newMAT = 0.0; for(int row = 1; row <= H.Nrows(); row++) { for(int col = 1; col <= H.Ncols(); col++) { newMAT += H(row,col)* tempMAT.SubMatrix(row, A.Nrows()+row-1,col,A.Ncols()+col-1); } } return newMAT; } void interp2(double* xpts, double* ypts, const Matrix Z, double* xi, double* yi, double* zi, int numPts) { int yIndices[4]; int xIndices[4]; ColumnVector Weights(4); for(int i = 0; i < numPts; i++) bilinearInterp(xpts, ypts, Z, xi[i], yi[i], zi[i], xIndices, yIndices, Weights); return; } void interp2mat(double* xpts, double* ypts, const Matrix Z, double* xi, double* yi, Matrix &zi) { double znum; for(int i = 0; i < zi.Nrows(); i++) { for(int j = 0; j < zi.Ncols(); j++) { interp2(xpts,ypts,Z,&xi[i],&yi[j],&znum,1); zi(i+1,j+1) = znum; } } return; } void nearestInterp(double* xpts, double* ypts, const Matrix &zvals, double xi, double yi, double &zi, int* xIndices, int* yIndices, ColumnVector &Weights) { Weights.ReSize(1); Weights(1) = 1; xIndices[0] = closestPtUniformArray(xi, xpts[0], xpts[zvals.Nrows()-1], zvals.Nrows()); yIndices[0] = closestPtUniformArray(yi, ypts[0], ypts[zvals.Ncols()-1], zvals.Ncols()); zi = zvals(xIndices[0]+1, yIndices[0]+1); //if the nearest value in the array is NaN, search radially //outward from the nearest point to find the closest non-NaN //grid value if(isnan(zi)) { xIndices[0]++; yIndices[0]++; findNearestValid(zvals, xIndices[0], yIndices[0]); xIndices[0]--; yIndices[0]--; zi = zvals(xIndices[0]+1, yIndices[0]+1); } } bool findNearestValid(const Matrix &zvals, int &xIndex, int &yIndex) { int i, j, r, rPower, rBase, maxRadius; i = 1; j = 1; r = 1; //radius of search away from nominal point rPower = 0; //radius power rBase = 2; //radius base maxRadius = 32; if(!isnan(zvals(xIndex, yIndex))) return true; r = (int)(pow(rBase,rPower)); // while ((r < zvals.Nrows()) || (r < zvals.Ncols())) while(r < maxRadius) { //search side columns for(i = -r; i <= r; i++) { //check that we are indexing within the grid if((xIndex+i < 1) || (xIndex+i > zvals.Nrows())) continue; for(j = -r; j <= r; j = j+2*r) { //check that we are indexing within the grid if((yIndex+j < 1) || (yIndex+j > zvals.Ncols())) continue; if(!isnan(zvals(xIndex+i,yIndex+j))) { xIndex += i; yIndex += j; return true; } } } //search top and bottom rows for(j = -r+1; j <= r-1; j++) { //check that we are indexing within the grid if((yIndex+j < 1) || (yIndex+j > zvals.Ncols())) continue; for(i = -r; i <= r; i = i+2*r) { //check that we are indexing within the grid if((xIndex+i < 1) || (xIndex+i > zvals.Nrows())) continue; if(!isnan(zvals(xIndex+i,yIndex+j))) { xIndex += i; yIndex += j; return true; } } } rPower++; r = (int)(pow(rBase,rPower)); } return false; } void bilinearInterp(double* xpts, double* ypts, const Matrix &zvals, double xi, double yi, double &zi, int* xIndices, int* yIndices, ColumnVector &Weights) { double dx, dy, t, u; double lowerX, lowerY, upperX, upperY; int i,x1, y1; Weights.ReSize(4); //find the lowest closest point in the reference data to desired point x1 = lowerBound(xi, xpts, zvals.Nrows()); y1 = lowerBound(yi, ypts, zvals.Ncols()); //ensure that the bounding box falls within the extracted map size //if the bounding box is outside the map, use nearestInterp if((x1 >= zvals.Nrows()-1) || (x1 < 0) || (y1 >= zvals.Ncols()-1) || (y1 < 0)) { nearestInterp(xpts, ypts, zvals, xi, yi, zi, xIndices, yIndices, Weights); for(i = 1; i < 4; i++) { xIndices[i] = 0; yIndices[i] = 0; } return; } //define surrounding four corners in the grid lowerX = xpts[x1]; upperX = xpts[x1+1]; lowerY = ypts[y1]; upperY = ypts[y1+1]; dx = upperX-lowerX; dy = upperY-lowerY; /*Define interpolation indices into zvals *The four interpolation points are labeled as follows: * 0 2 * 1 3 */ xIndices[0] = x1; yIndices[0] = y1; xIndices[1] = x1+1; yIndices[1] = y1; xIndices[2] = x1+1; yIndices[2] = y1+1; xIndices[3] = x1; yIndices[3] = y1+1; //Define bilinear interpolation weights t = (xi - lowerX)/dx; u = (yi - lowerY)/dy; Weights(1) = (1 - t)*(1 - u); Weights(2) = t*(1 - u); Weights(3) = t*u; Weights(4) = (1 - t)*u; //Compute interpolated zi by weighted sum of the four z points zi = 0.0; for(i = 0; i < 4; i++) zi += Weights(i+1)*zvals(xIndices[i]+1, yIndices[i]+1); //Check that the interpolated value is not NaN, otherwise, try nearestInterp if(isnan(zi)) { nearestInterp(xpts, ypts, zvals, xi, yi, zi, xIndices, yIndices, Weights); for(i = 1; i < 4; i++) { xIndices[i] = 0; yIndices[i] = 0; } } return; } void bicubicInterp(double* xpts, double* ypts, const Matrix &zvals, double xi, double yi, double &zi, int* xIndices, int* yIndices, ColumnVector &Weights) { int i, j, x1, y1; Matrix W(16,16); ColumnVector b(16); double dx, dy, t, u; Weights.ReSize(16); //find the lowest closest point in the reference data to desired point x1 = lowerBound(xi, xpts, zvals.Nrows()); y1 = lowerBound(yi, ypts, zvals.Ncols()); /*Define interpolation indices into zvals *The sixteen interpolation points are labeled as follows: *12 13 14 15 * 8 9 10 11 * 4 5 6 7 * 0 1 2 3*/ xIndices[5] = x1; yIndices[5] = y1; //ensure that the bounding box falls within the extracted map size //if bounding box is outside the map, attempt bilinear interpolation if((x1+2 > zvals.Nrows()-1) || (x1-1 < 0) || (y1+2 > zvals.Ncols()-1) || (y1-1 < 0)) { bilinearInterp(xpts, ypts, zvals, xi, yi, zi, xIndices, yIndices, Weights); for(i = 4; i < 16; i++) { xIndices[i] = 0; yIndices[i] = 0; } return; } //define indices of interpolation z points for(i = 0; i < 16; i=i+4) xIndices[i] = xIndices[5]-1; for(i = 1; i < 16; i=i+4) xIndices[i] = xIndices[5]; for(i = 2; i < 16; i=i+4) xIndices[i] = xIndices[5]+1; for(i = 3; i < 16; i=i+4) xIndices[i] = xIndices[5]+2; for(i = 0; i < 4; i++) yIndices[i] = yIndices[5]-1; for(i = 4; i < 8; i++) yIndices[i] = yIndices[5]; for(i = 8; i < 12; i++) yIndices[i] = yIndices[5]+1; for(i = 12; i < 16; i++) yIndices[i] = yIndices[5]+2; //define t and u parameters to be used for calculating interpolation //weights dx = xpts[xIndices[6]]-xpts[xIndices[5]]; dy = ypts[yIndices[9]]-ypts[yIndices[5]]; t = (xi - xpts[x1])/dx; u = (yi - ypts[y1])/dy; //Define interpolation weights for(i = 1; i <= 4; i++) { for(j = 1; j <= 4; j++) b((i-1)*4+j) = pow(t,i-1)*pow(u,j-1); } W << cubWeights; Weights = W.t()*b; //Determine z value based on interpolation weights: zi = 0.0; for(i = 0; i < 16; i++) zi += Weights(i+1)*zvals(xIndices[i]+1, yIndices[i]+1); //Check that the interpolated value is not NaN, otherwise,try bilinearInterp if(isnan(zi)) { bilinearInterp(xpts, ypts, zvals, xi, yi, zi, xIndices, yIndices, Weights); for(i = 4; i < 16; i++) { xIndices[i] = 0; yIndices[i] = 0; } } return; } void splineInterp(double* xpts, double* ypts, const Matrix &zvals, double xi, double yi, double &zi, int* xIndices, int* yIndices, ColumnVector &Weights) { #ifdef USE_MATLAB mxArray *Z = NULL; mxArray *X = NULL; mxArray *Y = NULL; mxArray *Xi; mxArray *Yi; mxArray *result = NULL; //copy contents of Surf into Matlab variable A Z = mxCreateDoubleMatrix(zvals.Ncols(), zvals.Nrows(), mxREAL); memcpy((void *)mxGetPr(Z), (void *) zvals.Store(), zvals.Storage()*sizeof(double)); X = mxCreateDoubleMatrix(1, zvals.Nrows(), mxREAL); memcpy((void *)mxGetPr(X), (void *) xpts, sizeof(xpts)); Y = mxCreateDoubleMatrix(1, zvals.Ncols(), mxREAL); memcpy((void *)mxGetPr(Y), (void *) ypts, sizeof(ypts)); Xi = mxCreateDoubleMatrix(1, 1, mxREAL); memcpy(mxGetPr(Xi), (void *)&xi, sizeof(double)); Yi = mxCreateDoubleMatrix(1, 1, mxREAL); memcpy(mxGetPr(Yi), (void *)&yi, sizeof(double)); //put data into Matlab workspace engPutVariable(matlabEng, "Z", Z); engPutVariable(matlabEng, "X", X); engPutVariable(matlabEng, "Y", Y); engPutVariable(matlabEng, "Yi", Yi); engPutVariable(matlabEng, "Xi", Xi); //compute interpolation in Maltab engEvalString(matlabEng, "[Xm,Ym] = meshgrid(X,Y);"); engEvalString(matlabEng, "Zi = interp2(Xm,Ym,Z,Xi,Yi,'spline');"); engEvalString(matlabEng, "figure(10);"); engEvalString(matlabEng, "surf(Zi, 'Linestyle', 'none');"); //extract interpolated value from Matlab result = engGetVariable(matlabEng,"Zi"); memcpy((void *)&zi,(void*) mxGetPr(result), sizeof(double)); //remove memory in Matlab mxDestroyArray(Z); mxDestroyArray(X); mxDestroyArray(Y); mxDestroyArray(Xi); mxDestroyArray(Yi); mxDestroyArray(result); #else nearestInterp(xpts, ypts, zvals, xi, yi, zi, xIndices, yIndices, Weights); #endif } void nearestInterp_mat(double* xpts, double* ypts, const Matrix &zvals, double* xi, double* yi, Matrix &zi, Matrix &var) { int* xIndices = NULL; int* yIndices = NULL; xIndices = new int[1]; yIndices = new int[1]; ColumnVector W; //perform nearest-neighbor interpolation for each point in (xi,yi) for(int i = 1; i <= zi.Nrows(); i++) { for(int j = 1; j<= zi.Ncols(); j++) nearestInterp(xpts, ypts, zvals, xi[i-1], yi[j-1], zi(i,j), xIndices, yIndices, W); } delete [] xIndices; delete [] yIndices; return; } void bilinearInterp_mat(double* xpts, double* ypts, const Matrix &zvals, double* xi, double* yi, Matrix &zi, Matrix &var) { int* xIndices = NULL; int* yIndices = NULL; xIndices = new int[4]; yIndices = new int[4]; ColumnVector W; //perform nearest-neighbor interpolation for each point in (xi,yi) for(int i = 1; i <= zi.Nrows(); i++) { for(int j = 1; j<= zi.Ncols(); j++) bilinearInterp(xpts, ypts, zvals, xi[i-1], yi[j-1], zi(i,j), xIndices, yIndices, W); } delete [] xIndices; delete [] yIndices; return; } void bicubicInterp_mat(double* xpts, double* ypts, const Matrix &zvals, double* xi, double* yi, Matrix &zi, Matrix &var) { int* xIndices = NULL; int* yIndices = NULL; xIndices = new int[16]; yIndices = new int[16]; ColumnVector W; //perform nearest-neighbor interpolation for each point in (xi,yi) for(int i = 1; i <= zi.Nrows(); i++) { for(int j = 1; j<= zi.Ncols(); j++) bicubicInterp(xpts, ypts, zvals, xi[i-1], yi[j-1], zi(i,j), xIndices, yIndices, W); } delete [] xIndices; delete [] yIndices; return; } void splineInterp_mat(double* xpts, double* ypts, const Matrix &zvals, double* xi, double* yi, Matrix &zi, Matrix &var) { #ifdef USE_MATLAB mxArray *Z = NULL; mxArray *X = NULL; mxArray *Y = NULL; mxArray *Xi; mxArray *Yi; mxArray *result = NULL; //copy contents of Surf into Matlab variable A Z = mxCreateDoubleMatrix(zvals.Ncols(), zvals.Nrows(), mxREAL); memcpy((void *)mxGetPr(Z), (void *) zvals.Store(), zvals.Storage()*sizeof(double)); X = mxCreateDoubleMatrix(1, zvals.Nrows(), mxREAL); memcpy((void *)mxGetPr(X), (void *) xpts, zvals.Nrows()*sizeof(double)); Y = mxCreateDoubleMatrix(1, zvals.Ncols(), mxREAL); memcpy((void *)mxGetPr(Y), (void *) ypts, zvals.Ncols()*sizeof(double)); Xi = mxCreateDoubleMatrix(1, zi.Nrows(), mxREAL); Yi = mxCreateDoubleMatrix(zi.Ncols(), 1, mxREAL); //put data into Matlab workspace engPutVariable(matlabEng, "Z", Z); engPutVariable(matlabEng, "X", X); engPutVariable(matlabEng, "Y", Y); engEvalString(matlabEng, "[Xm,Ym] = meshgrid(X,Y);"); memcpy((void *)mxGetPr(Xi), (void *) xi, zi.Nrows()*sizeof(double)); memcpy((void *)mxGetPr(Yi), (void *) yi, zi.Ncols()*sizeof(double)); engPutVariable(matlabEng, "Yi", Yi); engPutVariable(matlabEng, "Xi", Xi); engEvalString(matlabEng, "[Xd,Yd] = meshgrid(Xi,Yi);"); engEvalString(matlabEng, "Zi = interp2(Xm,Ym,Z,Xi,Yi,'spline');"); result = engGetVariable(matlabEng,"Zi"); memcpy((void *)zi.Store(), (void *) mxGetPr(result), zi.Storage()*sizeof(double)); //remove memory in Matlab mxDestroyArray(Z); mxDestroyArray(X); mxDestroyArray(Y); mxDestroyArray(Xi); mxDestroyArray(Yi); mxDestroyArray(result); #endif nearestInterp_mat(xpts, ypts, zvals, xi, yi, zi, var); return; } int closestPt(double key, const double *base, size_t nmemb) { int i = 0, idx = 0, j; double a, dt, dt0, minValue, maxValue; maxValue = *(base + (nmemb - 1)); minValue = *base; //If key is larger than the largest value in the array OR //the key is smaller than the smallest value, return the //largest or smallest array value accordingly if (key > maxValue) idx = nmemb - 1; else if (key < minValue) idx = 0; else { dt0 = maxValue; for (j = 0; j < int(nmemb); j++) { a = *(base + j); dt = fabs(key - a); if (dt <= dt0) { dt0 = dt; idx = i; } else break; ++i; } } return idx; } int lowerBound(double val, const double* vec, int numVals) { int nearestIndex; int lowerIndex; double diff; nearestIndex = closestPtUniformArray(val, vec[0], vec[numVals-1], numVals); diff = val - vec[nearestIndex]; if(diff >= 0) lowerIndex = nearestIndex; else lowerIndex = nearestIndex-1; return lowerIndex; } double randn(double mean, double stddev) { double rand1, rand2, gauss1, w; static int use_last = 0; static double gauss2; //If we already have a random variable waiting to be used, use it if (use_last) { gauss1 = gauss2; use_last = 0; } //Otherwise, generate two new random numbers else { //Use the Polar Form of the Box-Muller transformation: //1.Generate two uniform random variables within the unit circle do { //Generate two uniform random variables between -1 and 1 rand1 = unif(0,1); rand2 = unif(0,1); w = rand1*rand1 + rand2*rand2; } while ( w >= 1.0 ); //2.Convert random variables to gaussian variables, N(0,1). w = sqrt(-2.0*log(w)/w); gauss1 = rand1 * w; gauss2 = rand2 * w; //Set flag that we have an extra variable that can be used use_last = 1; } return (mean + stddev*gauss1); } char* charCat(char* dest, const char* front, const char* back) { sprintf(dest,"%s%s",front, back); return dest; } double computeKLdiv_gaussian_mat(double* xpts, double* ypts, const Matrix &refPDF, double* mu, const Matrix &Cov) { Matrix A(Cov.Nrows(), Cov.Ncols()); Matrix Value(1,1); ColumnVector dx(2); double q; int i, j; double eta; double kl = 0; //compute gaussian normalization factor A = 2.0*PI*Cov; eta = pow(A.Determinant(),-0.5); //compute inverse of covariance for gaussian calculation A = Cov.i(); //sum KL over all entries in refPDF for(i = 1; i <= refPDF.Nrows(); i++) { dx(1) = xpts[i-1] - mu[0]; for(j = 1; j <= refPDF.Ncols(); j++) { //compute current guassian probability dx(2) = ypts[j-1] - mu[1]; Value = dx.t()*A*dx; q = eta*exp(Value.AsScalar()*-0.5); //add current kl entry if(refPDF(i,j)/q > 1e-50 && refPDF(i,j)/q < 1e50) kl += refPDF(i,j)*log(refPDF(i,j)/q); } } return kl; } SymmetricMatrix computeMatrixSqrt(const SymmetricMatrix &A) { SymmetricMatrix Asqrt; DiagonalMatrix D; Matrix V; int i; //compute eigenvalue decomposition Jacobi(A,D,V); //compute sqrt of eigenvalue matrix for(i = 1; i <= A.Nrows(); i++) { //ensure positive definite matrix prior to taking the sqrt if(D(i) < 0) { output("Error: Tried to take the sqrt of a non-positive definite " "matrix. Exiting...\n"); exit(0); } D(i) = sqrt(D(i)); } //reconstruct sqrt matrix Asqrt << V*D*V.t(); return Asqrt; }