ordfilt2.c

来自「有关matlab的电子书籍有一定的帮助希望有用」· C语言 代码 · 共 680 行 · 第 1/2 页

/* Copyright 1993-1998 The MathWorks, Inc.  All Rights Reserved. */

/* $Revision: 5.13 $  $Date: 1997/11/24 16:14:43 $  $Author: eddins $ */

static char rcsid[] = "$Id: ordfilt2.c,v 5.13 1997/11/24 16:14:43 eddins Exp $";

/*

   ordfilt2.c  .MEX file
               Implements 2-D order-statistic filtering with 
               arbitrary domain and optional additive offsets.

               Y = ORDFILT2(X, ORDER, DOMAIN) replaces each
               element in X by the ORDER-th element in the sorted set
               of neighbors specified by the nonzero elements
               in DOMAIN.

               For example, Y=ORDFILT2(X,5,ones(3,3)) implements
               a 3-by-3 median filter.  
               Y=ORDFILT2(X,1,[0 1 0; 1 0 1; 0 1 0]) replaces each
               element in X by the minimum of its north, east, south,
               and west neighbors.

               Y = ORDFILT2(X,ORDER,DOMAIN,S), where S must be the
               same size as DOMAIN, uses the values of S corresponding
               to the nonzero values of DOMAIN as additive offsets.

               X must be a real, double or uint8, 2-D, numeric array.

               Steven L. Eddins 12-8-94
               Revised March 1996

*/

#include <math.h>
#include "mex.h"

double **Make2SubscriptMatrix(double *array, int rowDim, int colDim) {
    double **matrix;
    int i;

    matrix = (double **) mxCalloc(colDim, sizeof(double *));

    for (i = 0; i < colDim; i++) {
        matrix[i] = array + i*rowDim;
    }

    return(matrix);
}

uint8_T **Make2SubscriptMatrix8(uint8_T *array, int rowDim, int colDim) {
    uint8_T **matrix;
    int i;

    matrix = (uint8_T **) mxCalloc(colDim, sizeof(uint8_T *));

    for (i = 0; i < colDim; i++) {
        matrix[i] = array + i*rowDim;
    }

    return(matrix);
}

mxArray *ConvertToDouble(const mxArray *A)
{
    int nrhs = 1;
    int nlhs = 1;
    mxArray *prhs[1];
    mxArray *plhs[1];
    mxArray *result;
    int errorFlag;

    if (A == NULL)
    {
        mexErrMsgTxt("NULL array passed to ConvertToDouble()");
    }

    prhs[0] = (mxArray *) A;
    errorFlag = mexCallMATLAB(nlhs, plhs, nrhs, prhs, "double");
    if (errorFlag == 0) {
        /* Successful operation */
        result = plhs[0];
    } else {
        /* double conversion failed */
        /* We should never get here; error should have long-jumped */
        /* back to the command prompt */
        mexErrMsgTxt("Conversion to double failed");
    }

    return(result);

}

/*
 * input array must be double 
 */
int NumNonzeros(const mxArray *A) {
    int result = 0;
    int lengthA;
    int i;
    double *pr;
    double *pi;

    if (A == NULL)
    {
        mexErrMsgTxt("NULL array passed to NumNonzeros()");
    }
    if (! mxIsDouble(A))
    {
        mexErrMsgTxt("Nondouble array passed to NumNonzeros()");
    }

    lengthA = mxGetNumberOfElements(A);
    if (mxIsComplex(A)) {
        pr = mxGetPr(A);
        pi = mxGetPi(A);
        for (i = 0; i < lengthA; i++) {
            if ((*pr != 0.0) || (*pi != 0.0)) {
                result++;
            }
            pr++;
            pi++;
        }
    } else {
        pr = mxGetPr(A);
        for (i = 0; i < lengthA; i++) {
            if (*pr != 0.0) {
                result++;
            }
            pr++;
        }
    }

    return(result);
}

/* Selection algorithm using QuickSort-style partitioning. */
/* Based on discussion in Sedgewick, Algorithms, 2d ed, 1988, pp. 115-130 */
/* This function modifies the values in a[].  */
static double order_select(double *a, int N, int order)
{
    int i;
    int j;
    int left;
    int right;
    int mid;
    double key;
    double tmp;

    mxAssert(order >= 1, "");
    mxAssert(order <= N, "");
    
    /* Compensate for the fact that the input array is zero-based, */
    /* while order is one-based. */
    order--;
    
    /* The initial partition is the entire array. */
    left = 0;
    right = N - 1;

    /* Perform the scanning only if the current partition has at least */
    /* three elements. */
    while (right > (left+1)) 
    {
        /* Use median-of-three method to select the partitioning key. */
        /* This method makes worst-case behavior much less likely and */
        /* avoids the needs for sentinel values outside the array. */

        /* First we sort the first, middle, and last element of the */
        /* partition. */
        mid = (left + right) >> 1;
        if (a[left] > a[mid])
        {
            tmp = a[left];
            a[left] = a[mid];
            a[mid] = tmp;
        }
        if (a[left] > a[right])
        {
            tmp = a[left];
            a[left] = a[right];
            a[right] = tmp;
        }
        if (a[mid] > a[right])
        {
            tmp = a[mid];
            a[mid] = a[right];
            a[right] = tmp;
        }
        
        /* Now swap the middle value with the next-to-last value. */
        tmp = a[mid];
        a[mid] = a[right - 1];
        a[right - 1] = tmp;
        
        /* Use the median of the three values as the partitioning key */
        key = a[right - 1];
        
        /* Start the partitioning scan.  Note that because we sorted */
        /* the left and right values, we can start the comparisons */
        /* with (left+1) and (right-2).  This is how we avoid the */
        /* need for sentinel values. */
        i = left;
        j = right - 1;
        for (;;)
        {
            while (a[++i] < key) ;      mxAssert(i <= (N-1), "");
            while (a[--j] > key) ;      mxAssert(j >= 0, "");
            if (i >= j) break;     /* pointers crossed; end scan */
            /* swap values at end of current interval */
            tmp = a[i];
            a[i] = a[j];
            a[j] = tmp;
        }
        /* One last swap needed at end of scan */
        tmp = a[i];
        a[i] = a[right-1];
        a[right-1] = tmp;

        /* Select the left or right subpartition depending on */
        /* the value of order. */
        if (i >= order) right = i-1;
        if (i <= order) left = i+1;
    }

    if ((right - left) == 1)
    {
        /* Last partition has two elements that may not be sorted. */
        if (a[left] > a[right])
        {
            tmp = a[left];
            a[left] = a[right];
            a[right] = tmp;
        }
    }
            
    return(a[order]);
}

static void
ValidateInputs(int nlhs, int nrhs, const mxArray *prhs[], const mxArray **X, 
               const mxArray **Order, const mxArray **Domain,
               const mxArray **S) 
{
    int i;

    /* The number of input arguments must be between 3 and 4 */
    if (nrhs < 3) {
        mexErrMsgTxt("Three input arguments are required");
    }
    if (nrhs > 4) {
        mexErrMsgTxt("Too many input arguments");
    }

    /* ORDFILT2 has only one output argument */
    if (nlhs > 1) {
        mexErrMsgTxt("Too many output arguments");
    }

    /* All inputs must be numeric */
    for (i = 0; i < nrhs; i++) {
        if (!mxIsNumeric(prhs[i])) {
            mexErrMsgTxt("Inputs must be numeric");
        }
    }

    /* All inputs must be real */
    for (i = 0; i < nrhs; i++) {
        if (mxIsComplex(prhs[i])) {
            mexErrMsgTxt("Inputs must be real");
        }
    }

    /* First input must be double or uint8 */
    if (!mxIsDouble(prhs[0]) && !mxIsUint8(prhs[0])) {
        mexErrMsgTxt("X must be 'double' or 'uint8' class");
    }

    *X = prhs[0];
    *Order = prhs[1];
    *Domain = prhs[2];
    if (nrhs == 4) {
        *S = prhs[3];
    } else {
        *S = NULL;
    }

    /* Validate domain */
    if (mxIsEmpty(*Domain)) {
        mexErrMsgTxt("Domain cannot be empty");
    }

}

int GetOrder(const mxArray *Order, int domainSize)
{
    int result;
    double tmp;

    /* Validate Order */
    if ((mxGetM(Order) * mxGetN(Order)) > 1) {
        mexWarnMsgTxt("ORDER should be a scalar");
    }
    tmp = mxGetScalar(Order);
    result = (int) tmp;
    if ((double) result != tmp) {
        mexWarnMsgTxt("ORDER should be an integer");
    }
    if (result < 1) {
        mexErrMsgTxt("Order must be at least 1");
    }
    if (result > domainSize) {
        mexErrMsgTxt("ORDER must no bigger than the number "
                     "of nonzero domain elements");
    }

    return(result);
}

double **MakeZeroPaddedInput(double **xx, int mx, int nx, int md, int nd) {
    double **aa;
    int maa;
    int naa;
    int rowOrigin;
    int colOrigin;
    int row;
    int col;

    maa = mx + md - 1;
    naa = nx + nd - 1;

    rowOrigin = (md - 1) / 2;
    colOrigin = (nd - 1) / 2;

    aa = Make2SubscriptMatrix((double *) mxCalloc(maa*naa, sizeof(double)),
                              maa, naa);

    for (col = 0; col < nx; col++) {
        for (row = 0; row < mx; row++) {
             aa[col+colOrigin][row+rowOrigin] = xx[col][row];