modules/objdetect/src/fft.cpp

   1 #include "precomp.hpp"
   2 #include "_lsvm_fft.h"
   3
   4 // static int getEntireRes(int number, int divisor, int *entire, int *res)
   5 // {
   6 //     *entire = number / divisor;
   7 //     *res = number % divisor;
   8 //     return FFT_OK;
   9 // }
  10
  11 static int getMultipliers(int n, int *n1, int *n2)
  12 {
  13     int multiplier, i;
  14     if (n == 1)
  15     {
  16         *n1 = 1;
  17         *n2 = 1;
  18         return FFT_ERROR; // n = 1
  19     }
  20     multiplier = n / 2;
  21     for (i = multiplier; i >= 2; i--)
  22     {
  23         if (n % i == 0)
  24         {
  25             *n1 = i;
  26             *n2 = n / i;
  27             return FFT_OK; // n = n1 * n2
  28         }
  29     }
  30     *n1 = 1;
  31     *n2 = n;
  32     return FFT_ERROR; // n - prime number
  33 }
  34
  35 /*
  36 // 1-dimensional FFT
  37 //
  38 // API
  39 // int fft(float *x_in, float *x_out, int n, int shift);
  40 // INPUT
  41 // x_in              - input signal
  42 // n                 - number of elements for searching Fourier image
  43 // shift             - shift between input elements
  44 // OUTPUT
  45 // x_out             - output signal (contains 2n elements in order
  46                        Re(x_in[0]), Im(x_in[0]), Re(x_in[1]), Im(x_in[1]) and etc.)
  47 // RESULT
  48 // Error status
  49 */
  50 int fft(float *x_in, float *x_out, int n, int shift)
  51 {
  52     int n1, n2, res, k1, k2, m1, m2, index, idx;
  53     float alpha, beta, gamma, angle, cosAngle, sinAngle;
  54     float tmpGamma, tmpAlpha, tmpBeta;
  55     float tmpRe, tmpIm, phaseRe, phaseIm;
  56     res = getMultipliers(n, &n1, &n2);
  57     if (res == FFT_OK)
  58     {
  59         fft(x_in, x_out, n1, shift);
  60         fft(x_in, x_out, n2, shift);
  61     }
  62     alpha = (float)(2.0 * PI / ((float)n));
  63     beta = (float)(2.0 * PI / ((float)n1));
  64     gamma = (float)(2.0 * PI / ((float)n2));
  65     for (k1 = 0; k1 < n1; k1++)
  66     {
  67         tmpBeta = beta * k1;
  68         for (k2 = 0; k2 < n2; k2++)
  69         {
  70             idx = shift * (n2 * k1 + k2);
  71             x_out[idx] = 0.0;
  72             x_out[idx + 1] = 0.0;
  73             tmpGamma = gamma * k2;
  74             tmpAlpha = alpha * k2;
  75             for (m1 = 0; m1 < n1; m1++)
  76             {
  77                 tmpRe = 0.0;
  78                 tmpIm = 0.0;
  79                 for (m2 = 0; m2 < n2; m2++)
  80                 {
  81                     angle = tmpGamma * m2;
  82                     index = shift * (n1 * m2 + m1);
  83                     cosAngle = cosf(angle);
  84                     sinAngle = sinf(angle);
  85                     tmpRe += x_in[index] * cosAngle + x_in[index + 1] * sinAngle;
  86                     tmpIm += x_in[index + 1] * cosAngle - x_in[index] * sinAngle;
  87                 }
  88                 angle = tmpAlpha * m1;
  89                 cosAngle = cosf(angle);
  90                 sinAngle = sinf(angle);
  91                 phaseRe = cosAngle * tmpRe + sinAngle * tmpIm;
  92                 phaseIm = cosAngle * tmpIm - sinAngle * tmpRe;
  93                 angle = tmpBeta * m1;
  94                 cosAngle = cosf(angle);
  95                 sinAngle = sinf(angle);
  96                 x_out[idx] += (cosAngle * phaseRe + sinAngle * phaseIm);
  97                 x_out[idx + 1] += (cosAngle * phaseIm - sinAngle * phaseRe);
  98             }
  99         }
 100     }
 101     return FFT_OK;
 102 }
 103
 104 /*
 105 // Inverse 1-dimensional FFT
 106 //
 107 // API
 108 // int fftInverse(float *x_in, float *x_out, int n, int shift);
 109 // INPUT
 110 // x_in              - Fourier image of 1d input signal(contains 2n elements
 111                        in order Re(x_in[0]), Im(x_in[0]),
 112                        Re(x_in[1]), Im(x_in[1]) and etc.)
 113 // n                 - number of elements for searching counter FFT image
 114 // shift             - shift between input elements
 115 // OUTPUT
 116 // x_in              - input signal (contains n elements)
 117 // RESULT
 118 // Error status
 119 */
 120 int fftInverse(float *x_in, float *x_out, int n, int shift)
 121 {
 122     int n1, n2, res, k1, k2, m1, m2, index, idx;
 123     float alpha, beta, gamma, angle, cosAngle, sinAngle;
 124     float tmpRe, tmpIm, phaseRe, phaseIm;
 125     res = getMultipliers(n, &n1, &n2);
 126     if (res == FFT_OK)
 127     {
 128         fftInverse(x_in, x_out, n1, shift);
 129         fftInverse(x_in, x_out, n2, shift);
 130     }
 131     alpha = (float)(2.0f * PI / ((float)n));
 132     beta = (float)(2.0f * PI / ((float)n1));
 133     gamma = (float)(2.0f * PI / ((float)n2));
 134     for (m1 = 0; m1 < n1; m1++)
 135     {
 136         for (m2 = 0; m2 < n2; m2++)
 137         {
 138             idx = (n1 * m2 + m1) * shift;
 139             x_out[idx] = 0.0;
 140             x_out[idx + 1] = 0.0;
 141             for (k2 = 0; k2 < n2; k2++)
 142             {
 143                 tmpRe = 0.0;
 144                 tmpIm = 0.0;
 145                 for (k1 = 0; k1 < n1; k1++)
 146                 {
 147                     angle = beta * k1 * m1;
 148                     index = shift *(n2 * k1 + k2);
 149                     sinAngle = sinf(angle);
 150                     cosAngle = cosf(angle);
 151                     tmpRe += x_in[index] * cosAngle - x_in[index + 1] * sinAngle;
 152                     tmpIm += x_in[index] * sinAngle + x_in[index + 1] * cosAngle;
 153                 }
 154                 angle = alpha * m1 * k2;
 155                 sinAngle = sinf(angle);
 156                 cosAngle = cosf(angle);
 157                 phaseRe = cosAngle * tmpRe - sinAngle * tmpIm;
 158                 phaseIm = cosAngle * tmpIm + sinAngle * tmpRe;
 159                 angle = gamma * k2 * m2;
 160                 sinAngle = sinf(angle);
 161                 cosAngle = cosf(angle);
 162                 x_out[idx] += cosAngle * phaseRe - sinAngle * phaseIm;
 163                 x_out[idx + 1] += cosAngle * phaseIm + sinAngle * phaseRe;
 164             }
 165             x_out[idx] /= n;
 166             x_out[idx + 1] /= n;
 167         }
 168     }
 169     return FFT_OK;
 170 }
 171
 172 /*
 173 // 2-dimensional FFT
 174 //
 175 // API
 176 // int fft2d(float *x_in, float *x_out, int numRows, int numColls);
 177 // INPUT
 178 // x_in              - input signal (matrix, launched by rows)
 179 // numRows           - number of rows
 180 // numColls          - number of collumns
 181 // OUTPUT
 182 // x_out             - output signal (contains (2 * numRows * numColls) elements
 183                        in order Re(x_in[0][0]), Im(x_in[0][0]),
 184                        Re(x_in[0][1]), Im(x_in[0][1]) and etc.)
 185 // RESULT
 186 // Error status
 187 */
 188 int fft2d(float *x_in, float *x_out, int numRows, int numColls)
 189 {
 190     int i, size;
 191     float *x_outTmp;
 192     size = numRows * numColls;
 193     x_outTmp = (float *)malloc(sizeof(float) * (2 * size));
 194     for (i = 0; i < numRows; i++)
 195     {
 196         fft(x_in + i * 2 * numColls,
 197             x_outTmp + i * 2 * numColls,
 198             numColls, 2);
 199     }
 200     for (i = 0; i < numColls; i++)
 201     {
 202         fft(x_outTmp + 2 * i,
 203             x_out + 2 * i,
 204             numRows, 2 * numColls);
 205     }
 206     free(x_outTmp);
 207     return FFT_OK;
 208 }
 209
 210 /*
 211 // Inverse 2-dimensional FFT
 212 //
 213 // API
 214 // int fftInverse2d(float *x_in, float *x_out, int numRows, int numColls);
 215 // INPUT
 216 // x_in              - Fourier image of matrix (contains (2 * numRows * numColls)
 217                        elements in order Re(x_in[0][0]), Im(x_in[0][0]),
 218                        Re(x_in[0][1]), Im(x_in[0][1]) and etc.)
 219 // numRows           - number of rows
 220 // numColls          - number of collumns
 221 // OUTPUT
 222 // x_out             - initial signal (matrix, launched by rows)
 223 // RESULT
 224 // Error status
 225 */
 226 int fftInverse2d(float *x_in, float *x_out, int numRows, int numColls)
 227 {
 228     int i, size;
 229     float *x_outTmp;
 230     size = numRows * numColls;
 231     x_outTmp = (float *)malloc(sizeof(float) * (2 * size));
 232     for (i = 0; i < numRows; i++)
 233     {
 234         fftInverse(x_in + i * 2 * numColls,
 235             x_outTmp + i * 2 * numColls,
 236             numColls, 2);
 237     }
 238     for (i = 0; i < numColls; i++)
 239     {
 240         fftInverse(x_outTmp + 2 * i,
 241             x_out + 2 * i,
 242             numRows, 2 * numColls);
 243     }
 244     free(x_outTmp);
 245     return FFT_OK;
 246 }