cvd/convolution.h

/*
        This file is part of the CVD Library.

        Copyright (C) 2005 The Authors

        This library is free software; you can redistribute it and/or
        modify it under the terms of the GNU Lesser General Public
        License as published by the Free Software Foundation; either
        version 2.1 of the License, or (at your option) any later version.

        This library is distributed in the hope that it will be useful,
        but WITHOUT ANY WARRANTY; without even the implied warranty of
        MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
        Lesser General Public License for more details.

        You should have received a copy of the GNU Lesser General Public
        License along with this library; if not, write to the Free Software
        Foundation, Inc.,
    51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/

#ifndef CVD_CONVOLUTION_H_
#define CVD_CONVOLUTION_H_

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

#include <cvd/config.h>
#include <cvd/exceptions.h>
#include <cvd/image.h>
#include <cvd/internal/pixel_operations.h>
#include <cvd/internal/aligned_mem.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 {};

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

                
        struct OddSizedKernelRequired : public All {
            OddSizedKernelRequired(const std::string & function)
            {
                what = "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;

    AlignedMem<sum_type,16> buffer(w*(swin+1));
    AlignedMem<sum_type,16> aligned_rowbuf(w);
    AlignedMem<sum_type,16> 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*w;

    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 SubImage<float> in, SubImage<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