C++ Pattern Matching with FFT cross-correlation (изображения)

Все, что я пытаюсь реализовать patter matching с FFT, но я не уверен, каким должен быть результат (я думаю, что мне чего-то не хватает, хотя я прочитал много материала о проблеме и перепробовал много различных реализаций, эта пока лучшая). Вот моя корреляционная функция FFT.

void fft2d(fftw_complex**& a, int rows, int cols, bool forward = true)
{
    fftw_plan p;
    for (int i = 0; i < rows; ++i)
    {
        p = fftw_plan_dft_1d(cols, a[i], a[i], forward ? FFTW_FORWARD : FFTW_BACKWARD, FFTW_ESTIMATE);
        fftw_execute(p);
    }

    fftw_complex* t = (fftw_complex*)fftw_malloc(rows * sizeof(fftw_complex));

    for (int j = 0; j < cols; ++j)
    {
        for (int i = 0; i < rows; ++i)
        {
            t[i][0] = a[i][j][0];
            t[i][1] = a[i][j][1];
        }

        p = fftw_plan_dft_1d(rows, t, t, forward ? FFTW_FORWARD : FFTW_BACKWARD, FFTW_ESTIMATE);
        fftw_execute(p);

        for (int i = 0; i < rows; ++i)
        {
            a[i][j][0] = t[i][0];
            a[i][j][1] = t[i][1];
        }
    }

    fftw_free(t);
}

int findCorrelation(int argc, char* argv[])
{
    BMP bigImage;
    BMP keyImage;
    BMP result;
    RGBApixel blackPixel = { 0, 0, 0, 1 };
    const bool swapQuadrants = (argc == 4);

    if (argc < 3 || argc > 4) {
        cout << "correlation img1.bmp img2.bmp" << endl;
        return 1;
    }

    if (!keyImage.ReadFromFile(argv[1])) {
        return 1;
    }

    if (!bigImage.ReadFromFile(argv[2])) {
        return 1;
}
    //Preparations
    const int maxWidth = std::max(bigImage.TellWidth(), keyImage.TellWidth());
    const int maxHeight = std::max(bigImage.TellHeight(), keyImage.TellHeight());

    const int rowsCount = maxHeight;
    const int colsCount = maxWidth;

    BMP bigTemp = bigImage;
    BMP keyTemp = keyImage;

    keyImage.SetSize(maxWidth, maxHeight);
    bigImage.SetSize(maxWidth, maxHeight);

    for (int i = 0; i < rowsCount; ++i)
        for (int j = 0; j < colsCount; ++j) {
            RGBApixel p1;

            if (i < bigTemp.TellHeight() && j < bigTemp.TellWidth()) {
                p1 = bigTemp.GetPixel(j, i);
            } else {
                p1 = blackPixel;
            }

            bigImage.SetPixel(j, i, p1);

            RGBApixel p2;

            if (i < keyTemp.TellHeight() && j < keyTemp.TellWidth()) {
                p2 = keyTemp.GetPixel(j, i);
            } else {
                p2 = blackPixel;
            }

            keyImage.SetPixel(j, i, p2);
        }
    //Here is where the transforms begin
    fftw_complex **a = (fftw_complex**)fftw_malloc(rowsCount * sizeof(fftw_complex*));
    fftw_complex **b = (fftw_complex**)fftw_malloc(rowsCount * sizeof(fftw_complex*));
    fftw_complex **c = (fftw_complex**)fftw_malloc(rowsCount * sizeof(fftw_complex*));

    for (int i = 0; i < rowsCount; ++i) {

        a[i] = (fftw_complex*)fftw_malloc(colsCount * sizeof(fftw_complex));
        b[i] = (fftw_complex*)fftw_malloc(colsCount * sizeof(fftw_complex));
        c[i] = (fftw_complex*)fftw_malloc(colsCount * sizeof(fftw_complex));

        for (int j = 0; j < colsCount; ++j) {
            RGBApixel p1;

            p1 = bigImage.GetPixel(j, i);

            a[i][j][0] = (0.299*p1.Red + 0.587*p1.Green + 0.114*p1.Blue);
            a[i][j][1] = 0.0;

            RGBApixel p2;

            p2 = keyImage.GetPixel(j, i);

            b[i][j][0] = (0.299*p2.Red + 0.587*p2.Green + 0.114*p2.Blue);
            b[i][j][1] = 0.0;
        }
    }

    fft2d(a, rowsCount, colsCount);
    fft2d(b, rowsCount, colsCount);

    result.SetSize(maxWidth, maxHeight);

    for (int i = 0; i < rowsCount; ++i)
        for (int j = 0; j < colsCount; ++j) {
            fftw_complex& y = a[i][j];
            fftw_complex& x = b[i][j];

            double u = x[0], v = x[1];
            double m = y[0], n = y[1];

            c[i][j][0] = u*m + n*v;
            c[i][j][1] = v*m - u*n;

            int fx = j;
            if (fx>(colsCount / 2)) fx -= colsCount;

            int fy = i;
            if (fy>(rowsCount / 2)) fy -= rowsCount;

            float r2 = (fx*fx + fy*fy);

            const double cuttoffCoef = (maxWidth * maxHeight) / 37992.;
            if (r2<128 * 128 * cuttoffCoef)
                c[i][j][0] = c[i][j][1] = 0;
        }

    fft2d(c, rowsCount, colsCount, false);

    const int halfCols = colsCount / 2;
    const int halfRows = rowsCount / 2;

    if (swapQuadrants) {
        for (int i = 0; i < halfRows; ++i)
            for (int j = 0; j < halfCols; ++j) {
                std::swap(c[i][j][0], c[i + halfRows][j + halfCols][0]);
                std::swap(c[i][j][1], c[i + halfRows][j + halfCols][1]);
            }

        for (int i = halfRows; i < rowsCount; ++i)
            for (int j = 0; j < halfCols; ++j) {
                std::swap(c[i][j][0], c[i - halfRows][j + halfCols][0]);
                std::swap(c[i][j][1], c[i - halfRows][j + halfCols][1]);
            }
    }

    for (int i = 0; i < rowsCount; ++i)
        for (int j = 0; j < colsCount; ++j) {
            const double& g = c[i][j][0];
            RGBApixel pixel;

            pixel.Alpha = 0;
            int gInt = 255 - static_cast<int>(std::floor(g + 0.5));
            pixel.Red = gInt;
            pixel.Green = gInt;
            pixel.Blue = gInt;

            result.SetPixel(j, i, pixel);
        }


    BMP res;
    res.SetSize(maxWidth, maxHeight);

    result.WriteToFile("result.bmp");

    return 0;
}

Образец вывода