gaussian_conv_vyv.c

Go to the documentation of this file.

#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include "filter_util.h"
#include "complex_arith.h"
#include "invert_matrix.h"
#include "gaussian_short_conv.h"
#include "gaussian_conv_vyv.h"

#define YVY_NUM_NEWTON_ITERATIONS       6

static double variance(const complex *poles0, int K, double q)
{
    complex sum = {0, 0};
    int k;
    
    for (k = 0; k < K; ++k)
    {
        complex z = c_real_pow(poles0[k], 1/q), denom = z;
        denom.real -= 1;
        /* Compute sum += z / (z - 1)^2. */
        sum = c_add(sum, c_div(z, c_mul(denom, denom)));
    }
    
    return 2 * sum.real;
}

static double dq_variance(const complex *poles0, int K, double q)
{
    complex sum = {0, 0};
    int k;
    
    for (k = 0; k < K; ++k)
    {
        complex z = c_real_pow(poles0[k], 1/q), w = z, denom = z;
        w.real += 1;
        denom.real -= 1;
        /* Compute sum += z log(z) (z + 1) / (z - 1)^3 */
        sum = c_add(sum, c_div(c_mul(c_mul(z, c_log(z)), w),
            c_real_pow(denom, 3)));
    }
    
    return (2 / q) * sum.real;
}

static double compute_q(const complex *poles0, int K,
    double sigma, double q0)
{
    double sigma2 = sigma * sigma;
    double q = q0;
    int i;
    
    for (i = 0; i < YVY_NUM_NEWTON_ITERATIONS; ++i)
        q -= (variance(poles0, K, q) - sigma2)
            / dq_variance(poles0, K, q);
    
    return q;
}

static void expand_pole_product(double *c, const complex *poles, int K)
{
    complex denom[VYV_MAX_K + 1];
    int k, j;
    
    assert(K <= VYV_MAX_K);
    denom[0] = poles[0];
    denom[1] = make_complex(-1, 0);
    
    for (k = 1; k < K; ++k)
    {
        denom[k + 1] = c_neg(denom[k]);
        
        for (j = k; j > 0; --j)
            denom[j] = c_sub(c_mul(denom[j], poles[k]), denom[j - 1]);
        
        denom[0] = c_mul(denom[0], poles[k]);
    }
    
    for (k = 1; k <= K; ++k)
        c[k] = c_div(denom[k], denom[0]).real;
    
    for (c[0] = 1, k = 1; k <= K; ++k)
        c[0] += c[k];
    
    return;
}

void vyv_precomp(vyv_coeffs *c, double sigma, int K, num tol)
{
    /* Optimized unscaled pole locations. */
    static const complex poles0[VYV_MAX_K - VYV_MIN_K + 1][5] = {
        {{1.4165, 1.00829}, {1.4165, -1.00829}, {1.86543, 0}},
        {{1.13228, 1.28114}, {1.13228, -1.28114},
            {1.78534, 0.46763}, {1.78534, -0.46763}},
        {{0.8643, 1.45389}, {0.8643, -1.45389},
            {1.61433, 0.83134}, {1.61433, -0.83134}, {1.87504, 0}}
        };
    complex poles[VYV_MAX_K];
    double q, filter[VYV_MAX_K + 1];
    double A[VYV_MAX_K * VYV_MAX_K], inv_A[VYV_MAX_K * VYV_MAX_K];
    int i, j, matrix_size;
    
    assert(c && sigma > 0 && VYV_VALID_K(K) && tol > 0);
    
    /* Make a crude initial estimate of q. */
    q = sigma / 2;
    /* Compute an accurate value of q using Newton's method. */
    q = compute_q(poles0[K - VYV_MIN_K], K, sigma, q);
    
    for (i = 0; i < K; ++i)
        poles[i] = c_real_pow(poles0[K - VYV_MIN_K][i], 1/q);
    
    /* Compute the filter coefficients b_0, a_1, ..., a_K. */
    expand_pole_product(filter, poles, K);
    
    /* Compute matrix for handling the right boundary. */
    for (i = 0, matrix_size = K * K; i < matrix_size; ++i)
        A[i] = 0.0;
        
    for (i = 0; i < K; ++i)
        for (A[i + K * i] = 1.0, j = 1; j <= K; ++j)
            A[i + K * extension(K, i + j)] += filter[j];
    
    invert_matrix(inv_A, A, K);
    
    for (i = 0; i < matrix_size; ++i)
        inv_A[i] *= filter[0];
    
    /* Store precomputations in coeffs struct. */
    for (i = 0; i <= K; ++i)
        c->filter[i] = filter[i];
    
    for (i = 0; i < matrix_size; ++i)
        c->M[i] = (num)inv_A[i];
    
    c->K = K;
    c->sigma = (num)sigma;
    c->tol = tol;
    c->max_iter = (num)(10 * sigma);
    return;
}

void vyv_gaussian_conv(vyv_coeffs c,
    num *dest, 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_5 = stride * 5;
    const long stride_N = stride * N;
    num q[VYV_MAX_K];
    long i;
    int m, n;
    
    assert(dest && src && N > 0 && stride != 0);
    
    if (N <= 4)
    {   /* Special case for very short signals. */
        gaussian_short_conv(dest, src, N, stride, c.sigma);
        return;
    }

    /* Handle the left boundary. */
    init_recursive_filter(q, src, N, stride,
        c.filter, 0, c.filter, c.K, 1.0f, c.tol, c.max_iter);

    for (m = 0; m < c.K; ++m)
        dest[stride * m] = q[m];
    
    /* The following applies the causal recursive filter according to the
       filter order c.K. The loops implement the pseudocode
     
       For n = K, ..., N - 1,
          dest(n) = filter(0) src(n) - \sum_{k=1}^K dest(n - k)
      
       Variable i = stride * n is the offset to the nth sample. */
    
    switch (c.K)
    {
    case 3:
        for (i = stride_3; i < stride_N; i += stride)
            dest[i] = c.filter[0] * src[i]
                    - c.filter[1] * dest[i - stride]
                    - c.filter[2] * dest[i - stride_2]
                    - c.filter[3] * dest[i - stride_3];
        break;
    case 4:
        for (i = stride_4; i < stride_N; i += stride)
            dest[i] = c.filter[0] * src[i]
                    - c.filter[1] * dest[i - stride]
                    - c.filter[2] * dest[i - stride_2]
                    - c.filter[3] * dest[i - stride_3]
                    - c.filter[4] * dest[i - stride_4];
        break;
    case 5:
        for (i = stride_5; i < stride_N; i += stride)
            dest[i] = c.filter[0] * src[i]
                    - c.filter[1] * dest[i - stride]
                    - c.filter[2] * dest[i - stride_2]
                    - c.filter[3] * dest[i - stride_3]
                    - c.filter[4] * dest[i - stride_4]
                    - c.filter[5] * dest[i - stride_5];
        break;
    }
    
    /* Handle the right boundary by multiplying matrix c.M with last K
       samples dest(N - K - m), m = 0, ..., K - 1. */
    
    /* Copy last K samples into array q, q(m) = dest(N - K + m). */
    for (m = 0; m < c.K; ++m)
        q[m] = dest[stride_N - stride * (c.K - m)];
    
    /* Perform matrix multiplication,
       dest(N - K + m) = \sum_{n=0}^{K-1} M(m, n) q(n). */
    for (m = 0; m < c.K; ++m)
    {
        num accum = (num)0;
        
        for (n = 0; n < c.K; ++n)
            accum += c.M[m + c.K*n] * q[n];
        
        dest[stride_N - stride * (c.K - m)] = accum;
    }
    
    /* The following applies the anticausal filter, implementing the
       pseudocode
       
       For n = N - K - 1, ..., 0,
          dest(n) = filter(0) dest(n) - \sum_{k=1}^K dest(n + k)
      
       Variable i = stride * n is the offset to the nth sample. */
    switch (c.K)
    {
    case 3:
        for (i = stride_N - stride_4; i >= 0; i -= stride)
            dest[i] = c.filter[0] * dest[i]
                    - c.filter[1] * dest[i + stride]
                    - c.filter[2] * dest[i + stride_2]
                    - c.filter[3] * dest[i + stride_3];
        break;
    case 4:
        for (i = stride_N - stride_5; i >= 0; i -= stride)
            dest[i] = c.filter[0] * dest[i]
                    - c.filter[1] * dest[i + stride]
                    - c.filter[2] * dest[i + stride_2]
                    - c.filter[3] * dest[i + stride_3]
                    - c.filter[4] * dest[i + stride_4];
        break;
    case 5:
        for (i = stride_N - stride * 6; i >= 0; i -= stride)
            dest[i] = c.filter[0] * dest[i]
                    - c.filter[1] * dest[i + stride]
                    - c.filter[2] * dest[i + stride_2]
                    - c.filter[3] * dest[i + stride_3]
                    - c.filter[4] * dest[i + stride_4]
                    - c.filter[5] * dest[i + stride_5];
        break;
    }

    return;
}

void vyv_gaussian_conv_image(vyv_coeffs c, num *dest, const num *src,
    int width, int height, int num_channels)
{
    const long num_pixels = ((long)width) * ((long)height);
    int x, y, channel;
    
    assert(dest && 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)
        {
            vyv_gaussian_conv(c, dest_y, src_y, width, 1);
            dest_y += width;
            src_y += width;
        }
        
        /* Filter each column of the channel. */
        for (x = 0; x < width; ++x)
            vyv_gaussian_conv(c, dest + x, dest + x, height, width);
        
        dest += num_pixels;
        src += num_pixels;
    }
    
    return;
}