tpswarp_version_1_3.c

matlab aamtool box

    get_w(Q, w, Nvert);
    get_a(Q, a, Nvert);
    mxFree(P);
    mxFree(col);
    mxFree(K);
    mxFree(Q);
    mxFree(invL);
    mxFree(L);
    mxFree(Y);    
}
void get_w(float **A, float **w, int N){
    int i;
    for(i=0; i<N; i++){
        w[i][0] = A[i][0];
        w[i][1] = A[i][1];
    }
}
void get_a(float **A, float **a, int N){
    int i;
    for(i=N; i<N+3; i++){
        a[i-N][0] = A[i][0];
        a[i-N][1] = A[i][1];
    }
}
void Y_mat(float *ipts, float **Y, int Nvert){
    int i, ii;
    for(i=0; i<Nvert; i++){
        for(ii=0; ii<2; ii++){
            Y[i][ii] = ipts[i*2 + ii];
        }
    }
    for(i=Nvert; i<Nvert+3; i++){
        for(ii=0; ii<2; ii++){
            Y[i][ii] = 0;
        }
    }
}
void L_mat(float *K, float *P, float **L, int Nvert){
    int i, ii;
    for(i=0; i<Nvert; i++){
        for(ii=0; ii<Nvert; ii++){
            L[i][ii] = K[i*Nvert + ii];
            
        }
        
    }
    for(ii=Nvert; ii<Nvert+3; ii++){
        for(i=0; i<Nvert; i++){
            L[i][ii] = P[i + Nvert*(ii-Nvert)];
        }
    }
    for(i=Nvert; i<Nvert+3; i++){
        for(ii=0; ii<Nvert; ii++){
            L[i][ii] = P[ii + Nvert*(i-Nvert)];
            
        }
        
    }
    for(i=Nvert; i<Nvert+3; i++){
        for(ii=Nvert; ii<Nvert+3; ii++){
            L[i][ii] = 0;
            
        }
        
    }
}
void P_mat(float *ipts, float *P, int Nvert){
    int i;
    
    for(i=0; i<Nvert; i++){
        P[i] = 1;
    }
    for(i=0; i<Nvert; i++){
        P[i+Nvert] = ipts[2*i];
    }
    for(i=0; i<Nvert; i++){
        P[i+2*Nvert] = ipts[2*i+1];
    }
}

void print_mat(float *mat, int M, int N){
    int i, ii, indx;
    float x;
    mexPrintf("Size: %dx%d\n", M, N);
    for(i=0; i<M; i++){
        mexPrintf("\n%d) ", i);
        for(ii=0; ii<N; ii++){
            indx = i*N + ii;
            x = mat[indx];
            mexPrintf("%f ", x);
        }
    }
    mexPrintf("\n");
    
}

void K_mat(float *opts, int Nvert, float *K, float *regterm){
    float *r;
		int i;
    r = mxCalloc(Nvert*Nvert, sizeof(float));
    matdistance(opts, opts, Nvert, Nvert, r);
    U_rbs(r, Nvert, Nvert, K);
		for(i=0; i<(Nvert*Nvert); i++){
			K[i] = K[i]*(*regterm);
		}
		
}


void matdistance(float *opts, float *ipts, int Nverto, int Nverti, float *r){
    int i, ii;
    for(i=0; i<Nverto; i++){
        for(ii=0; ii<Nverti; ii++){
            r[ii+Nverti*i] = (float)sqrt((opts[2*i] - ipts[2*ii])*(opts[2*i] - ipts[2*ii]) + (opts[2*i+1] - ipts[2*ii+1])*(opts[2*i+1] - ipts[2*ii+1]));
        }
    }
}

void U_rbs(float *r, int M, int N, float *Ur){
    int i;
    for(i=0; i<M*N; i++){
        Ur[i] = r[i]*r[i];
        if(Ur[i] == 0)
            Ur[i] = 1;
        Ur[i] = Ur[i]*(float)log(Ur[i]);
    }
}

void init_image(mxArray *img){
    int i, width, height, ndim;
    float *I;
    const int *dim;
    
    ndim = mxGetNumberOfDimensions(img);
    dim = mxGetDimensions(img);
    
    width = dim[0];
    height = dim[1];
    I = (float*)mxGetData(img);
    for (i=0; i<(width*height); i++){
        I[i] = 0;
    }
    mxSetData(img, I);
}



float *GetInputImage(const mxArray *img, int *width, int *height)
{
    int	ndim, nx, ny;
    const int *dim;
    
    if(!mxIsNumeric(img) || mxIsEmpty(img) || !mxIsSingle(img))
        mexErrMsgTxt("Input image must be float 2D or 3D numeric matrix");
    
    ndim = mxGetNumberOfDimensions(img);
    if(ndim != 3)
        mexErrMsgTxt("Input image must be (nx * ny * 3) 24-bit RGB");
    
    dim = mxGetDimensions(img);
    *width = nx = dim[0];
    *height = ny = dim[1];
    
    if(dim[2] > 3)
        mexWarnMsgTxt("Only using first 3 values of 3rd dimension");
    else if(dim[2] < 3)
        mexErrMsgTxt("Third dimension must have size = 3");
    
    return((float*)mxGetData(img));
}

float **dmatrix(long nrow, long ncol){
    
    long i;
    float **m;
    
    m = (float **)mxMalloc((size_t)(nrow*sizeof(float*)));
    if (!m)
        mexPrintf("error: could not make matrix\n");
    m[0] = (float *)mxMalloc((size_t)((nrow*ncol)*sizeof(float)));
    if (!m[0])
        mexPrintf("error: could not make matrix\n");
    for(i=1; i<nrow; i++)
        m[i] = m[i-1] + ncol;
    return m;
}
void _minverse(float **m, float **invm, int *indx, int N, float *col){
    int i, j;
    for(j=0; j<N; j++){
        for (i=0; i<N; i++){
            col[i] = 0.0;
        }
        col[j] = 1.0;
        lubksb(m, N, indx, col);
        for (i=0; i<N; i++){
            invm[i][j] = col[i];
        }
    }
}
void lubksb(float **a, int n, int *indx, float b[])
{
    int i,ii=0,ip,j;
    float sum;
    for (i=0;i<n;i++) {
        ip=indx[i];
        sum=b[ip];
        b[ip]=b[i];
        if (ii)
            for (j=ii-1;j<=i-1;j++) sum -= a[i][j]*b[j];
        else if (sum) ii=i+1;
        b[i]=sum;
    }
    for (i=n-1;i>=0;i--) {
        sum=b[i];
        for (j=i+1;j<n;j++) sum -= a[i][j]*b[j];
        b[i]=sum/a[i][i];
    }
}

void ludcmp(float **a, int n, int *indx, float *d)
{
    int i,imax,j,k;
    float big,dum,sum,temp;
    float *vv;
    
    vv = (float *)mxMalloc(n*sizeof(float));
    
    //vv=vector(0,n-1);
    (*d)=1.0;
    for (i=0;i<n;i++) {
        big=0.0;
        for (j=0;j<n;j++)
            if ((temp=(float)fabs(a[i][j])) > big) big=temp;
        if (big == 0.0){
            //printf("WARNING: Singular matrix in routine ludcmp!!\n");
            *d = 0;
            return;
        }
        vv[i]=(float)1.0/big;
    }
    for (j=0;j<n;j++) {
        for (i=0;i<j;i++) {
            sum=a[i][j];
            for (k=0;k<i;k++) sum -= a[i][k]*a[k][j];
            a[i][j]=sum;
        }
        big=0.0;
        for (i=j;i<n;i++) {
            sum=a[i][j];
            for (k=0;k<j;k++) sum -= a[i][k]*a[k][j];
            a[i][j]=sum;
            if ( (dum=vv[i]*(float)fabs(sum)) >= big) {
                big=dum;
                imax=i;
            }
        }
        if (j != imax) {
            for (k=0;k<n;k++) {
                dum=a[imax][k];
                a[imax][k]=a[j][k];
                a[j][k]=dum;
            }
            *d = -(*d);
            vv[imax]=vv[j];
        }
        indx[j]=imax;
        if (a[j][j] == 0.0) a[j][j]=(float)TINY;
        if (j != n-1) {
            dum=(float)1.0/(a[j][j]);
            for (i=j+1;i<n;i++) a[i][j] *= dum;
        }
    }
    //free_vector(vv,0,n-1);
}
void _mmult(float **mat1, float **mat2, float **z, int l, int m, int n) {
    int i, ii, iii;
    for(i=0; i<l; i++){
        for(ii=0; ii<n; ii++){
            z[i][ii] = 0;
            for(iii=0; iii<m; iii++){
                z[i][ii] += mat1[i][iii]*mat2[iii][ii];
            }
        }
    }
}