convolution.h

#ifndef CVD_CONVOLUTION_H_
#define CVD_CONVOLUTION_H_

#include <algorithm>
#include <memory>
#include <numeric>
#include <vector>

#include <cvd/config.h>
#include <cvd/exceptions.h>
#include <cvd/image.h>
#include <cvd/internal/pixel_operations.h>
#include <cvd/utility.h>

namespace CVD
{

template <class T>
T gaussianKernel(std::vector<T>& k, T maxval, double stddev)
{
    double sum = 0;
    unsigned int i, argmax = 0;
    std::vector<double> kernel(k.size());
    for(i = 0; i < k.size(); i++)
    {
        double x = i + 0.5 - k.size() / 2.0;
        sum += kernel[i] = exp(-x * x / (2 * stddev * stddev));
        if(kernel[i] > kernel[argmax])
            argmax = i;
    }
    T finalSum = 0;
    for(i = 0; i < k.size(); i++)
        finalSum += k[i] = (T)(kernel[i] * maxval / kernel[argmax]);
    return finalSum;
}

template <class S, class T>
T scaleKernel(const std::vector<S>& k, std::vector<T>& scaled, T maxval)
{
    unsigned int i, argmax = 0;
    for(i = 1; i < k.size(); i++)
        if(k[i] > k[argmax])
            argmax = i;
    scaled.resize(k.size());
    T sum = 0;
    for(i = 0; i < k.size(); i++)
        sum += (scaled[i] = (T)((k[i] * maxval) / k[argmax]));
    return sum;
}

template <class T>
void convolveGaussian5_1(BasicImage<T>& I)
{
    int w = I.size().x;
    int h = I.size().y;
    int i, j;
    for(j = 0; j < w; j++)
    {
        T* src = I.data() + j;
        T* end = src + w * (h - 4);
        while(src != end)
        {
            T sum = (T)(0.0544887 * (src[0] + src[4 * w])
                + 0.2442010 * (src[w] + src[3 * w])
                + 0.4026200 * src[2 * w]);
            *(src) = sum;
            src += w;
        }
    }
    for(i = h - 5; i >= 0; i--)
    {
        T* src = I.data() + i * w;
        T* end = src + w - 4;
        while(src != end)
        {
            T sum = (T)(0.0544887 * (src[0] + src[4])
                + 0.2442010 * (src[1] + src[3])
                + 0.4026200 * src[2]);
            *(src + 2 * w + 2) = sum;
            ++src;
        }
    }
}

namespace Exceptions
{

    namespace Convolution
    {
        struct All : public CVD::Exceptions::All
        {
            using CVD::Exceptions::All::All;
        };

        struct IncompatibleImageSizes : public All
        {
            IncompatibleImageSizes(const std::string& function)
                : All("Incompatible image sizes in " + function)
            {
            }
        };

        struct OddSizedKernelRequired : public All
        {
            OddSizedKernelRequired(const std::string& function)
                : All("Odd sized kernel required in " + function) {};
        };
    }
}
//void convolveGaussian5_1(BasicImage<byte>& I);

template <class T>
void convolveWithBox(const BasicImage<T>& I, BasicImage<T>& J, ImageRef hwin)
{
    typedef typename Pixel::traits<T>::wider_type sum_type;
    if(I.size() != J.size())
    {
        throw Exceptions::Convolution::IncompatibleImageSizes("convolveWithBox");
    }
    int w = I.size().x;
    int h = I.size().y;
    ImageRef win = 2 * hwin + ImageRef(1, 1);
    const double factor = 1.0 / (win.x * win.y);
    std::vector<sum_type> buffer(w * win.y);
    std::vector<sum_type> sums_v(w);
    sum_type* sums = &sums_v[0];
    sum_type* next_row = &buffer[0];
    sum_type* oldest_row = &buffer[0];
    zeroPixels(sums, w);
    const T* input = I.data();
    T* output = J[hwin.y] - hwin.x;
    for(int i = 0; i < h; i++)
    {
        sum_type hsum = sum_type();
        const T* back = input;
        int j;
        for(j = 0; j < win.x - 1; j++)
            hsum += input[j];
        for(; j < w; j++)
        {
            hsum += input[j];
            next_row[j] = hsum;
            sums[j] += hsum;
            hsum -= *(back++);
        }
        if(i >= win.y - 1)
        {
            assign_multiple(sums + win.x - 1, factor, output + win.x - 1, w - win.x + 1);
            differences(sums + win.x - 1, oldest_row + win.x - 1, sums + win.x - 1, w - win.x + 1);
            output += w;
            oldest_row += w;
            if(oldest_row == &buffer[0] + w * win.y)
                oldest_row = &buffer[0];
        }
        input += w;
        next_row += w;
        if(next_row == &buffer[0] + w * win.y)
            next_row = &buffer[0];
    }
}

template <class T>
inline void convolveWithBox(const BasicImage<T>& I, BasicImage<T>& J, int hwin)
{
    convolveWithBox(I, J, ImageRef(hwin, hwin));
}

template <class T>
inline void convolveWithBox(BasicImage<T>& I, int hwin)
{
    convolveWithBox(I, I, hwin);
}

template <class T>
inline void convolveWithBox(BasicImage<T>& I, ImageRef hwin)
{
    convolveWithBox(I, I, hwin);
}

template <class T, int A, int B, int C>
void convolveSymmetric(Image<T>& I)
{
    typedef typename Pixel::traits<T>::wider_type wider;
    static const wider S = (A + B + C + B + A);
    int width = I.size().x;
    int height = I.size().y;
    T* p = I.data();
    int i, j;
    for(i = 0; i < height; i++)
    {
        wider a = p[0];
        wider b = p[1];
        wider c = p[2];
        wider d = p[3];
        p[0] = (T)(((c + c) * A + (b + b) * B + a * C) / S);
        p[1] = (T)(((b + d) * A + (a + c) * B + b * C) / S);
        for(j = 0; j < width - 4; j++, p++)
        {
            wider e = p[4];
            p[2] = (T)(((a + e) * A + (b + d) * B + c * C) / S);
            a = b;
            b = c;
            c = d;
            d = e;
        }
        p[2] = (T)(((a + c) * A + (b + d) * B + c * C) / S);
        p[3] = (T)(((b + b) * A + (c + c) * B + d * C) / S);
        p += 4;
    }
    for(j = 0; j < width; j++)
    {
        p = I.data() + j;
        wider a = p[0];
        wider b = p[width];
        p[0] = (T)(((p[2 * width] + p[2 * width]) * A + (b + b) * B + a * C) / S);
        p[width] = (T)(((b + p[width * 3]) * A + (a + p[2 * width]) * B + b * C) / S);
        for(i = 0; i < height - 4; i++)
        {
            wider c = p[2 * width];
            p[2 * width] = (T)(((a + p[4 * width]) * A + (b + p[3 * width]) * B + c * C) / S);
            a = b;
            b = c;
            p += width;
        }
        wider c = p[2 * width];
        p[2 * width] = (T)(((a + c) * A + (b + p[width * 3]) * B + c * C) / S);
        p[3 * width] = (T)(((b + b) * A + (c + c) * B + p[width * 3] * C) / S);
    }
}

template <class T, int A, int B, int C, int D>
void convolveSymmetric(Image<T>& I)
{
    typedef typename Pixel::traits<T>::wider_type wider;
    static const wider S = (A + B + C + D + C + B + A);
    int width = I.size().x;
    int height = I.size().y;
    T* p = I.data();
    int i, j;
    for(i = 0; i < height; i++)
    {
        wider a = p[0];
        wider b = p[1];
        wider c = p[2];
        wider d = p[3];
        p[0] = (T)(((d + d) * A + (c + c) * B + (b + b) * C + a * D) / S);
        p[1] = (T)(((c + p[4]) * A + (b + d) * B + (a + c) * C + b * D) / S);
        p[2] = (T)(((b + p[5]) * A + (a + p[4]) * B + (b + d) * C + c * D) / S);
        for(j = 0; j < width - 6; j++, p++)
        {
            d = p[3];
            p[3] = (T)(((a + p[6]) * A + (b + p[5]) * B + (c + p[4]) * C + d * D) / S);
            a = b;
            b = c;
            c = d;
        }
        d = p[3];
        wider e = p[4];
        p[3] = (T)(((a + e) * A + (b + p[5]) * B + (c + e) * C + d * D) / S);
        p[4] = (T)(((b + d) * A + (c + e) * B + (d + p[5]) * C + e * D) / S);
        p[5] = (T)(((c + c) * A + (d + d) * B + (e + e) * C + p[5] * D) / S);
        p += 6;
    }
    for(j = 0; j < width; j++)
    {
        p = I.data() + j;
        wider a = p[0];
        wider b = p[width];
        wider c = p[2 * width];
        wider d = p[3 * width];
        p[0] = (T)(((d + d) * A + (c + c) * B + (b + b) * C + a * D) / S);
        p[width] = (T)(((c + p[4 * width]) * A + (b + d) * B + (a + c) * C + b * D) / S);
        p[2 * width] = (T)(((b + p[5 * width]) * A + (a + p[4 * width]) * B + (b + d) * C + c * D) / S);
        for(i = 0; i < height - 6; i++)
        {
            d = p[3 * width];
            p[3 * width] = (T)(((a + p[width * 6]) * A + (b + p[width * 5]) * B + (c + p[width * 4]) * C + d * D) / S);
            a = b;
            b = c;
            c = d;
            p += width;
        }
        d = p[3 * width];
        wider e = p[4 * width];
        p[3 * width] = (T)(((a + e) * A + (b + p[5 * width]) * B + (c + e) * C + d * D) / S);
        p[4 * width] = (T)(((b + d) * A + (c + e) * B + (d + p[5 * width]) * C + e * D) / S);
        p[5 * width] = (T)(((c + c) * A + (d + d) * B + (e + e) * C + p[5 * width] * D) / S);
    }
}

template <class T, class K>
void convolveSeparableSymmetric(Image<T>& I, const std::vector<K>& kernel, K divisor)
{
    typedef typename Pixel::traits<T>::wider_type sum_type;
    int w = I.size().x;
    int h = I.size().y;
    int r = (int)kernel.size() / 2;
    int i, j;
    int m;
    double factor = 1.0 / divisor;
    for(j = 0; j < w; j++)
    {
        T* src = I.data() + j;
        for(i = 0; i < h - 2 * r; i++, src += w)
        {
            sum_type sum = src[r * w] * kernel[r], v;
            for(m = 0; m < r; m++)
                sum += (src[m * w] + src[(2 * r - m) * w]) * kernel[m];
            *(src) = static_cast<T>(sum * factor);
        }
    }
    int offset = r * w + r;
    for(i = h - 2 * r - 1; i >= 0; i--)
    {
        T* src = I[w];
        for(j = 0; j < w - 2 * r; j++, src++)
        {
            sum_type sum = src[r] * kernel[r], v;
            for(m = 0; m < r; m++)
                sum += (src[m] + src[2 * r - m]) * kernel[m];
            *(src + offset) = static_cast<T>(sum * factor);
        }
    }
}

template <class A, class B>
struct GetPixelRowTyped
{
    static inline const B* get(const A* row, int w, B* rowbuf)
    {
        std::copy(row, row + w, rowbuf);
        return rowbuf;
    }
};

template <class T>
struct GetPixelRowTyped<T, T>
{
    static inline const T* get(const T* row, int, T*)
    {
        return row;
    }
};

template <class A, class B>
const B* getPixelRowTyped(const A* row, int n, B* rowbuf)
{
    return GetPixelRowTyped<A, B>::get(row, n, rowbuf);
}

template <class T, class S>
struct CastCopy
{
    static inline void cast_copy(const T* from, S* to, int count)
    {
        for(int i = 0; i < count; i++)
            to[i] = static_cast<S>(from[i]);
    }
};

template <class T>
struct CastCopy<T, T>
{
    static inline void cast_copy(const T* from, T* to, int count)
    {
        std::copy(from, from + count, to);
    }
};

template <class T, class S>
inline void cast_copy(const T* from, S* to, int count) { CastCopy<T, S>::cast_copy(from, to, count); }

template <class T, int N = -1, int C = Pixel::Component<T>::count>
struct ConvolveMiddle
{
    template <class S>
    static inline T at(const T* input, const S& factor, const S* kernel) { return ConvolveMiddle<T, -1, C>::at(input, factor, kernel, N); }
};

template <class T, int N>
struct ConvolveMiddle<T, N, 1>
{
    template <class S>
    static inline T at(const T* input, const S& factor, const S* kernel) { return ConvolveMiddle<T, N - 1>::at(input, factor, kernel) + (input[-N] + input[N]) * kernel[N - 1]; }
};

template <class T>
struct ConvolveMiddle<T, -1, 1>
{
    template <class S>
    static inline T at(const T* input, const S& factor, const S* kernel, int ksize)
    {
        T hsum = *input * factor;
        for(int k = 0; k < ksize; k++)
            hsum += (input[-k - 1] + input[k + 1]) * kernel[k];
        return hsum;
    }
};

template <class T, int C>
struct ConvolveMiddle<T, -1, C>
{
    template <class S>
    static inline T at(const T* input, const S& factor, const S* kernel, int ksize)
    {
        T hsum = *input * factor;
        for(int k = 0; k < ksize; k++)
            hsum += (input[-k - 1] + input[k + 1]) * kernel[k];
        return hsum;
    }
};

template <class T>
struct ConvolveMiddle<T, 0, 1>
{
    template <class S>
    static inline T at(const T* input, const S& factor, const S*) { return *input * factor; }
};

template <class T, class S>
inline const T* convolveMiddle(const T* input, const S& factor, const S* kernel, int ksize, int n, T* output)
{
#define CALL_CM(I)                                                 \
    for(int j = 0; j < n; ++j, ++input, ++output)                  \
    {                                                              \
        *output = ConvolveMiddle<T, I>::at(input, factor, kernel); \
    }                                                              \
    break

    switch(ksize)
    {
        case 0:
            CALL_CM(0);
        case 1:
            CALL_CM(1);
        case 2:
            CALL_CM(2);
        case 3:
            CALL_CM(3);
        case 4:
            CALL_CM(4);
        case 5:
            CALL_CM(5);
        case 6:
            CALL_CM(6);
        case 7:
            CALL_CM(7);
        case 8:
            CALL_CM(8);
        default:
            for(int j = 0; j < n; j++, input++)
            {
                *(output++) = ConvolveMiddle<T, -1>::at(input, factor, kernel, ksize);
            }
    }
    return input;
#undef CALL_CM
}

template <class T>
inline void convolveGaussian(BasicImage<T>& I, double sigma, double sigmas = 3.0)
{
    convolveGaussian(I, I, sigma, sigmas);
}

template <class T>
void convolveGaussian(const BasicImage<T>& I, BasicImage<T>& out, double sigma, double sigmas = 3.0)
{
    typedef typename Pixel::traits<typename Pixel::Component<T>::type>::float_type sum_comp_type;
    typedef typename Pixel::traits<T>::float_type sum_type;
    assert(out.size() == I.size());
    int ksize = (int)ceil(sigmas * sigma);
    //std::cerr << "sigma: " << sigma << " kernel: " << ksize << std::endl;
    std::vector<sum_comp_type> kernel(ksize);
    sum_comp_type ksum = sum_comp_type();
    for(int i = 1; i <= ksize; i++)
        ksum += (kernel[i - 1] = static_cast<sum_comp_type>(exp(-i * i / (2 * sigma * sigma))));
    for(int i = 0; i < ksize; i++)
        kernel[i] /= (2 * ksum + 1);
    double factor = 1.0 / (2 * ksum + 1);
    int w = I.size().x;
    int h = I.size().y;
    int swin = 2 * ksize;

    if(w < ksize || h < ksize)
    {
        out.copy_from(I);
        return;
    }

    std::vector<sum_type> buffer(std::max(w, ksize) * (swin + 1));
    std::vector<sum_type> aligned_rowbuf(w);
    std::vector<sum_type> aligned_outbuf(w);

    sum_type* rowbuf = aligned_rowbuf.data();
    sum_type* outbuf = aligned_outbuf.data();

    std::vector<sum_type*> rows(swin + 1);
    for(int k = 0; k < swin + 1; k++)
        rows[k] = buffer.data() + k * std::max(w, ksize);

    T* output = out.data();
    for(int i = 0; i < h; i++)
    {
        sum_type* next_row = rows[swin];
        const sum_type* input = getPixelRowTyped(I[i], w, rowbuf);
        // beginning of row
        for(int j = 0; j < ksize; j++)
        {
            sum_type hsum = static_cast<sum_type>(input[j] * factor);
            for(int k = 0; k < ksize; k++)
                hsum += (input[std::max(j - k - 1, 0)] + input[j + k + 1]) * kernel[k];
            next_row[j] = hsum;
        }
        // middle of row
        input += ksize;
        input = convolveMiddle<sum_type, sum_comp_type>(input, static_cast<sum_comp_type>(factor), &kernel.front(), ksize, w - swin, next_row + ksize);
        // end of row
        for(int j = w - ksize; j < w; j++, input++)
        {
            sum_type hsum = static_cast<sum_type>(*input * factor);
            const int room = w - j;
            for(int k = 0; k < ksize; k++)
            {
                hsum += (input[-k - 1] + input[std::min(k + 1, room - 1)]) * kernel[k];
            }
            next_row[j] = hsum;
        }
        // vertical
        if(i >= swin)
        {
            const sum_type* middle_row = rows[ksize];
            assign_multiple(middle_row, factor, outbuf, w);
            for(int k = 0; k < ksize; k++)
            {
                const sum_comp_type m = kernel[k];
                const sum_type* row1 = rows[ksize - k - 1];
                const sum_type* row2 = rows[ksize + k + 1];
                add_multiple_of_sum(row1, row2, m, outbuf, w);
            }
            cast_copy(outbuf, output, w);
            output += w;
            if(i == h - 1)
            {
                for(int r = 0; r < ksize; r++)
                {
                    const sum_type* middle_row = rows[ksize + r + 1];
                    assign_multiple(middle_row, factor, outbuf, w);
                    for(int k = 0; k < ksize; k++)
                    {
                        const sum_comp_type m = kernel[k];
                        const sum_type* row1 = rows[ksize + r - k];
                        const sum_type* row2 = rows[std::min(ksize + r + k + 2, swin)];
                        add_multiple_of_sum(row1, row2, m, outbuf, w);
                    }
                    cast_copy(outbuf, output, w);
                    output += w;
                }
            }
        }
        else if(i == swin - 1)
        {
            for(int r = 0; r < ksize; r++)
            {
                const sum_type* middle_row = rows[r + 1];
                assign_multiple(middle_row, factor, outbuf, w);
                for(int k = 0; k < ksize; k++)
                {
                    const sum_comp_type m = kernel[k];
                    const sum_type* row1 = rows[std::max(r - k - 1, 0) + 1];
                    const sum_type* row2 = rows[r + k + 2];
                    add_multiple_of_sum(row1, row2, m, outbuf, w);
                }
                cast_copy(outbuf, output, w);
                output += w;
            }
        }

        sum_type* tmp = rows[0];
        for(int r = 0; r < swin; r++)
            rows[r] = rows[r + 1];
        rows[swin] = tmp;
    }
}

void compute_van_vliet_b(double sigma, double b[]);
void compute_triggs_M(const double b[], double M[][3]);
void van_vliet_blur(const double b[], const BasicImage<float> in, BasicImage<float> out);

void convolveGaussian(const BasicImage<float>& I, BasicImage<float>& out, double sigma, double sigmas = 3.0);
void convolveGaussian_fir(const BasicImage<float>& I, BasicImage<float>& out, double sigma, double sigmas = 3.0);

template <class T, class O, class K>
void convolve_gaussian_3(const BasicImage<T>& I, BasicImage<O>& out, K k1, K k2)
{
    assert(I.size() == out.size());
    const T* a = I.data();
    const int w = I.size().x;
    O* o = out.data() + w + 1;
    int total = I.totalsize() - 2 * w - 2;
    const double cross = k1 * k2;
    k1 *= k1;
    k2 *= k2;
    while(total--)
    {
        const double sum = k1 * (a[0] + a[2] + a[w * 2] + a[w * 2 + 2]) + cross * (a[1] + a[w * 2 + 1] + a[w] + a[w + 2]) + k2 * a[w + 1];
        *o++ = Pixel::scalar_convert<O, T, double>(sum);
        ++a;
    }
}

} // namespace CVD

#endif