Skip to content

Commit

Permalink
Plain C version
Browse files Browse the repository at this point in the history
  • Loading branch information
@markondej
markondej committed Oct 26, 2020
1 parent 37409c2 commit 3f95b5d
Showing 1 changed file with 58 additions and 61 deletions.
119 changes: 58 additions & 61 deletions phase_correl.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,31 +2,28 @@
#include <stdlib.h>
#include <math.h>

#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;
}
}
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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;
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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;
}

0 comments on commit 3f95b5d

Please sign in to comment.