C++ 实现matlab中值滤波函数medfilt2

void MedianFilt(std::vector src, std::vector dst, MatrixParam& param, int kernel_height, int kernel_width)
{
    // 将一维数组转换为二维数组
    int** src_2d = new int* [param.NRow()];

    for (int i = 0; i < param.NRow(); i++)
    {
        src_2d[i] = new int[param.NCol()];
        for (int j = 0; j < param.NCol(); j++)
        {
            src_2d[i][j] = src[i * param.NCol() + j];
        }
    }

    // 创建中值滤波移动窗口
    int* medianWindow = new int[kernel_height * kernel_width]{ 0 };

    //数据边界复制填充
    int a = param.NRow() + (kernel_height - 1);
    int b = param.NCol() + (kernel_width - 1);
    int** newSrc = new int* [a];
    for (int i = 0; i < a; i++)
    {
        newSrc[i] = new int[b] {0};
    }

    // 复制中间部分数据
    for (int i = ((kernel_height - 1) / 2); i < (a - (kernel_height - 1) / 2); i++)
    {
        for (int j = ((kernel_width - 1) / 2); j < (b - (kernel_width - 1) / 2); j++)
        {
            newSrc[i][j] = src_2d[i - (kernel_height - 1) / 2][j - (kernel_width - 1) / 2];
        }
    }

    // 中值滤波   
    for (int i = ((kernel_height - 1) / 2); i < (a - (kernel_height - 1) / 2); i++)
    {
        for (int j = ((kernel_width - 1) / 2); j < (b - (kernel_width - 1) / 2); j++)
        {
            int k = 0;
            // get the elements of median kernel from window
            for (int ii = (i - ((kernel_height - 1) / 2)); ii < ((i + (kernel_height - 1) / 2) + 1); ii++)
            {
                for (int jj = (j - ((kernel_width - 1) / 2)); jj < ((j + (kernel_width - 1) / 2) + 1); jj++)
                {
                    
                    medianWindow[k] = newSrc[ii][jj];
                    k++;
                }
            }
            // sort elements(only half of them)
            for (int m = 0; m < (kernel_height * kernel_width + 1) / 2; m++)
            {             
                for (int n = m + 1; n < (kernel_height * kernel_width); n++)
                {
                    if (medianWindow[n] < medianWindow[m])
                    {
                        // put found minimum element in its place
                        float temp = medianWindow[m];
                        medianWindow[m] = medianWindow[n];
                        medianWindow[n] = temp;
                    }
                }
            }
            dst[(i - (kernel_height - 1) / 2) * param.NCol() + (j - (kernel_width - 1) / 2)] = medianWindow[(kernel_height * kernel_width - 1) / 2];          
        }
    }

    for (int i = 0; i < param.NRow(); i++)
    {
        delete[] src_2d[i];
        src_2d[i] = NULL;
    }
    delete[] src_2d;
    src_2d = NULL;

    delete[] medianWindow;
    medianWindow = NULL;

    for (int i = 0; i < a; i++)
    {
        delete[] newSrc[i];
        newSrc[i] = NULL;
    }
    delete[] newSrc;
    newSrc = NULL;
}