gaussian_conv_deriche.c

Go to the documentation of this file.

#include "gaussian_conv_deriche.h"
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "filter_util.h"
#include "complex_arith.h"
#include "gaussian_short_conv.h"

#ifndef M_SQRT2PI

#define M_SQRT2PI   2.50662827463100050241576528481104525
#endif

static void make_filter(num *result_b, num *result_a,
    const complex *alpha, const complex *beta, int K, double sigma);

void deriche_precomp(deriche_coeffs *c, double sigma, int K, num tol)
{
    /* Deriche's optimized filter parameters. */
    static const complex alpha[DERICHE_MAX_K - DERICHE_MIN_K + 1][4] = {
        {{0.48145, 0.971}, {0.48145, -0.971}},
        {{-0.44645, 0.5105}, {-0.44645, -0.5105}, {1.898, 0}},
        {{0.84, 1.8675}, {0.84, -1.8675},
            {-0.34015, -0.1299}, {-0.34015, 0.1299}}
        };
    static const complex lambda[DERICHE_MAX_K - DERICHE_MIN_K + 1][4] = {
        {{1.26, 0.8448}, {1.26, -0.8448}},
        {{1.512, 1.475}, {1.512, -1.475}, {1.556, 0}},
        {{1.783, 0.6318}, {1.783, -0.6318},
            {1.723, 1.997}, {1.723, -1.997}}
        };
    complex beta[DERICHE_MAX_K];
    
    int k;
    double accum, accum_denom = 1.0;
    
    assert(c && sigma > 0 && DERICHE_VALID_K(K) && 0 < tol && tol < 1);
    
    for (k = 0; k < K; ++k)
    {
        double temp = exp(-lambda[K - DERICHE_MIN_K][k].real / sigma);
        beta[k] = make_complex(
            -temp * cos(lambda[K - DERICHE_MIN_K][k].imag / sigma),
            temp * sin(lambda[K - DERICHE_MIN_K][k].imag / sigma));
    }
    
    /* Compute the causal filter coefficients */
    make_filter(c->b_causal, c->a, alpha[K - DERICHE_MIN_K], beta, K, sigma);
    
    /* Numerator coefficients of the anticausal filter */
    c->b_anticausal[0] = (num)(0.0);
    
    for (k = 1; k < K; ++k)
        c->b_anticausal[k] = c->b_causal[k] - c->a[k] * c->b_causal[0];
    
    c->b_anticausal[K] = -c->a[K] * c->b_causal[0];
    
    /* Impulse response sums */
    for (k = 1; k <= K; ++k)
        accum_denom += c->a[k];
    
    for (k = 0, accum = 0.0; k < K; ++k)
        accum += c->b_causal[k];
    
    c->sum_causal = (num)(accum / accum_denom);
    
    for (k = 1, accum = 0.0; k <= K; ++k)
        accum += c->b_anticausal[k];
    
    c->sum_anticausal = (num)(accum / accum_denom);
    
    c->sigma = (num)sigma;
    c->K = K;
    c->tol = tol;
    c->max_iter = (num)ceil(10 * sigma);
    return;
}

static void make_filter(num *result_b, num *result_a,
    const complex *alpha, const complex *beta, int K, double sigma)
{
    const double denom = sigma * M_SQRT2PI;
    complex b[DERICHE_MAX_K], a[DERICHE_MAX_K + 1];
    int k, j;
        
    b[0] = alpha[0];    /* Initialize b/a = alpha[0] / (1 + beta[0] z^-1) */
    a[0] = make_complex(1, 0);
    a[1] = beta[0];
    
    for (k = 1; k < K; ++k)
    {   /* Add kth term, b/a += alpha[k] / (1 + beta[k] z^-1) */
        b[k] = c_mul(beta[k], b[k-1]);
        
        for (j = k - 1; j > 0; --j)
            b[j] = c_add(b[j], c_mul(beta[k], b[j - 1]));
        
        for (j = 0; j <= k; ++j)
            b[j] = c_add(b[j], c_mul(alpha[k], a[j]));
        
        a[k + 1] = c_mul(beta[k], a[k]);
        
        for (j = k; j > 0; --j)
            a[j] = c_add(a[j], c_mul(beta[k], a[j - 1]));
    }
    
    for (k = 0; k < K; ++k)
    {
        result_b[k] = (num)(b[k].real / denom);
        result_a[k + 1] = (num)a[k + 1].real;
    }
    
    return;
}

void deriche_gaussian_conv(deriche_coeffs c,
    num *dest, num *buffer, const num *src, long N, long stride)
{
    const long stride_2 = stride * 2;
    const long stride_3 = stride * 3;
    const long stride_4 = stride * 4;
    const long stride_N = stride * N;
    num *y_causal, *y_anticausal;
    long i, n;
    
    assert(dest && buffer && src && buffer != src && N > 0 && stride != 0);
    
    if (N <= 4)
    {   /* Special case for very short signals. */
        gaussian_short_conv(dest, src, N, stride, c.sigma);
        return;
    }
    
    /* Divide buffer into two buffers each of length N. */
    y_causal = buffer;
    y_anticausal = buffer + N;
    
    /* Initialize the causal filter on the left boundary. */
    init_recursive_filter(y_causal, src, N, stride,
        c.b_causal, c.K - 1, c.a, c.K, c.sum_causal, c.tol, c.max_iter);
    
    /* The following filters the interior samples according to the filter
       order c.K. The loops below implement the pseudocode
       
       For n = K, ..., N - 1,
           y^+(n) = \sum_{k=0}^{K-1} b^+_k src(n - k)
                    - \sum_{k=1}^K a_k y^+(n - k)
       
       Variable i tracks the offset to the nth sample of src, it is
       updated together with n such that i = stride * n. */
    switch (c.K)
    {
    case 2:
        for (n = 2, i = stride_2; n < N; ++n, i += stride)
            y_causal[n] = c.b_causal[0] * src[i]
                + c.b_causal[1] * src[i - stride]
                - c.a[1] * y_causal[n - 1]
                - c.a[2] * y_causal[n - 2];
        break;
    case 3:
        for (n = 3, i = stride_3; n < N; ++n, i += stride)
            y_causal[n] = c.b_causal[0] * src[i]
                + c.b_causal[1] * src[i - stride]
                + c.b_causal[2] * src[i - stride_2]
                - c.a[1] * y_causal[n - 1]
                - c.a[2] * y_causal[n - 2]
                - c.a[3] * y_causal[n - 3];
        break;
    case 4:
        for (n = 4, i = stride_4; n < N; ++n, i += stride)
            y_causal[n] = c.b_causal[0] * src[i]
                + c.b_causal[1] * src[i - stride]
                + c.b_causal[2] * src[i - stride_2]
                + c.b_causal[3] * src[i - stride_3]
                - c.a[1] * y_causal[n - 1]
                - c.a[2] * y_causal[n - 2]
                - c.a[3] * y_causal[n - 3]
                - c.a[4] * y_causal[n - 4];
        break;
    }
    
    /* Initialize the anticausal filter on the right boundary. */
    init_recursive_filter(y_anticausal, src + stride_N - stride, N, -stride,
        c.b_anticausal, c.K, c.a, c.K, c.sum_anticausal, c.tol, c.max_iter);
    
    /* Similar to the causal filter code above, the following implements
       the pseudocode
       
       For n = K, ..., N - 1,
           y^-(n) = \sum_{k=1}^K b^-_k src(N - n - 1 - k)
                    - \sum_{k=1}^K a_k y^-(n - k)
     
       Variable i is updated such that i = stride * (N - n - 1). */
    switch (c.K)
    {
    case 2:
        for (n = 2, i = stride_N - stride_3; n < N; ++n, i -= stride)
            y_anticausal[n] = c.b_anticausal[1] * src[i + stride]
                + c.b_anticausal[2] * src[i + stride_2]
                - c.a[1] * y_anticausal[n - 1]
                - c.a[2] * y_anticausal[n - 2];
        break;
    case 3:
        for (n = 3, i = stride_N - stride_4; n < N; ++n, i -= stride)
            y_anticausal[n] = c.b_anticausal[1] * src[i + stride]
                + c.b_anticausal[2] * src[i + stride_2]
                + c.b_anticausal[3] * src[i + stride_3]
                - c.a[1] * y_anticausal[n - 1]
                - c.a[2] * y_anticausal[n - 2]
                - c.a[3] * y_anticausal[n - 3];
        break;
    case 4:
        for (n = 4, i = stride_N - stride * 5; n < N; ++n, i -= stride)
            y_anticausal[n] = c.b_anticausal[1] * src[i + stride]
                + c.b_anticausal[2] * src[i + stride_2]
                + c.b_anticausal[3] * src[i + stride_3]
                + c.b_anticausal[4] * src[i + stride_4]
                - c.a[1] * y_anticausal[n - 1]
                - c.a[2] * y_anticausal[n - 2]
                - c.a[3] * y_anticausal[n - 3]
                - c.a[4] * y_anticausal[n - 4];
        break;
    }
    
    /* Sum the causal and anticausal responses to obtain the final result. */
    for (n = 0, i = 0; n < N; ++n, i += stride)
        dest[i] = y_causal[n] + y_anticausal[N - n - 1];
    
    return;
}

void deriche_gaussian_conv_image(deriche_coeffs c,
    num *dest, num *buffer, const num *src,
    int width, int height, int num_channels)
{
    long num_pixels = ((long)width) * ((long)height);
    int x, y, channel;
    
    assert(dest && buffer && src && num_pixels > 0);
    
    /* Loop over the image channels. */
    for (channel = 0; channel < num_channels; ++channel)
    {
        num *dest_y = dest;
        const num *src_y = src;
        
        /* Filter each row of the channel. */
        for (y = 0; y < height; ++y)
        {
            deriche_gaussian_conv(c,
                dest_y, buffer, src_y, width, 1);
            dest_y += width;
            src_y += width;
        }
        
        /* Filter each column of the channel. */
        for (x = 0; x < width; ++x)
            deriche_gaussian_conv(c,
                dest + x, buffer, dest + x, height, width);
        
        dest += num_pixels;
        src += num_pixels;
    }
    
    return;
}