Поиск фрагмента в изображении

#include <fstream>
#include <complex>
#include <vector>
#include <iostream>
 
#include "fftw3.h"
 
using namespace std;
 
void loadData(vector<complex<double>> &arr, int w, int h){
    arr.clear();
    arr.resize(w*h);
 
    for (int y = 0; y < h; y++) {
        for (int x = 0; x < w; x++) {
            arr[y * w + x] = rand()%5 + 0;
        }
    }
}
 
void loadFragment(vector<complex<double>> arr, int w, int h, vector<complex<double>> &fragment, int xo, int yo, int wf, int hf){
    fragment.clear();
    fragment.resize(wf*hf);
 
    for (int y = yo; y < yo+hf; y++) {
        for (int x = xo; x < xo+wf; x++) {
            fragment[(x-xo)+(y-yo)*wf] = arr[x+y*w];
        }
    }
}
 
void fft2(vector<complex<double>> arr, int w, int h, vector<complex<double>> &out){ 
    out.clear();
    out.resize(w*h);
    fftw_plan plan = fftw_plan_dft_2d(w, h, (fftw_complex*) &arr[0], (fftw_complex*) &out[0], FFTW_FORWARD, FFTW_ESTIMATE);
    fftw_execute(plan);
}
 
void ifft2(vector<complex<double>> arr, int w, int h, vector<complex<double>> &out){    
    out.clear();
    out.resize(w*h);
    fftw_plan plan = fftw_plan_dft_2d(w, h, (fftw_complex*) &arr[0], (fftw_complex*) &out[0], FFTW_BACKWARD, FFTW_ESTIMATE);
    fftw_execute(plan);
 
    for (int y = 0; y < h; y++) {
        for (int x = 0; x < w; x++) {
            out[y * w + x] = out[y * w + x].real() / (w * h);
        }
    }
}
 
void expandArr(vector<complex<double>> &arr, int w, int h, int wf, int hf){
    vector<complex<double>> temp;
    temp.resize(w*h);
 
    for (int y = 0; y < hf; y++) {
        for (int x = 0; x < wf; x++) {
            temp[y * w + x] = arr[y * wf + x];
        }
    }
 
    arr.clear();
    arr.resize(w*h);
 
    for (int y = 0; y < h; y++) {
        for (int x = 0; x < w; x++) {
            arr[y * w + x] = temp[y * w + x];
        }
    }
}
 
void print(vector<complex<double>> arr, int w, int h){
    for (int y = 0; y < h; y++) {
        for (int x = 0; x < w; x++) {
            cout << arr[y * w + x] << " ";
        }
        cout << endl;
    }
    cout << endl;
}
 
void conj(vector<complex<double>> &arr){    
    for(int k = 0; k < arr.size(); ++k) arr[k].imag(arr[k].imag()*(-1));
}
 
void avarage(vector<complex<double>> &arr){
    double a = 0;
 
    for (int y = 0; y < arr.size(); y++) a += arr[y].real();
 
    a /= arr.size();
 
    for (int y = 0; y < arr.size(); y++) arr[y] -= a;
}
 
void main()
{
    int w = 500;
    int h = 500;
    vector<complex<double>> arr;
    loadData(arr, w, h);
    avarage(arr);
 
    vector<complex<double>> forward;
    fft2(arr, w, h, forward);
    conj(forward);
 
    vector<complex<double>> fragment;
    int xo = 15;
    int yo = 35;
    int wf = 50;
    int hf = 60;
 
    loadFragment(arr, w, h, fragment, xo, yo, wf, hf);
    avarage(fragment);
    expandArr(fragment, w, h, wf, hf);
    vector<complex<double>> forwardF;
    fft2(fragment, w, h, forwardF);
 
    vector<complex<double>> conv;
 
    for (int y = 0; y < h-hf; y++) {
        for (int x = 0; x < w-wf; x++) {
            complex<double> sum = 0;
            for (int j = y; j < y+hf; j++) {
                for (int i = x; i < x+wf; i++) {
                    sum += (forwardF[i-x,j-y]*forward[x,y]);    
                }
            }
            sum /= (wf*hf);
            conv.push_back(sum);
        }
    }
 
    vector<complex<double>> backward;
    ifft2(conv, w-wf, h-hf, backward);
 
    system("pause");
}