__inline void interpgrad2d(double *Ireturn, double *I, int *Isize, double *point) {
/* Linear interpolation variables */
int xBas0, xBas1, yBas0, yBas1;
double perc[4]={0, 0, 0, 0};
double xCom, yCom, xComi, yComi;
double fTlocalx, fTlocaly;
int f;
int index[4];
fTlocalx = floor(point[0]); fTlocaly = floor(point[1]);
xBas0=(int) fTlocalx; yBas0=(int) fTlocaly;
xBas1=xBas0+1; yBas1=yBas0+1;
/* Linear interpolation constants (percentages) */
xCom=point[0]-fTlocalx; yCom=point[1]-fTlocaly;
xComi=(1-xCom); yComi=(1-yCom);
perc[0]=xComi * yComi;
perc[1]=xComi * yCom;
perc[2]=xCom * yComi;
perc[3]=xCom * yCom;
/* Stick to boundary */
if(xBas0<0) { xBas0=0; if(xBas1<0) { xBas1=0; }}
if(yBas0<0) { yBas0=0; if(yBas1<0) { yBas1=0; }}
if(xBas1>(Isize[0]-1)) { xBas1=Isize[0]-1; if(xBas0>(Isize[0]-1)) { xBas0=Isize[0]-1; }}
if(yBas1>(Isize[1]-1)) { yBas1=Isize[1]-1; if(yBas0>(Isize[1]-1)) { yBas0=Isize[1]-1; }}
/* Get the neighbour intensities */
index[0]=mindex2(xBas0, yBas0, Isize[0]);
index[1]=mindex2(xBas0, yBas1, Isize[0]);
index[2]=mindex2(xBas1, yBas0, Isize[0]);
index[3]=mindex2(xBas1, yBas1, Isize[0]);
f=Isize[0]*Isize[1];
/* the interpolated color */
Ireturn[0]=I[index[0]]*perc[0]+I[index[1]]*perc[1]+I[index[2]]*perc[2]+I[index[3]]*perc[3];
Ireturn[1]=I[index[0]+f]*perc[0]+I[index[1]+f]*perc[1]+I[index[2]+f]*perc[2]+I[index[3]+f]*perc[3];
}
/* The matlab mex function */
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
/* Ox and Oy are the grid points */
/* Zo is the input image */
/* Zi is the transformed image */
/* nx and ny are the number of grid points (inside the image) */
double *Ox,*Oy,*Oz,*dxa, *dya,*dza,*Iout;
mxArray *matlabCallOut[1]={0};
mxArray *matlabCallIn[1]={0};
double *Nthreadsd;
int Nthreads;
/* double pointer array to store all needed function variables) */
double ***ThreadArgs;
double **ThreadArgs1;
/* Handles to the worker threads */
ThreadHANDLE *ThreadList;
/* ID of Threads */
double **ThreadID;
double *ThreadID1;
double nlhs_d[1]={0};
/* Size of input image */
double *Isize_d;
mwSize dims[3];
/* Size of grid */
mwSize Osizex, Osizey, Osizez;
double Osize_d[3]={0,0,0};
const mwSize *dimso;
/* B-spline variablesl */
double u,v,w;
int u_index=0;
int v_index=0;
int w_index=0;
double *Bu, *Bv, *Bw;
double *Bdu, *Bdv, *Bdw;
/* Loop variable */
int i;
/* Grid distance */
int dx,dy,dz;
/* X,Y,Z coordinates of current pixel */
int x,y,z;
/* Check for proper number of arguments. */
if(nrhs!=7) {
mexErrMsgTxt("Seven inputs are required.");
}
/* Get the sizes of the grid */
dimso = mxGetDimensions(prhs[0]);
Osizex = dimso[0];
Osizey = dimso[1];
Osizez = dimso[2];
/* Assign pointers to each input. */
Ox=(double *)mxGetData(prhs[0]);
Oy=(double *)mxGetData(prhs[1]);
Oz=(double *)mxGetData(prhs[2]);
Isize_d=(double *)mxGetData(prhs[3]);
dxa=(double *)mxGetData(prhs[4]);
dya=(double *)mxGetData(prhs[5]);
dza=(double *)mxGetData(prhs[6]);
/* Create image matrix for the return arguments with the size of input image */
dims[0]=(mwSize)Isize_d[0];
dims[1]=(mwSize)Isize_d[1];
dims[2]=(mwSize)Isize_d[2];
plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
/* Get the spacing of the uniform b-spline grid */
dx=(int)dxa[0]; dy=(int)dya[0]; dz=(int)dza[0];
/* Get number of allowed threads */
mexCallMATLAB(1, matlabCallOut, 0, matlabCallIn, "maxNumCompThreads");
Nthreadsd=mxGetPr(matlabCallOut[0]);
Nthreads=(int)Nthreadsd[0];
/* Reserve room for handles of threads in ThreadList */
ThreadList = (ThreadHANDLE*)malloc(Nthreads* sizeof( ThreadHANDLE ));
ThreadID = (double **)malloc( Nthreads* sizeof(double *) );
ThreadArgs = (double ***)malloc( Nthreads* sizeof(double **) );
/* Assign pointer to output. */
Iout = (double *)mxGetData(plhs[0]);
/* Make polynomial look up tables */
Bu=malloc(dx*4*sizeof(double));
Bv=malloc(dy*4*sizeof(double));
Bw=malloc(dz*4*sizeof(double));
Bdu=malloc(dx*4*sizeof(double));
Bdv=malloc(dy*4*sizeof(double));
Bdw=malloc(dz*4*sizeof(double));
for (x=0; x<dx; x++)
{
u=((double)x/(double)dx)-floor((double)x/(double)dx);
Bu[mindex2(0,x,4)] = BsplineCoefficient(u,0);
Bu[mindex2(1,x,4)] = BsplineCoefficient(u,1);
Bu[mindex2(2,x,4)] = BsplineCoefficient(u,2);
Bu[mindex2(3,x,4)] = BsplineCoefficient(u,3);
Bdu[mindex2(0,x,4)] = BsplineCoefficientDerivative(u,0)/dxa[0];
Bdu[mindex2(1,x,4)] = BsplineCoefficientDerivative(u,1)/dxa[0];
Bdu[mindex2(2,x,4)] = BsplineCoefficientDerivative(u,2)/dxa[0];
Bdu[mindex2(3,x,4)] = BsplineCoefficientDerivative(u,3)/dxa[0];
}
for (y=0; y<dy; y++)
{
v=((double)y/(double)dy)-floor((double)y/(double)dy);
Bv[mindex2(0,y,4)] = BsplineCoefficient(v,0);
Bv[mindex2(1,y,4)] = BsplineCoefficient(v,1);
Bv[mindex2(2,y,4)] = BsplineCoefficient(v,2);
Bv[mindex2(3,y,4)] = BsplineCoefficient(v,3);
Bdv[mindex2(0,y,4)] = BsplineCoefficientDerivative(v,0)/dya[0];
Bdv[mindex2(1,y,4)] = BsplineCoefficientDerivative(v,1)/dya[0];
Bdv[mindex2(2,y,4)] = BsplineCoefficientDerivative(v,2)/dya[0];
Bdv[mindex2(3,y,4)] = BsplineCoefficientDerivative(v,3)/dya[0];
}
for (z=0; z<dz; z++)
{
w=((double)z/(double)dz)-floor((double)z/(double)dz);
Bw[mindex2(0,z,4)] = BsplineCoefficient(w,0);
Bw[mindex2(1,z,4)] = BsplineCoefficient(w,1);
Bw[mindex2(2,z,4)] = BsplineCoefficient(w,2);
Bw[mindex2(3,z,4)] = BsplineCoefficient(w,3);
Bdw[mindex2(0,z,4)] = BsplineCoefficientDerivative(w,0)/dza[0];
Bdw[mindex2(1,z,4)] = BsplineCoefficientDerivative(w,1)/dza[0];
Bdw[mindex2(2,z,4)] = BsplineCoefficientDerivative(w,2)/dza[0];
Bdw[mindex2(3,z,4)] = BsplineCoefficientDerivative(w,3)/dza[0];
}
Osize_d[0]=(double)Osizex; Osize_d[1]=(double)Osizey; Osize_d[2]=(double)Osizez;
nlhs_d[0]=(double)nlhs;
/* Reserve room for 16 function variables(arrays) */
for (i=0; i<Nthreads; i++)
{
/* Make Thread ID */
ThreadID1= (double *)malloc( 1* sizeof(double) );
ThreadID1[0]=(double)i;
ThreadID[i]=ThreadID1;
/* Make Thread Structure */
ThreadArgs1 = (double **)malloc( 17* sizeof( double * ) );
ThreadArgs1[0]=Bu;
ThreadArgs1[1]=Bv;
ThreadArgs1[2]=Bw;
ThreadArgs1[3]=Isize_d;
ThreadArgs1[4]=Osize_d;
ThreadArgs1[5]=Iout;
ThreadArgs1[6]=dxa;
ThreadArgs1[7]=dya;
ThreadArgs1[8]=dza;
ThreadArgs1[9]=ThreadID[i];
ThreadArgs1[10]=Ox;
ThreadArgs1[11]=Oy;
ThreadArgs1[12]=Oz;
ThreadArgs1[13]=Nthreadsd;
ThreadArgs1[14]=Bdu;
ThreadArgs1[15]=Bdv;
ThreadArgs1[16]=Bdw;
ThreadArgs[i]=ThreadArgs1;
StartThread(ThreadList[i], &transformvolume_jacobiandet, ThreadArgs[i])
}
for (i=0; i<Nthreads; i++) { WaitForThreadFinish(ThreadList[i]); }
for (i=0; i<Nthreads; i++)
{
free(ThreadArgs[i]);
free(ThreadID[i]);
}
free(ThreadArgs);
free(ThreadID );
free(ThreadList);
free(Bu);
free(Bv);
free(Bw);
free(Bdu);
free(Bdv);
free(Bdw);
}
/* The matlab mex function */
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] ) {
/* Ox and Oy are the grid points */
/* Zo is the input image */
/* Zi is the transformed image */
/* dx and dy are the spacing of the b-spline knots */
double *Ox, *Oy, *dxa, *dya, *E, *Egradient;
double *ThreadErrorOut, *ThreadGradientOutX, *ThreadGradientOutY;
mxArray *matlabCallOut[1]={0};
mxArray *matlabCallIn[1]={0};
double *Nthreadsd;
int Nthreads;
/* Finite difference step size */
double step=0.001;
/* index offsets */
int offset1;
/* double pointer array to store all needed function variables) */
double ***ThreadArgs;
double **ThreadArgs1;
/* Handles to the worker threads */
ThreadHANDLE *ThreadList;
/* ID of Threads */
double **ThreadID;
double *ThreadID1;
/* Dims outputs */
const int dims_error[2]={1, 1};
int dims_error_gradient[3]={1, 1, 2};
/* Size of input image */
double *Isize_d;
/* Size of grid */
mwSize Osizex, Osizey;
int Onumel;
double Inumel;
double Osize_d[2]={0, 0};
/* B-spline variablesl */
double u, v;
int u_index=0;
int v_index=0;
double *Bu, *Bv, *Bdu, *Bdv;
/* Loop variables */
int i, j;
/* X,Y coordinates of current pixel */
int x, y;
/* Grid distance */
int dx, dy;
/* Check for proper number of arguments. */
if(nrhs!=5) {
mexErrMsgTxt("Five nputs are required.");
}
/* Get the sizes of the grid */
Osizex = (mwSize)mxGetM(prhs[0]);
Osizey = (mwSize)mxGetN(prhs[0]);
/* Assign pointers to each input. */
Ox=mxGetPr(prhs[0]);
Oy=mxGetPr(prhs[1]);
Isize_d=mxGetPr(prhs[2]);
dxa=mxGetPr(prhs[3]);
dya=mxGetPr(prhs[4]);
Onumel= Osizex*Osizey;
Inumel = Isize_d[0]*Isize_d[1];
/* Create image matrix for the Error return argument */
plhs[0] = mxCreateNumericArray(2, dims_error, mxDOUBLE_CLASS, mxREAL);
if(nlhs>1) {
dims_error_gradient[0]=Osizex;
dims_error_gradient[1]=Osizey;
/* Error Gradient needed */
plhs[1] = mxCreateNumericArray(3, dims_error_gradient, mxDOUBLE_CLASS, mxREAL);
}
/* Get the spacing of the uniform b-spline grid */
dx=(int)dxa[0]; dy=(int)dya[0];
/* Get number of allowed threads */
mexCallMATLAB(1, matlabCallOut, 0, matlabCallIn, "maxNumCompThreads");
Nthreadsd=mxGetPr(matlabCallOut[0]);
Nthreads=(int)Nthreadsd[0];
/* Reserve room for handles of threads in ThreadList */
ThreadList = (ThreadHANDLE*)malloc(Nthreads* sizeof( ThreadHANDLE ));
ThreadID = (double **)malloc( Nthreads* sizeof(double *) );
ThreadArgs = (double ***)malloc( Nthreads* sizeof(double **) );
ThreadErrorOut= (double *)malloc(Nthreads* sizeof(double) );
if(nlhs==1)
{
ThreadGradientOutX=NULL;
ThreadGradientOutY=NULL;
}
else
{
ThreadGradientOutX= (double *)malloc(Nthreads*Onumel*sizeof(double));
ThreadGradientOutY= (double *)malloc(Nthreads*Onumel*sizeof(double));
}
/* Assign pointer to output. */
E = mxGetPr(plhs[0]);
if(nlhs>1) { Egradient = mxGetPr(plhs[1]); }
/* Make polynomial look up tables */
Bu=malloc(dx*4*sizeof(double));
Bv=malloc(dy*4*sizeof(double));
Bdu=malloc(dx*4*sizeof(double));
Bdv=malloc(dy*4*sizeof(double));
for (x=0; x<dx; x++) {
u=(x/(double)dx)-floor(x/(double)dx);
Bu[mindex2(0, x, 4)] = BsplineCoefficient(u, 0);
Bu[mindex2(1, x, 4)] = BsplineCoefficient(u, 1);
Bu[mindex2(2, x, 4)] = BsplineCoefficient(u, 2);
Bu[mindex2(3, x, 4)] = BsplineCoefficient(u, 3);
Bdu[mindex2(0, x, 4)] = BsplineCoefficientDerivative(u, 0)/dxa[0];
Bdu[mindex2(1, x, 4)] = BsplineCoefficientDerivative(u, 1)/dxa[0];
Bdu[mindex2(2, x, 4)] = BsplineCoefficientDerivative(u, 2)/dxa[0];
Bdu[mindex2(3, x, 4)] = BsplineCoefficientDerivative(u, 3)/dxa[0];
}
for (y=0; y<dy; y++) {
v=(y/(double)dy)-floor(y/(double)dy);
Bv[mindex2(0, y, 4)] = BsplineCoefficient(v, 0);
Bv[mindex2(1, y, 4)] = BsplineCoefficient(v, 1);
Bv[mindex2(2, y, 4)] = BsplineCoefficient(v, 2);
Bv[mindex2(3, y, 4)] = BsplineCoefficient(v, 3);
Bdv[mindex2(0, y, 4)] = BsplineCoefficientDerivative(v, 0)/dya[0];
Bdv[mindex2(1, y, 4)] = BsplineCoefficientDerivative(v, 1)/dya[0];
Bdv[mindex2(2, y, 4)] = BsplineCoefficientDerivative(v, 2)/dya[0];
Bdv[mindex2(3, y, 4)] = BsplineCoefficientDerivative(v, 3)/dya[0];
}
Osize_d[0]=Osizex; Osize_d[1]=Osizey;
/* Reserve room for 14 function variables(arrays) */
for (i=0; i<Nthreads; i++) {
/* Make Thread ID */
ThreadID1= (double *)malloc( 1* sizeof(double) );
ThreadID1[0]=i;
ThreadID[i]=ThreadID1;
/* Make Thread Structure */
ThreadArgs1 = (double **)malloc( 15 * sizeof( double * ) );
ThreadArgs1[0]=Bu;
ThreadArgs1[1]=Bv;
ThreadArgs1[2]=Isize_d;
ThreadArgs1[3]=Osize_d;
ThreadArgs1[4]=ThreadErrorOut;
ThreadArgs1[5]=dxa;
ThreadArgs1[6]=dya;
ThreadArgs1[7]=ThreadID[i];
ThreadArgs1[8]=Ox;
ThreadArgs1[9]=Oy;
ThreadArgs1[10]=Nthreadsd;
ThreadArgs1[11]=Bdu;
ThreadArgs1[12]=Bdv;
ThreadArgs1[13]=ThreadGradientOutX;
ThreadArgs1[14]=ThreadGradientOutY;
ThreadArgs[i]=ThreadArgs1;
if(nlhs>1)
{
StartThread(ThreadList[i], &jacobian_errorgradient, ThreadArgs[i])
}
else
{
StartThread(ThreadList[i], &jacobian_error, ThreadArgs[i])
}
}
for (i=0; i<Nthreads; i++) { WaitForThreadFinish(ThreadList[i]); }
/* Add accumlated error of all threads */
E[0]=0;
for (i=0; i<Nthreads; i++)
{
E[0]+=ThreadErrorOut[i];
}
E[0]/=Inumel;
if(nlhs>1) {
for (i=0; i<Nthreads; i++) {
offset1=i*Onumel;
for(j=0; j<Onumel; j++) {
Egradient[j]+=ThreadGradientOutX[j+offset1];
Egradient[j+Onumel]+=ThreadGradientOutY[j+offset1];
}
}
for(j=0; j<Onumel; j++) {
Egradient[j]/=Inumel*step;
Egradient[j+Onumel]/=Inumel*step;
}
}
for (i=0; i<Nthreads; i++) {
free(ThreadArgs[i]);
free(ThreadID[i]);
}
free(ThreadErrorOut);
free(ThreadGradientOutX);
free(ThreadGradientOutY);
free(ThreadArgs);
free(ThreadID );
free(ThreadList);
free(Bu);
free(Bdu);
free(Bv);
free(Bdv);
}
voidthread transformvolume_jacobiandet(double **Args) {
double *Bu, *Bv, *Bdu, *Bdv, *Iout, *dxa, *dya, *ThreadID, *Ox, *Oy;
double *Isize_d;
double *Osize_d;
int Isize[3]={0,0,0};
int Osize[2]={0,0};
double *Nthreadsd;
int Nthreads;
/* Multiple threads, one does the odd the other even indexes */
int ThreadOffset;
/* Location of pixel which will be come the current pixel */
double Tlocalx,Tlocaly;
double Tlocaldxx, Tlocaldxy, Tlocaldyy, Tlocaldyx;
/* Variables to store 1D index */
int indexO;
int indexI;
/* Grid distance */
int dx,dy;
/* X,Y coordinates of current pixel */
int x,y;
/* B-spline variablesl */
int u_index=0;
int v_index=0;
int i, j;
/* temporary value */
double valx,valy;
/* Look up tables index */
int *u_index_array, *i_array;
int *v_index_array, *j_array;
/* B-Spline loop variabels */
int l,m;
/* Current voxel/pixel */
double Ipixel[3]={0,0,0};
/* Split input into variables */
Bu=Args[0];
Bv=Args[1];
Isize_d=Args[2];
Osize_d=Args[3];
Iout=Args[4];
dxa=Args[5];
dya=Args[6];
ThreadID=Args[7];
Ox=Args[8];
Oy=Args[9];
Nthreadsd=Args[10]; Nthreads=(int)Nthreadsd[0];
Bdu=Args[11];
Bdv=Args[12];
Isize[0] = (int)Isize_d[0];
Isize[1] = (int)Isize_d[1];
Osize[0] = (int)Osize_d[0];
Osize[1] = (int)Osize_d[1];
/* Get the spacing of the uniform b-spline grid */
dx=(int)dxa[0]; dy=(int)dya[0];
ThreadOffset=(int) ThreadID[0];
/* Calculate the indexs need to look up the B-spline values. */
u_index_array= (int*)malloc(Isize[0]* sizeof(int));
i_array= (int*)malloc(Isize[0]* sizeof(int));
v_index_array= (int*)malloc(Isize[1]* sizeof(int));
j_array= (int*)malloc(Isize[1]* sizeof(int));
for (x=0; x<Isize[0]; x++) {
u_index_array[x]=(x%dx)*4; /* Already multiplied by 4, because it specifies the y dimension */
i_array[x]=(int)floor((double)x/dx); /* (first row outside image against boundary artefacts) */
}
for (y=ThreadOffset; y<Isize[1]; y++) {
v_index_array[y]=(y%dy)*4; j_array[y]=(int)floor((double)y/dy);
}
/* Loop through all image pixel coordinates */
for (y=ThreadOffset; y<Isize[1]; y=y+Nthreads)
{
v_index=v_index_array[y]; j=j_array[y];
for (x=0; x<Isize[0]; x++)
{
/* Calculate the index needed to loop up the B-spline values. */
u_index=u_index_array[x]; i=i_array[x];
/* This part calculates the coordinates of the pixel */
/* which will be transformed to the current x,y pixel. */
Tlocalx=0; Tlocaly=0;
Tlocaldxx=0; Tlocaldxy=0;
Tlocaldyy=0; Tlocaldyx=0;
for(l=0; l<4; l++)
{
if(((i+l)>=0)&&((i+l)<Osize[0]))
{
for(m=0; m<4; m++)
{
if(((j+m)>=0)&&((j+m)<Osize[1]))
{
indexO=(i+l)+(j+m)*Osize[0];
valx=Bdu[l+u_index]*Bv[m+v_index];
valy=Bu[l+u_index]*Bdv[m+v_index];
Tlocaldxx+=valx*Ox[indexO];
Tlocaldyy+=valy*Oy[indexO];
Tlocaldxy+=valy*Ox[indexO];
Tlocaldyx+=valx*Oy[indexO];
}
}
}
}
/* Set the current pixel value */
indexI=mindex2(x,y,Isize[0]);
Iout[indexI]=Tlocaldxx*Tlocaldyy-Tlocaldyx*Tlocaldxy;
}
}
/* Free memory index look up tables */
free(u_index_array);
free(v_index_array);
free(i_array);
free(j_array);
/* explicit end thread, helps to ensure proper recovery of resources allocated for the thread */
EndThread;
}
unsigned __stdcall transformvolume_gray(double **Args) {
#else
void transformvolume_gray(double **Args) {
#endif
double *Isize_d, *A, *Iin, *Iout, *ThreadID, *moded, *Jsize_d;
int Isize[3]={0,0,0};
int Jsize[3]={0,0,0};
double Imean[2]={0,0};
double Jmean[2]={0,0};
int mode=0;
int x,y;
double *Nthreadsd;
int Nthreads;
/* Location of pixel which will be come the current pixel */
double Tlocalx;
double Tlocaly;
/* X,Y,Z coordinates of current pixel */
double xd,yd;
/* Parts of location calculation */
double compa0, compa1, compb0, compb1;
/* Variables to store 1D index */
int indexI;
/* Cubic and outside black booleans */
bool black, cubic;
/* Multiple threads, one does the odd the other even indexes */
int ThreadOffset;
/* int start; */
/* int end;; */
Isize_d=Args[0];
Jsize_d=Args[1];
A=Args[2];
Iin=Args[3];
Iout=Args[4];
ThreadID=Args[5];
moded=Args[6]; mode=(int) moded[0];
Nthreadsd=Args[7]; Nthreads=(int)Nthreadsd[0];
/* Center of the image */
Imean[0]=Isize_d[0]/2; Imean[1]=Isize_d[1]/2;
Jmean[0]=Jsize_d[0]/2; Jmean[1]=Jsize_d[1]/2;
black = true;
cubic = false;
if(mode==0||mode==2){ black = false; } else { black = true; }
if(mode==0||mode==1){ cubic = false; } else { cubic = true; }
Isize[0] = (int)Isize_d[0];
Isize[1] = (int)Isize_d[1];
Isize[2] = (int)Isize_d[2];
Jsize[0] = (int)Jsize_d[0];
Jsize[1] = (int)Jsize_d[1];
Jsize[2] = (int)Jsize_d[2];
ThreadOffset=(int) ThreadID[0];
compb0= A[2] + Imean[0];
compb1= A[5] + Imean[1];
/* Loop through all image pixel coordinates */
for (y=ThreadOffset; y<Jsize[1]; y=y+Nthreads)
{
yd=(double)y-Jmean[1];
compa0 = A[1] *yd + compb0;
compa1 = A[4] *yd + compb1;
for (x=0; x<Jsize[0]; x++)
{
xd=(double)x-Jmean[0];
Tlocalx = A[0] * xd + compa0;
Tlocaly = A[3] * xd + compa1;
/* Set the current pixel value */
indexI=mindex2(x,y,Jsize[0]);
/* interpolate the intensities */
Iout[indexI]=interpolate_2d_double_gray(Tlocalx, Tlocaly, Isize, Iin,cubic,black);
}
}
/* explicit end thread, helps to ensure proper recovery of resources allocated for the thread */
#ifdef _WIN32
_endthreadex( 0 );
return 0;
#else
pthread_exit(NULL);
#endif
}
/* The matlab mex function */
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
/* Ox and Oy are the grid points */
/* Zo is the input image */
/* Zi is the transformed image */
/* nx and ny are the number of grid points (inside the image) */
double *Iin, *Iout, *M, *moded;
mxArray *matlabCallOut[1]={0};
//mxArray *matlabCallIn[1]={0}; // commented by Ilya Belevich
mxArray *matlabCallIn[1]={mxCreateString("Numcores")}; // added by Ilya Belevich
double *Nthreadsd;
int Nthreads;
/* double pointer array to store all needed function variables) */
double ***ThreadArgs;
double **ThreadArgs1;
/* Handles to the worker threads */
#ifdef _WIN32
HANDLE *ThreadList;
#else
pthread_t *ThreadList;
#endif
/* ID of Threads */
double **ThreadID;
double *ThreadID1;
/* Transformation matrix */
double A[9]={0,0,0,0,0,0,0,0,0};
/* Loop variable */
int i;
/* Size of input image */
double Isize_d[3]={0,0,0};
double Jsize_d[3]={0,0,0};
int Jdimsc[3]={0,0,0};
const mwSize *dims;
double *Jdims;
/* Check for proper number of arguments. */
if(nrhs<3) {
mexErrMsgTxt("3 or 4 inputs are required.");
} else if(nlhs!=1) {
mexErrMsgTxt("One output required");
}
/* Get the sizes of the image */
dims = mxGetDimensions(prhs[0]);
Isize_d[0] = (double)dims[0]; Isize_d[1] = (double)dims[1];
/* Detect if color image */
if(mxGetNumberOfDimensions(prhs[0])>2) { Isize_d[2]=(double)3; } else { Isize_d[2]=1; }
/* Get output image size */
if(nrhs==4)
{
Jdims = mxGetPr(prhs[3]);
Jsize_d[0] = (double)Jdims[0];
Jsize_d[1] = (double)Jdims[1];
if(Isize_d[2]>0) { Jsize_d[2]=Isize_d[2]; }
}
else
{
Jsize_d[0] = Isize_d[0];
Jsize_d[1] = Isize_d[1];
if(Isize_d[2]>0) { Jsize_d[2]=Isize_d[2]; }
}
/* Create output array */
Jdimsc[0]=(int)Jsize_d[0];
Jdimsc[1]=(int)Jsize_d[1];
Jdimsc[2]=(int)Jsize_d[2];
if(Isize_d[2]>1) {
plhs[0] = mxCreateNumericArray(3, Jdimsc, mxDOUBLE_CLASS, mxREAL);
}
else {
plhs[0] = mxCreateNumericArray(2, Jdimsc, mxDOUBLE_CLASS, mxREAL);
}
/* Assign pointers to each input. */
Iin=mxGetPr(prhs[0]);
M=mxGetPr(prhs[1]);
moded=mxGetPr(prhs[2]);
A[0] = M[mindex2(0,0,3)]; A[1] = M[mindex2(0,1,3)]; A[2] = M[mindex2(0,2,3)];
A[3] = M[mindex2(1,0,3)]; A[4] = M[mindex2(1,1,3)]; A[5] = M[mindex2(1,2,3)];
A[6] = M[mindex2(2,0,3)]; A[7] = M[mindex2(2,1,3)]; A[8] = M[mindex2(2,2,3)];
//mexCallMATLAB(1, matlabCallOut, 0, matlabCallIn, "maxNumCompThreads"); // commented by Ilya Belevich
mexCallMATLAB(1, matlabCallOut, 1, matlabCallIn, "feature"); // feature('Numcores') added by Ilya Belevich
Nthreadsd=mxGetPr(matlabCallOut[0]);
Nthreads=(int)Nthreadsd[0];
/* Reserve room for handles of threads in ThreadList */
#ifdef _WIN32
ThreadList = (HANDLE*)malloc(Nthreads* sizeof( HANDLE ));
#else
ThreadList = (pthread_t*)malloc(Nthreads* sizeof( pthread_t ));
#endif
ThreadID = (double **)malloc( Nthreads* sizeof(double *) );
ThreadArgs = (double ***)malloc( Nthreads* sizeof(double **) );
/* Assign pointer to output. */
Iout = mxGetPr(plhs[0]);
for (i=0; i<Nthreads; i++)
{
/* Make Thread ID */
ThreadID1= (double *)malloc( 1* sizeof(double) );
ThreadID1[0]=i;
ThreadID[i]=ThreadID1;
/* Make Thread Structure */
ThreadArgs1 = (double **)malloc( 9* sizeof( double * ) );
ThreadArgs1[0]=Isize_d;
ThreadArgs1[1]=Jsize_d;
ThreadArgs1[2]=A;
ThreadArgs1[3]=Iin;
ThreadArgs1[4]=Iout;
ThreadArgs1[5]=ThreadID[i];
ThreadArgs1[6]=moded;
ThreadArgs1[7]=Nthreadsd;
/* Start a Thread */
ThreadArgs[i]=ThreadArgs1;
if(Isize_d[2]>1) {
#ifdef _WIN32
ThreadList[i] = (HANDLE)_beginthreadex( NULL, 0, &transformvolume_color, ThreadArgs[i] , 0, NULL );
#else
pthread_create ((pthread_t*)&ThreadList[i], NULL, (void *) &transformvolume_color, ThreadArgs[i]);
#endif
}
else
{
#ifdef _WIN32
ThreadList[i] = (HANDLE)_beginthreadex( NULL, 0, &transformvolume_gray, ThreadArgs[i] , 0, NULL );
#else
pthread_create ((pthread_t*)&ThreadList[i], NULL, (void *) &transformvolume_gray, ThreadArgs[i]);
#endif
}
}
#ifdef _WIN32
for (i=0; i<Nthreads; i++) { WaitForSingleObject(ThreadList[i], INFINITE); }
for (i=0; i<Nthreads; i++) { CloseHandle( ThreadList[i] ); }
#else
for (i=0; i<Nthreads; i++) { pthread_join(ThreadList[i],NULL); }
#endif
for (i=0; i<Nthreads; i++)
{
free(ThreadArgs[i]);
free(ThreadID[i]);
}
free(ThreadArgs);
free(ThreadID );
free(ThreadList);
}
/* The matlab mex function */
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
/* I is the input image, Iout the transformed image */
/* Tx and Ty images of the translation of every pixel. */
double *Iin, *Iout, *Tx, *Ty, *ImageSizeT;
double *moded;
mxArray *matlabCallOut[1]={0};
mxArray *matlabCallIn[1]={0};
double *Nthreadsd;
int Nthreads;
double *Tlocalx, *Tlocaly;
int x,y;
int index;
/* double pointer array to store all needed function variables) */
double ***ThreadArgs;
double **ThreadArgs1;
/* Handles to the worker threads */
ThreadHANDLE *ThreadList;
/* ID of Threads */
double **ThreadID;
double *ThreadID1;
/* Size of input image */
const mwSize *dims;
double Isize_d[3]={0,0,0};
int Isize[3]={1,1,1};
/* Size of output image */
double ImageSize_d[3]={0,0,0};
int ImageSize[3]={1,1,1};
/* Loop variable */
int i;
/* Check for proper number of arguments. */
if(nrhs<4) {
mexErrMsgTxt("Four inputs are required.");
} else if(nlhs!=1) {
mexErrMsgTxt("One output required");
}
/* Get the sizes of the image */
dims = mxGetDimensions(prhs[0]);
Isize_d[0] = (double)dims[0]; Isize_d[1] = (double)dims[1];
/* Detect if color image */
if(mxGetNumberOfDimensions(prhs[0])>2) { Isize_d[2]=(double)3; } else { Isize_d[2]=1; }
Isize[0]=(int)Isize_d[0];
Isize[1]=(int)Isize_d[1];
Isize[2]=(int)Isize_d[2];
/* Assign pointers to each input. */
Iin=mxGetPr(prhs[0]);
Tx=mxGetPr(prhs[1]);
Ty=mxGetPr(prhs[2]);
moded=mxGetPr(prhs[3]);
if(nrhs==5)
{
ImageSizeT=mxGetPr(prhs[4]);
}
if(nrhs==5)
{
ImageSize_d[0]=ImageSizeT[0];
ImageSize_d[1]=ImageSizeT[1];
ImageSize_d[2]=Isize_d[2];
}
else
{
ImageSize_d[0]= Isize_d[0];
ImageSize_d[1]= Isize_d[1];
ImageSize_d[2]= Isize_d[2];
}
ImageSize[0]=(int)ImageSize_d[0];
ImageSize[1]=(int)ImageSize_d[1];
ImageSize[2]=(int)ImageSize_d[2];
/* Create image matrix for the return arguments with the size of input image */
if(Isize_d[2]>1) {
plhs[0] = mxCreateNumericArray(3, ImageSize, mxDOUBLE_CLASS, mxREAL);
}
else {
plhs[0] = mxCreateNumericArray(2, ImageSize, mxDOUBLE_CLASS, mxREAL);
}
/* Assign pointer to output. */
Iout = mxGetPr(plhs[0]);
if(moded[0]==4)
{
Tlocalx=(double*)malloc(Isize[0]*Isize[1]*sizeof(double));
Tlocaly=(double*)malloc(Isize[0]*Isize[1]*sizeof(double));
for (y=0; y<Isize[1]; y++)
{
for (x=0; x<Isize[0]; x++)
{
index=mindex2(x,y,Isize[0]);
Tlocalx[index]=((double)x)+Tx[index];
Tlocaly[index]=((double)y)+Ty[index];
}
}
interpolate_forward_2d_double(Iin, Tlocalx, Tlocaly, Isize, ImageSize, Iout);
free(Tlocalx);
free(Tlocaly);
}
else
{
mexCallMATLAB(1, matlabCallOut, 0, matlabCallIn, "maxNumCompThreads");
Nthreadsd=mxGetPr(matlabCallOut[0]);
Nthreads=(int)Nthreadsd[0];
/* Reserve room for handles of threads in ThreadList */
ThreadList = (ThreadHANDLE*)malloc(Nthreads* sizeof( ThreadHANDLE ));
ThreadID = (double **)malloc( Nthreads* sizeof(double *) );
ThreadArgs = (double ***)malloc( Nthreads* sizeof(double **) );
for (i=0; i<Nthreads; i++)
{
/* Make Thread ID */
ThreadID1= (double *)malloc( 1* sizeof(double) );
ThreadID1[0]=i;
ThreadID[i]=ThreadID1;
/* Make Thread Structure */
ThreadArgs1 = (double **)malloc( 9* sizeof( double * ) );
ThreadArgs1[0]=Iin;
ThreadArgs1[1]=Iout;
ThreadArgs1[2]=Tx;
ThreadArgs1[3]=Ty;
ThreadArgs1[4]=Isize_d;
ThreadArgs1[5]=ThreadID[i];
ThreadArgs1[6]=moded;
ThreadArgs1[7]=Nthreadsd;
ThreadArgs1[8]=ImageSize_d;
ThreadArgs[i]=ThreadArgs1;
if(Isize_d[2]>1) {
StartThread(ThreadList[i], &transformvolume_color, ThreadArgs[i])
}
else
{
StartThread(ThreadList[i], &transformvolume_gray, ThreadArgs[i])
}
}
for (i=0; i<Nthreads; i++) { WaitForThreadFinish(ThreadList[i]); }
for (i=0; i<Nthreads; i++)
{
free(ThreadArgs[i]);
free(ThreadID[i]);
}
free(ThreadArgs);
free(ThreadID );
free(ThreadList);
}
}
voidthread transformvolume_color(double **Args) {
/* I is the input image, Iout the transformed image */
/* Tx and Ty images of the translation of every pixel. */
double *Iin, *Iout, *Tx, *Ty;
double *Nthreadsd;
int Nthreads;
/* if one outside pixels are set to zero. */
double *moded;
int mode=0;
/* Output image size */
double *ImageSize_d;
int ImageSize[3];
/* 2D index storage */
int indexI;
/* Size of input image */
int Isize[3]={0,0,0};
double *Isize_d;
/* Location of translated pixel */
double Tlocalx;
double Tlocaly;
/* Cubic and outside black booleans */
bool black, cubic;
/* loop throught the colors r,g,b */
int rgb=0;
/* Current voxel/pixel */
double Ipixel[3]={0,0,0};
/* offset */
int offset=0;
/* The thread ID number/name */
double *ThreadID;
/* X,Y coordinates of current pixel */
int x,y;
Iin=Args[0];
Iout=Args[1];
Tx=Args[2];
Ty=Args[3];
Isize_d=Args[4];
ThreadID=Args[5];
moded=Args[6]; mode=(int) moded[0];
Nthreadsd=Args[7]; Nthreads=(int)Nthreadsd[0];
ImageSize_d=Args[8];
if(mode==0||mode==2){ black = false; } else { black = true; }
if(mode==0||mode==1){ cubic = false; } else { cubic = true; }
Isize[0]=(int)Isize_d[0];
Isize[1]=(int)Isize_d[1];
Isize[2]=(int)Isize_d[2];
ImageSize[0] = (int)ImageSize_d[0];
ImageSize[1] = (int)ImageSize_d[1];
ImageSize[2] = (int)ImageSize_d[2];
offset=(int) ThreadID[0];
/* Loop through all image pixel coordinates */
for (y=offset; y<ImageSize[1]; y=y+Nthreads)
{
for (x=0; x<ImageSize[0]; x++)
{
indexI=mindex2(x,y,ImageSize[0]);
Tlocalx=((double)x)+Tx[indexI];
Tlocaly=((double)y)+Ty[indexI];
/* interpolate the intensities */
interpolate_2d_double_color(Ipixel,Tlocalx, Tlocaly, Isize, Iin,cubic,black);
/* Set the current pixel value */
for (rgb=0; rgb<3; rgb++)
{
Iout[indexI+rgb*ImageSize[0]*ImageSize[1]]=Ipixel[rgb];
}
}
}
/* explicit end thread, helps to ensure proper recovery of resources allocated for the thread */
EndThread;
}
unsigned __stdcall transformvolume(double **Args)
{
double *Isize_d, *mean_in, *A, *Iin, *Iout, *ThreadID;
double *ImageSize_d, *check_bil_intp;
double mean_out[2]={0,0};
int bilintp=0;
int Isize[2]={0,0};
int ImageSize[2]={0,0};
int x,y;
double *Nthreadsd;
int Nthreads;
// Location of pixel which will be come the current pixel
double Tlocalx;
double Tlocaly;
// Linear interpolation variables
int xBas[4], yBas[4];
double perc[4]={0,0,0,0};
double xCom, yCom;
double color[4]={0,0,0,0};
// Color offsets;
int offset_out[3]={0,0,0};
int offset_in[3]={0,0,0};
// X,Y,Z coordinates of current pixel
double xd,yd;
// Variables to store 1D index
int indexI;
int indexI1, indexI2, indexI3, indexI4;
// Multiple threads, one does the odd the other even indexes
int offset;
//int start;
//int end;
// Loop through all colors r,g,b or only gray
int c;
Isize_d=Args[0];
mean_in=Args[1];
A=Args[2];
Iin=Args[3];
Iout=Args[4];
ThreadID=Args[5];
ImageSize_d=Args[6];
check_bil_intp=Args[7]; bilintp=(int)check_bil_intp[0];
Nthreadsd=Args[8]; Nthreads=(int)Nthreadsd[0];
/* Center of the output image */
mean_out[0]=ImageSize_d[0]/2; mean_out[1]=ImageSize_d[1]/2;
ImageSize[0] = (int)ImageSize_d[0];
ImageSize[1] = (int)ImageSize_d[1];
Isize[0] = (int)Isize_d[0];
Isize[1] = (int)Isize_d[1];
Isize[2] = (int)Isize_d[2];
offset=(int) ThreadID[0];
offset_out[0]=0;
offset_in[0]=0;
offset_out[1]=ImageSize[0]*ImageSize[1];
offset_in[1]=Isize[0]*Isize[1];
offset_out[2]=2*ImageSize[0]*ImageSize[1];
offset_in[2]=2*Isize[0]*Isize[1];
// Loop through all image pixel coordinates
for (y=offset; y<ImageSize[1]; y=y+Nthreads)
{
for (x=0; x<ImageSize[0]; x++)
{
xd=(double)x-mean_out[0]; yd=(double)y-mean_out[1];
Tlocalx = mean_in[0] + A[0] * xd + A[1] *yd + A[2] * 1;
Tlocaly = mean_in[1] + A[3] * xd + A[4] *yd + A[5] * 1;
if(bilintp>0)
{
// Determine the coordinates of the pixel(s) which will be come the current pixel
// (using linear interpolation)
xBas[0]=(int) floor(Tlocalx); yBas[0]=(int) floor(Tlocaly);
xBas[1]=xBas[0]+0; yBas[1]=yBas[0]+1;
xBas[2]=xBas[0]+1; yBas[2]=yBas[0]+0;
xBas[3]=xBas[0]+1; yBas[3]=yBas[0]+1;
// Linear interpolation constants (percentages)
xCom=Tlocalx-floor(Tlocalx); yCom=Tlocaly-floor(Tlocaly);
perc[0]=(1-xCom) * (1-yCom);
perc[1]=(1-xCom) * yCom;
perc[2]=xCom * (1-yCom);
perc[3]=xCom * yCom;
indexI1=mindex2c(xBas[0],yBas[0],Isize[0],Isize[1],perc,0);
indexI2=mindex2c(xBas[1],yBas[1],Isize[0],Isize[1],perc,1);
indexI3=mindex2c(xBas[2],yBas[2],Isize[0],Isize[1],perc,2);
indexI4=mindex2c(xBas[3],yBas[3],Isize[0],Isize[1],perc,3);
// Get index current pixel value
indexI=mindex2(x,y,ImageSize[0],ImageSize[1]);
for(c=0; c<Isize[2]; c++)
{
color[0]=Iin[indexI1+offset_in[c]];
color[1]=Iin[indexI2+offset_in[c]];
color[2]=Iin[indexI3+offset_in[c]];
color[3]=Iin[indexI4+offset_in[c]];
Iout[indexI+offset_out[c]]=color[0]*perc[0]+color[1]*perc[1]+color[2]*perc[2]+color[3]*perc[3];
}
}
else
{
// Get index current pixel value
indexI=mindex2(x,y,ImageSize[0],ImageSize[1]);
perc[0]=1;
indexI1=mindex2c((int) floor(Tlocalx+0.5),(int) floor(Tlocaly+0.5),Isize[0],Isize[1],perc,0);
for(c=0; c<Isize[2]; c++)
{
Iout[indexI+offset_out[c]]=Iin[indexI1+offset_in[c]]*perc[0];
}
}
}
}
// explicit end thread, helps to ensure proper recovery of resources allocated for the thread
_endthreadex( 0 );
return 0;
}
// The matlab mex function
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
// Ox and Oy are the grid points
// Zo is the input image
// Zi is the transformed image
// nx and ny are the number of grid points (inside the image)
double *Iin, *Iout, *M, *ImageSize, *check_bil_intp;
mxArray *matlabCallOut[1]={0};
mxArray *matlabCallIn[1]={0};
double *Nthreadsd;
int Nthreads;
// double pointer array to store all needed function variables
double ***ThreadArgs;
double **ThreadArgs1;
HANDLE *ThreadList; // Handles to the worker threads
// ID of Threads
double **ThreadID;
double *ThreadID1;
// Transformation matrix
double A[9]={0,0,0,0,0,0,0,0,0};
// Loop variable
int i;
// Size of input image
double Isize_d[3]={0,0,0};
const mwSize *dims;
mwSize dims_out3[3]={0,0,0};
mwSize dimnum;
double mean_in[2]={0,0};
/* Check for proper number of arguments. */
if(nrhs!=4) {
mexErrMsgTxt("Four inputs are required.");
} else if(nlhs!=1) {
mexErrMsgTxt("One output required");
}
// nsubs=mxGetNumberOfDimensions(prhs[0]);
// Get the sizes of the image
dimnum=mxGetNumberOfDimensions(prhs[0]);
dims = mxGetDimensions(prhs[0]);
Isize_d[0] = (double)dims[0]; Isize_d[1] = (double)dims[1];
if(dimnum>2)
{
Isize_d[2] = (double)dims[2];
}
else
{
Isize_d[2]=1;
}
/* Assign pointers to each input. */
Iin=mxGetPr(prhs[0]);
M=mxGetPr(prhs[1]);
ImageSize=mxGetPr(prhs[2]);
check_bil_intp=mxGetPr(prhs[3]);
dims_out3[0]=(mwSize)ImageSize[0]; dims_out3[1]=(mwSize)ImageSize[1]; dims_out3[2]=(mwSize)Isize_d[2];
plhs[0] = mxCreateNumericArray(3, dims_out3, mxDOUBLE_CLASS, mxREAL);
A[0] = M[mindex2(0,0,3,3)]; A[1] = M[mindex2(0,1,3,3)]; A[2] = M[mindex2(0,2,3,3)];
A[3] = M[mindex2(1,0,3,3)]; A[4] = M[mindex2(1,1,3,3)]; A[5] = M[mindex2(1,2,3,3)];
A[6] = M[mindex2(2,0,3,3)]; A[7] = M[mindex2(2,1,3,3)]; A[8] = M[mindex2(2,2,3,3)];
mexCallMATLAB(1, matlabCallOut, 0, matlabCallIn, "maxNumCompThreads");
Nthreadsd=mxGetPr(matlabCallOut[0]);
Nthreads=(int)Nthreadsd[0];
// Reserve room for handles of threads in ThreadList
ThreadList = (HANDLE*)malloc(Nthreads* sizeof( HANDLE ));
ThreadID = (double **)malloc( Nthreads* sizeof(double *) );
ThreadArgs = (double ***)malloc( Nthreads* sizeof(double **) );
/* Assign pointer to output. */
Iout = mxGetPr(plhs[0]);
/* Center of the volume */
mean_in[0]=Isize_d[0]/2; mean_in[1]=Isize_d[1]/2;
for (i=0; i<Nthreads; i++)
{
// Make Thread ID
ThreadID1= (double *)malloc( 1* sizeof(double) );
ThreadID1[0]=i;
ThreadID[i]=ThreadID1;
// Make Thread Structure
ThreadArgs1 = (double **)malloc( 8* sizeof( double * ) );
ThreadArgs1[0]=Isize_d;
ThreadArgs1[1]=mean_in;
ThreadArgs1[2]=A;
ThreadArgs1[3]=Iin;
ThreadArgs1[4]=Iout;
ThreadArgs1[5]=ThreadID[i];
ThreadArgs1[6]=ImageSize;
ThreadArgs1[7]=check_bil_intp;
ThreadArgs1[8]=Nthreadsd;
// Start a Thread
ThreadArgs[i]=ThreadArgs1;
ThreadList[i] = (HANDLE)_beginthreadex( NULL, 0, &transformvolume, ThreadArgs[i] , 0, NULL );
}
for (i=0; i<Nthreads; i++) { WaitForSingleObject(ThreadList[i], INFINITE); }
for (i=0; i<Nthreads; i++) { CloseHandle( ThreadList[i] ); }
for (i=0; i<Nthreads; i++)
{
free(ThreadArgs[i]);
free(ThreadID[i]);
}
free(ThreadArgs);
free(ThreadID );
free(ThreadList);
}
/* The matlab mex function */
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
/* Ox and Oy are the grid points */
/* Zo is the input image */
/* Zi is the transformed image */
/* nx and ny are the number of grid points (inside the image) */
double *Ox,*Oy,*Oz,*I1,*I2,*dxa, *dya,*dza, *E, *Egradient, *ThreadOut;
mxArray *matlabCallOut[1]={0};
mxArray *matlabCallIn[1]={0};
double *Nthreadsd;
int Nthreads;
/* Finite difference step size */
double step=0.01;
/* index offsets */
int offset1, offset2, offset3;
/* Dims outputs */
const int dims_error[2]={1,1};
int dims_error_gradient[4]={1,1,1,3};
/* double pointer array to store all needed function variables) */
double ***ThreadArgs;
double **ThreadArgs1;
/* Handles to the worker threads */
ThreadHANDLE *ThreadList;
/* ID of Threads */
double **ThreadID;
double *ThreadID1;
/* Size of input image */
mwSize Isizex, Isizey, Isizez;
double Isize_d[3]={0,0,0};
const mwSize *dims;
/* Size of grid */
mwSize Osizex, Osizey, Osizez;
int Onumel;
double Osize_d[3]={0,0,0};
/* B-spline variablesl */
double u,v,w;
int u_index=0;
int v_index=0;
int w_index=0;
double *Bu, *Bv, *Bw;
/* Loop variables */
int i,j;
/* Grid distance */
int dx,dy,dz;
/* X,Y,Z coordinates of current pixel */
int x,y,z;
/* Check for proper number of arguments. */
if(nrhs!=8) {
mexErrMsgTxt("Eight inputs are required.");
}
/* Get the sizes of the grid */
dims = mxGetDimensions(prhs[0]);
Osizex = dims[0];
Osizey = dims[1];
Osizez = dims[2];
Onumel = Osizex*Osizey*Osizez;
/* Create image matrix for the return arguments with the size of input image */
dims = mxGetDimensions(prhs[3]);
Isizex = dims[0];
Isizey = dims[1];
Isizez = dims[2];
/* Create image matrix for the Error return argument */
plhs[0] = mxCreateNumericArray(2, dims_error, mxDOUBLE_CLASS, mxREAL);
if(nlhs>1)
{
dims_error_gradient[0]=Osizex; dims_error_gradient[1]=Osizey; dims_error_gradient[2]=Osizez;
/* Error Gradient needed */
plhs[1] = mxCreateNumericArray(4, dims_error_gradient, mxDOUBLE_CLASS, mxREAL);
}
/* Assign pointers to each input. */
Ox=(double *)mxGetData(prhs[0]);
Oy=(double *)mxGetData(prhs[1]);
Oz=(double *)mxGetData(prhs[2]);
I1=(double *)mxGetData(prhs[3]);
I2=(double *)mxGetData(prhs[4]);
dxa=(double *)mxGetData(prhs[5]);
dya=(double *)mxGetData(prhs[6]);
dza=(double *)mxGetData(prhs[7]);
/* Get the spacing of the uniform b-spline grid */
dx=(int)dxa[0]; dy=(int)dya[0]; dz=(int)dza[0];
/* Get number of allowed threads */
mexCallMATLAB(1, matlabCallOut, 0, matlabCallIn, "maxNumCompThreads");
Nthreadsd=mxGetPr(matlabCallOut[0]);
Nthreads=(int)Nthreadsd[0];
/* Reserve room for handles of threads in ThreadList */
ThreadList = (ThreadHANDLE*)malloc(Nthreads* sizeof( ThreadHANDLE ));
ThreadID = (double **)malloc( Nthreads* sizeof(double *) );
ThreadArgs = (double ***)malloc( Nthreads* sizeof(double **) );
if(nlhs==1){ ThreadOut = (double *)malloc(Nthreads* sizeof(double) ); }
else { ThreadOut = (double *)malloc(Nthreads*(1+Onumel*3)*sizeof(double) ); }
/* Assign pointer to output. */
E = mxGetPr(plhs[0]);
if(nlhs>1) { Egradient = mxGetPr(plhs[1]); }
/* Make polynomial look up tables */
Bu=malloc(dx*4*sizeof(double));
Bv=malloc(dy*4*sizeof(double));
Bw=malloc(dz*4*sizeof(double));
for (x=0; x<dx; x++)
{
u=((double)x/(double)dx)-floor((double)x/(double)dx);
Bu[mindex2(0,x,4)] = (double)pow((1-u),3)/6;
Bu[mindex2(1,x,4)] = (double)( 3*pow(u,3) - 6*pow(u,2) + 4)/6;
Bu[mindex2(2,x,4)] = (double)(-3*pow(u,3) + 3*pow(u,2) + 3*u + 1)/6;
Bu[mindex2(3,x,4)] = (double)pow(u,3)/6;
}
for (y=0; y<dy; y++)
{
v=((double)y/(double)dy)-floor((double)y/(double)dy);
Bv[mindex2(0,y,4)] = (double)pow((1-v),3)/6;
Bv[mindex2(1,y,4)] = (double)( 3*pow(v,3) - 6*pow(v,2) + 4)/6;
Bv[mindex2(2,y,4)] = (double)(-3*pow(v,3) + 3*pow(v,2) + 3*v + 1)/6;
Bv[mindex2(3,y,4)] = (double)pow(v,3)/6;
}
for (z=0; z<dz; z++)
{
w=((double)z/(double)dz)-floor((double)z/(double)dz);
Bw[mindex2(0,z,4)] = (double)pow((1-w),3)/6;
Bw[mindex2(1,z,4)] = (double)( 3*pow(w,3) - 6*pow(w,2) + 4)/6;
Bw[mindex2(2,z,4)] = (double)(-3*pow(w,3) + 3*pow(w,2) + 3*w + 1)/6;
Bw[mindex2(3,z,4)] = (double)pow(w,3)/6;
}
Isize_d[0]=(double)Isizex; Isize_d[1]=(double)Isizey; Isize_d[2]=(double)Isizez;
Osize_d[0]=(double)Osizex; Osize_d[1]=(double)Osizey; Osize_d[2]=(double)Osizez;
/* Reserve room for 16 function variables(arrays) */
for (i=0; i<Nthreads; i++)
{
/* Make Thread ID */
ThreadID1= (double *)malloc( 1* sizeof(double) );
ThreadID1[0]=(double)i;
ThreadID[i]=ThreadID1;
/* Make Thread Structure */
ThreadArgs1 = (double **)malloc( 16 * sizeof( double * ) );
ThreadArgs1[0]=Bu;
ThreadArgs1[1]=Bv;
ThreadArgs1[2]=Bw;
ThreadArgs1[3]=Isize_d;
ThreadArgs1[4]=Osize_d;
ThreadArgs1[5]=ThreadOut;
ThreadArgs1[6]=dxa;
ThreadArgs1[7]=dya;
ThreadArgs1[8]=dza;
ThreadArgs1[9]=ThreadID[i];
ThreadArgs1[10]=Ox;
ThreadArgs1[11]=Oy;
ThreadArgs1[12]=Oz;
ThreadArgs1[13]=I1;
ThreadArgs1[14]=I2;
ThreadArgs1[15]=Nthreadsd;
ThreadArgs[i]=ThreadArgs1;
if(nlhs==1){
StartThread(ThreadList[i], &transformvolume_error, ThreadArgs[i])
}
else{
StartThread(ThreadList[i], &transformvolume_gradient, ThreadArgs[i])
}
}
for (i=0; i<Nthreads; i++) { WaitForThreadFinish(ThreadList[i]); }
/* Add accumlated error of all threads */
E[0]=0; for (i=0; i<Nthreads; i++) { E[0]+=ThreadOut[i]; } E[0]/=Nthreads;
if(nlhs>1)
{
for (i=0; i<Nthreads; i++)
{
offset1=i*(3*Onumel);
offset2=offset1+Onumel;
offset3=offset2+Onumel;
for(j=0; j<Onumel; j++)
{
Egradient[j]+=ThreadOut[Nthreads+j+offset1]/step;
Egradient[j+Onumel]+=ThreadOut[Nthreads+j+offset2]/step;
Egradient[j+2*Onumel]+=ThreadOut[Nthreads+j+offset3]/step;
}
}
for(j=0; j<3*Onumel; j++)
{
Egradient[j]/=Nthreads;
}
}
for (i=0; i<Nthreads; i++)
{
free(ThreadArgs[i]);
free(ThreadID[i]);
}
free(ThreadArgs);
free(ThreadID );
free(ThreadList);
free(Bu);
free(Bv);
free(Bw);
}
// The matlab mex function
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) {
// Ox and Oy are the grid points
// Zo is the input image
// Zi is the transformed image
// nx and ny are the number of grid points (inside the image)
double *Iin, *x1, *y1, *x2, *y2, *steps, *Iout;
double Pi=3.14159265358979323846;
// Dimensions of in and outputs
const mwSize *Iin_dims;
const mwSize *x1_dims;
mwSize Iout_dims[3]={1, 1, 1};
// Center of the image
double center[2]={0, 0};
// Position of current pixel
double Tlocalx, Tlocaly;
double current_angle=0;
// Floor of coordinate
double fTlocalx, fTlocaly;
// Zero neighbor
int xBas0, yBas0;
// The location in between the pixels 0..1
double tx, ty;
// Neighbor loccations
int xn[4], yn[4];
// The vectors
double vector_tx[4], vector_ty[4];
double vector_qx[4]; //={0.25,0.25,0.25,0.25};
double vector_qy[4]; //={0.25,0.25,0.25,0.25};
double vector_b[4];
// Interpolated Intensity;
double Ipixel=0;
// Temporary value boundary
int b;
// Percentage pixel position between center and outer position
double perc=0;
// Loop variables
int i, j, k,l;
// 2D to 1D index
int index, indexoff1, indexoff2;
/* Check for proper number of arguments. */
if(nrhs!=6) {
mexErrMsgTxt("6 inputs are required.");
} else if(nlhs!=1) {
mexErrMsgTxt("One output required");
}
// Get the sizes of the image
Iin_dims = mxGetDimensions(prhs[0]);
// Get the sizes of the position vectors
x1_dims = mxGetDimensions(prhs[1]);
/* Assign pointers to input. */
Iin=mxGetPr(prhs[0]);
x1=mxGetPr(prhs[1]);
y1=mxGetPr(prhs[2]);
x2=mxGetPr(prhs[3]);
y2=mxGetPr(prhs[4]);
steps=mxGetPr(prhs[5]);
Iout_dims[0]=(int)steps[0];
Iout_dims[1]=(int)x1_dims[0];
// Make output array
if(mxGetNumberOfDimensions(prhs[0])>2) {
Iout_dims[2]=Iin_dims[2];
}
plhs[0] = mxCreateNumericArray(3, Iout_dims, mxDOUBLE_CLASS, mxREAL);
/* Assign pointer to output. */
Iout = mxGetPr(plhs[0]);
// Loop through all positions
for (i=0; i<(int)x1_dims[0]; i++) {
for (j=0; j<(int)steps[0]; j++) {
perc=((double)j)/steps[0];
Tlocalx=x1[i]*(1-perc)+x2[i]*perc;
Tlocaly=y1[i]*(1-perc)+y2[i]*perc;
// Determine of the zero neighbor
fTlocalx = floor(Tlocalx); fTlocaly = floor(Tlocaly);
xBas0=(int) fTlocalx; yBas0=(int) fTlocaly;
// Determine the location in between the pixels 0..1
tx=Tlocalx-fTlocalx; ty=Tlocaly-fTlocaly;
// Determine the t vectors
vector_tx[0]= 0.5; vector_tx[1]= 0.5*tx; vector_tx[2]= 0.5*pow2(tx); vector_tx[3]= 0.5*pow3(tx);
vector_ty[0]= 0.5; vector_ty[1]= 0.5*ty; vector_ty[2]= 0.5*pow2(ty); vector_ty[3]= 0.5*pow3(ty);
// t vector multiplied with 4x4 bicubic kernel gives the to q vectors
vector_qx[0]= -1.0*vector_tx[1]+2.0*vector_tx[2]-1.0*vector_tx[3];
vector_qx[1]= 2.0*vector_tx[0]-5.0*vector_tx[2]+3.0*vector_tx[3];
vector_qx[2]= 1.0*vector_tx[1]+4.0*vector_tx[2]-3.0*vector_tx[3];
vector_qx[3]= -1.0*vector_tx[2]+1.0*vector_tx[3];
vector_qy[0]= -1.0*vector_ty[1]+2.0*vector_ty[2]-1.0*vector_ty[3];
vector_qy[1]= 2.0*vector_ty[0]-5.0*vector_ty[2]+3.0*vector_ty[3];
vector_qy[2]= 1.0*vector_ty[1]+4.0*vector_ty[2]-3.0*vector_ty[3];
vector_qy[3]= -1.0*vector_ty[2]+1.0*vector_ty[3];
// Determine 1D neighbour coordinates
xn[0]=xBas0-1; xn[1]=xBas0; xn[2]=xBas0+1; xn[3]=xBas0+2;
yn[0]=yBas0-1; yn[1]=yBas0; yn[2]=yBas0+1; yn[3]=yBas0+2;
// Clamp to image boundary if outside image
if(xn[0]<0) { xn[0]=0;if(xn[1]<0) { xn[1]=0;if(xn[2]<0) { xn[2]=0; if(xn[3]<0) { xn[3]=0; }}}}
if(yn[0]<0) { yn[0]=0;if(yn[1]<0) { yn[1]=0;if(yn[2]<0) { yn[2]=0; if(yn[3]<0) { yn[3]=0; }}}}
b=Iin_dims[0]-1;
if(xn[3]>b) { xn[3]=b;if(xn[2]>b) { xn[2]=b;if(xn[1]>b) { xn[1]=b; if(xn[0]>b) { xn[0]=b; }}}}
b=Iin_dims[1]-1;
if(yn[3]>b) { yn[3]=b;if(yn[2]>b) { yn[2]=b;if(yn[1]>b) { yn[1]=b; if(yn[0]>b) { yn[0]=b; }}}}
// First do interpolation in the x direction followed by interpolation in the y direction
index=mindex2(j, i, Iout_dims[0], Iout_dims[1]);
for (k=0; k<Iout_dims[2]; k++) {
indexoff1=k*Iout_dims[0]*Iout_dims[1];
indexoff2=k*Iin_dims[0]*Iin_dims[1];
Iout[index+indexoff1]=0;
for(l=0; l<4; l++) {
vector_b[l] =vector_qx[0]*Iin[xn[0]+yn[l]*Iin_dims[0]+indexoff2];
vector_b[l]+=vector_qx[1]*Iin[xn[1]+yn[l]*Iin_dims[0]+indexoff2];
vector_b[l]+=vector_qx[2]*Iin[xn[2]+yn[l]*Iin_dims[0]+indexoff2];
vector_b[l]+=vector_qx[3]*Iin[xn[3]+yn[l]*Iin_dims[0]+indexoff2];
Iout[index+indexoff1]+= vector_qy[l]*vector_b[l];
}
}
}
}
}
/* The matlab mex function */
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[] )
{
double *Fx, *Fy, *Bx, *By, *H, *Num;
/* Size of input transformation fields */
mwSize Bsizex, Bsizey;
const mwSize *Bdims;
/* Size of the input kernel */
mwSize Hsizex, Hsizey;
const mwSize *Hdims;
int hx_center, hy_center;
/* Variables to store 1D index */
int index;
/* Variable to store vector */
double valx, valy;
/* Loop variable */
int i, t, iHx, iHy;
/* Linear interpolation variables */
int xBas0, xBas1,yBas0,yBas1;
double perc[4], perct;
double xCom, yCom;
double Tlocalx, Tlocaly;
/* X,Y coordinates of current pixel */
int x,y, tx, ty;
/* Check for proper number of arguments. */
if(nrhs!=3) {
mexErrMsgTxt("Three inputs are required.");
} else if(nlhs!=2) {
mexErrMsgTxt("Two outputs are required");
}
/* Assign pointers to each input. */
Bx=mxGetPr(prhs[0]);
By=mxGetPr(prhs[1]);
H=mxGetPr(prhs[2]);
/* Get the sizes of the kernel */
Hdims = mxGetDimensions(prhs[2]);
Hsizex = Hdims[0]; Hsizey = Hdims[1];
/* Get the sizes of the input transformation fields */
Bdims = mxGetDimensions(prhs[0]);
Bsizex = Bdims[0]; Bsizey = Bdims[1];
/* Create array to count number of kernels added */
Num = (double *)malloc(Bsizex*Bsizey*sizeof(double));
for (i=0; i<(Bsizex*Bsizey); i++){ Num[i] = 0;}
/* Create output array */
plhs[0] = mxCreateNumericArray(2, Bdims, mxDOUBLE_CLASS, mxREAL);
plhs[1] = mxCreateNumericArray(2, Bdims, mxDOUBLE_CLASS, mxREAL);
/* Assign pointer to output. */
Fx = mxGetPr(plhs[0]);
Fy = mxGetPr(plhs[1]);
/* Gaussian kernel center */
hx_center=(int)-floor((double)Hsizex/2);
hy_center=(int)-floor((double)Hsizey/2);
/* Loop through all image pixel coordinates */
for (y=0; y<Bsizey; y++)
{
for (x=0; x<Bsizex; x++)
{
valx=-Bx[mindex2(x,y,Bsizex)];
valy=-By[mindex2(x,y,Bsizex)];
Tlocalx =x-valx;
Tlocaly =y-valy;
/* Determine the coordinates of the pixel(s) which will be come the current pixel */
/* (using linear interpolation) */
xBas0=(int) floor(Tlocalx);
yBas0=(int) floor(Tlocaly);
xBas1=xBas0+1;
yBas1=yBas0+1;
/* Linear interpolation constants (percentages) */
xCom=Tlocalx-floor(Tlocalx); yCom=Tlocaly-floor(Tlocaly);
perc[0]=(1-xCom) * (1-yCom);
perc[1]=(1-xCom) * yCom;
perc[2]=xCom * (1-yCom);
perc[3]=xCom * yCom;
/* Loop through the whole kernel */
for (iHy=0; iHy<Hsizey; iHy++)
{
for (iHx=0; iHx<Hsizex; iHx++)
{
/* Process all 4 neighbors */
for(t=0; t<4; t++)
{
if(t==0){
tx=xBas0+iHx+hx_center; ty=yBas0+iHy+hy_center;
perct=perc[0]*H[mindex2(iHx,iHy,Hsizex)];
}
else if(t==1){
tx=xBas0+iHx+hx_center; ty=yBas1+iHy+hy_center;
perct=perc[1]*H[mindex2(iHx,iHy,Hsizex)];
}
else if(t==2){
tx=xBas1+iHx+hx_center; ty=yBas0+iHy+hy_center;
perct=perc[2]*H[mindex2(iHx,iHy,Hsizex)];
}
else{
tx=xBas1+iHx+hx_center; ty=yBas1+iHy+hy_center;
perct=perc[3]*H[mindex2(iHx,iHy,Hsizex)];
}
if((tx>=0)&&(ty>=0)&&(tx<Bsizex)&&(ty<Bsizey))
{
index=mindex2(tx,ty,Bsizex);
Fx[index]+=valx*perct;
Fy[index]+=valy*perct;
Num[index]=Num[index]+perct;
}
}
}
}
}
}
for (y=0; y<Bsizey; y++)
{
for (x=0; x<Bsizex; x++)
{
index=mindex2(x,y,Bsizex);
Fx[index]/=(Num[index]+0.00000001);
Fy[index]/=(Num[index]+0.00000001);
}
}
free(Num);
}