diff --git a/phase_correl.c b/phase_correl.c index 2c614b6..b8cf66c 100644 --- a/phase_correl.c +++ b/phase_correl.c @@ -2,31 +2,28 @@ #include #include -#define IMAGE_WIDTH 256 -#define IMAGE_HEIGHT 128 - -void ditfft2(double *input, double *output, int size, int s, int invert) { +void ditfft2(double *input, double *output, unsigned size, unsigned stride, int invert) { double temp[4], fi, kfi, cosfi, sinfi; - int k, k2; + unsigned k; if (size > 1) { - ditfft2(&input[0], &output[0], size >> 1, s << 1, invert); - ditfft2(&input[s << 1], &output[size], size >> 1, s << 1, invert); + ditfft2(&input[0], &output[0], size >> 1, stride << 1, invert); + ditfft2(&input[stride << 1], &output[size], size >> 1, stride << 1, invert); if (!invert) { fi = -2.0 * M_PI / (double)size; kfi = 0.0; } else { fi = 2.0 * M_PI / (double)size; kfi = 0.0; } - for (k = 0; k < size >> 1; k++) { - cosfi = cos(kfi); sinfi = sin(kfi); k2 = k << 1; - temp[0] = output[k2]; temp[1] = output[k2 + 1]; - temp[2] = output[size + k2]; temp[3] = output[size + k2 + 1]; - - output[k2] = temp[0] + temp[2] * cosfi - temp[3] * sinfi; - output[k2 + 1] = temp[1] + temp[2] * sinfi + temp[3] * cosfi; - output[size + k2] = temp[0] - temp[2] * cosfi + temp[3] * sinfi; - output[size + k2 + 1] = temp[1] - temp[2] * sinfi - temp[3] * cosfi; + for (k = 0; k < size; k += 2) { + cosfi = cos(kfi); sinfi = sin(kfi); + temp[0] = output[k]; temp[1] = output[k + 1]; + temp[2] = output[size + k]; temp[3] = output[size + k + 1]; + + output[k] = temp[0] + temp[2] * cosfi - temp[3] * sinfi; + output[k + 1] = temp[1] + temp[2] * sinfi + temp[3] * cosfi; + output[size + k] = temp[0] - temp[2] * cosfi + temp[3] * sinfi; + output[size + k + 1] = temp[1] - temp[2] * sinfi - temp[3] * cosfi; kfi += fi; } } @@ -36,16 +33,16 @@ void ditfft2(double *input, double *output, int size, int s, int invert) { } } -void fft(double *input, double *output, int size) { +void fft(double *input, double *output, unsigned size) { ditfft2(input, output, size, 1, 0); } -void ifft(double *input, double *output, int size) { +void ifft(double *input, double *output, unsigned size) { ditfft2(input, output, size, 1, 1); } -void fft2D(double *input, int width, int height) { - int i, j; +void fft2D(double *input, unsigned width, unsigned height) { + unsigned i, j; double *fft_input = (double *)malloc((sizeof(double) * width) << 1); double *fft_output = (double *)malloc((sizeof(double) * width) << 1); @@ -80,7 +77,7 @@ void fft2D(double *input, int width, int height) { } void ifft2D(double *input, int width, int height) { - int i, j; + unsigned i, j; double *fft_input = (double *)malloc((sizeof(double) * width) << 1); double *fft_output = (double *)malloc((sizeof(double) * width) << 1); @@ -101,8 +98,8 @@ void ifft2D(double *input, int width, int height) { for (i = 0; i < width; i++) { fft_input[i << 1] = fft_temp[(i + j * width) << 1]; fft_input[(i << 1) + 1] = fft_temp[((i + j * width) << 1) + 1]; - } - ifft(fft_input, fft_output, width); + } + ifft(fft_input, fft_output, width); for (i = 0; i < width; i++) { input[(i + j * width) << 1] = fft_output[i << 1] / (double)width; input[((i + j * width) << 1) + 1] = fft_output[(i << 1) + 1] / (double)width; @@ -114,8 +111,23 @@ void ifft2D(double *input, int width, int height) { free(fft_temp); } -void computeShift(unsigned char *image1, unsigned char *image2, int width, int height, int *tx, int *ty) { - int i, j; +void computeNormalized(double *f, double *g, double *r) { + double a1 = (f[1] != 0.0f) ? ((f[0] != 0.0f) ? atan(f[1] / fabs(f[0])) : M_PI * f[1] / (2.0f * fabs(f[1]))) : 0.0f; + if (f[0] < 0.0f) { + a1 = ((f[1] < 0.0f) ? -1.0f : 1.0f) * M_PI - a1; + } + + double a2 = (g[1] != 0.0f) ? ((g[0] != 0.0f) ? atan(g[1] / fabs(g[0])) : M_PI * g[1] / (2.0f * fabs(g[1]))) : 0.0f; + if (g[0] < 0.0f) { + a2 = ((g[1] < 0.0f) ? -1.0f : 1.0f) * M_PI - a2; + } + + r[0] = cos(a1 - a2); + r[1] = sin(a1 - a2); +} + +void computeShift(unsigned char *image1, unsigned char *image2, unsigned width, unsigned height, int *deltax, int *deltay) { + unsigned i, j; double* fft_input1 = (double *)malloc((sizeof(double) * width * height) << 1); double* fft_input2 = (double *)malloc((sizeof(double) * width * height) << 1); @@ -136,42 +148,26 @@ void computeShift(unsigned char *image1, unsigned char *image2, int width, int h // Compute normalized cross power spectrum for (i = 0; i < width * height; i++) { - double a = fft_input1[i << 1]; - double b = fft_input1[(i << 1) + 1]; - double c = fft_input2[i << 1]; - double d = fft_input2[(i << 1) + 1]; - - double a1 = (b != 0.0) ? ((a != 0.0) ? atan(b / fabs(a)) : M_PI * b / (2.0 * fabs(b))) : 0.0; - if (a < 0.0) { - a1 = ((b < 0.0) ? -1.0 : 1.0) * M_PI - a1; - } - - double a2 = (d != 0.0) ? ((c != 0.0) ? atan(d / fabs(c)) : M_PI * d / (2.0 * fabs(d))) : 0.0; - if (c < 0.0) { - a2 = ((d < 0.0) ? -1.0 : 1.0) * M_PI - a2; - } - - fft_output[i << 1] = cos(a1 - a2); - fft_output[(i << 1) + 1] = sin(a1 - a2); + computeNormalized(&fft_input1[i << 1], &fft_input2[i << 1], &fft_output[i << 1]); } // Perform inversed 2D FFT on obtained matrix ifft2D(fft_output, width, height); // Search for peak - double max = 0.0; *tx = 0; *ty = 0; + double max = 0.0; *deltax = 0; *deltay = 0; for (j = 0; j < height; j++) for (i = 0; i < width; i++) { double d = sqrt(pow(fft_output[(i + j * width) << 1], 2) + pow(fft_output[((i + j * width) << 1) + 1], 2)); if (d > max) { - max = d; *tx = i; *ty = j; + max = d; *deltax = i; *deltay = j; } } - if (*tx > width >> 1) - *tx = *tx - width; - if (*ty > height >> 1) - *ty = *ty - height; + if (*deltax > width >> 1) + *deltax = *deltax - width; + if (*deltay > height >> 1) + *deltay = *deltay - height; free(fft_input1); free(fft_input2); @@ -180,28 +176,29 @@ void computeShift(unsigned char *image1, unsigned char *image2, int width, int h int main() { - unsigned char image1[IMAGE_WIDTH * IMAGE_HEIGHT]; - unsigned char image2[IMAGE_WIDTH * IMAGE_HEIGHT]; + unsigned char image1[256 * 128]; + unsigned char image2[256 * 128]; - int i, j, tx, ty; + unsigned i, j; + int deltax, deltay; // Generate pair of images - for (j = 0; j < IMAGE_HEIGHT; j++) - for (i = 0; i < IMAGE_WIDTH; i++) { - if ((i > 16) && (i < 80) && (j > 32) && (j < 96)) - image1[i + j * IMAGE_WIDTH] = 128; + for (j = 0; j < 128; j++) + for (i = 0; i < 256; i++) { + if ((i >= 16) && (i < 76) && (j >= 32) && (j < 92)) + image1[i + j * 256] = 128; else - image1[i + j * IMAGE_WIDTH] = 0; + image1[i + j * 256] = 0; - if ((i > 8) && (i < 72) && (j > 40) && (j < 104)) - image2[i + j * IMAGE_WIDTH] = 16; + if ((i >= 8) && (i < 68) && (j >= 40) && (j < 100)) + image2[i + j * 256] = 16; else - image2[i + j * IMAGE_WIDTH] = 255; + image2[i + j * 256] = 255; } - computeShift(image1, image2, IMAGE_WIDTH, IMAGE_HEIGHT, &tx, &ty); + computeShift(image1, image2, 256, 128, &deltax, &deltay); - printf("Computed shift: [%d, %d]\n", tx, ty); + printf("Computed shift: [%d, %d]\n", deltax, deltay); return 0; }