Fixed FFT and Phase Correlation!

There were some mistakes, they are fixed now. FFT 1D and 2D work
flawlessly. No problems with that code anymore.

As for phase correlation I need to study how to interpret the output
better. The function generates noisy results once you move the image too
far and I'm not quite sure if I have the code right for detecting
positive and negative displacements.
This commit is contained in:
Kwabena W. Agyeman 2016-06-14 00:00:07 -04:00
parent 8c8e1d08c8
commit 15aed91efd
3 changed files with 44 additions and 42 deletions

View File

@ -382,33 +382,35 @@ static void do_ifft(float *inout, int N_pow2, int stride)
} }
} }
} }
float div = 1.0 / (N >> 1); float div = 1.0 / (N >> 1);
for (int i = 0; i < N; i++) { for (int i = 0; i < N; i += 2) {
inout[i] *= div; inout[(i*stride)+0] *= div;
inout[(i*stride)+1] *= div;
} }
} }
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
void alloc_fft1d_buffer(fft1d_controller_t *controller, uint8_t *data, int len) void fft1d_alloc(fft1d_controller_t *controller, uint8_t *buf, int len)
{ {
controller->d_pointer = data; controller->d_pointer = buf;
controller->d_len = len; controller->d_len = len;
controller->pow2 = int_clog2(len); controller->pow2 = int_clog2(len);
controller->data = fb_alloc((2 << controller->pow2) * sizeof(float)); controller->data = fb_alloc((2 << controller->pow2) * sizeof(float));
} }
void dealloc_fft1d_buffer() void fft1d_dealloc()
{ {
fb_free(); fb_free();
} }
void do_1dfft(fft1d_controller_t *controller) void fft1d_run(fft1d_controller_t *controller)
{ {
// We can speed up the FFT by packing data into both the real and imaginary // We can speed up the FFT by packing data into both the real and imaginary
// values. This results in having to do an FFT of half the size normally. // values. This results in having to do an FFT of half the size normally.
float *h_buffer = fb_alloc((2 << (controller->pow2-1)) * sizeof(float)); float *h_buffer = fb_alloc((1 << controller->pow2) * sizeof(float));
prepare_real_input(controller->d_pointer, controller->d_len, prepare_real_input(controller->d_pointer, controller->d_len,
h_buffer, controller->pow2 - 1); h_buffer, controller->pow2 - 1);
do_fft(h_buffer, controller->pow2 - 1, 1); do_fft(h_buffer, controller->pow2 - 1, 1);
@ -416,24 +418,24 @@ void do_1dfft(fft1d_controller_t *controller)
fb_free(); fb_free();
} }
void do_1difft(fft1d_controller_t *controller) void ifft1d_run(fft1d_controller_t *controller)
{ {
// We can speed up the FFT by packing data into both the real and imaginary // We can speed up the FFT by packing data into both the real and imaginary
// values. This results in having to do an FFT of half the size normally. // values. This results in having to do an FFT of half the size normally.
float *h_buffer = fb_alloc((2 << (controller->pow2-1)) * sizeof(float)); float *h_buffer = fb_alloc((1 << controller->pow2) * sizeof(float));
pack_fft(controller->data, h_buffer, controller->pow2 - 1); pack_fft(controller->data, h_buffer, controller->pow2 - 1);
prepare_complex_input(h_buffer, h_buffer, prepare_complex_input(h_buffer, h_buffer,
controller->pow2 - 1, 1); controller->pow2 - 1, 1);
do_ifft(h_buffer, controller->pow2 - 1, 1); do_ifft(h_buffer, controller->pow2 - 1, 1);
memset(controller->data, 0, (2 << controller->pow2) * sizeof(float)); memset(controller->data, 0, (2 << controller->pow2) * sizeof(float));
memcpy(controller->data, h_buffer, (2 << (controller->pow2-1)) * sizeof(float)); memcpy(controller->data, h_buffer, (1 << controller->pow2) * sizeof(float));
fb_free(); fb_free();
} }
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
void alloc_fft2d_buffer(fft2d_controller_t *controller, image_t *img, rectangle_t *r) void fft2d_alloc(fft2d_controller_t *controller, image_t *img, rectangle_t *r)
{ {
controller->img = img; controller->img = img;
if (!rectangle_subimg(controller->img, r, &controller->r)) ff_no_intersection(NULL); if (!rectangle_subimg(controller->img, r, &controller->r)) ff_no_intersection(NULL);
@ -445,7 +447,7 @@ void alloc_fft2d_buffer(fft2d_controller_t *controller, image_t *img, rectangle_
fb_alloc0(2 * (1 << controller->w_pow2) * (1 << controller->h_pow2) * sizeof(float)); fb_alloc0(2 * (1 << controller->w_pow2) * (1 << controller->h_pow2) * sizeof(float));
} }
void dealloc_fft2d_buffer() void fft2d_dealloc()
{ {
fb_free(); fb_free();
} }
@ -453,7 +455,7 @@ void dealloc_fft2d_buffer()
// RGB565 to YUV conversion // RGB565 to YUV conversion
extern const int8_t yuv_table[196608]; extern const int8_t yuv_table[196608];
void do_2dfft(fft2d_controller_t *controller) void fft2d_run(fft2d_controller_t *controller)
{ {
// This section copies image data into the fft buffer. It takes care of // This section copies image data into the fft buffer. It takes care of
// extracting the grey channel from RGB images if necessary. The code // extracting the grey channel from RGB images if necessary. The code
@ -472,11 +474,11 @@ void do_2dfft(fft2d_controller_t *controller)
} }
// Do FFT on image data and copy to main buffer. // Do FFT on image data and copy to main buffer.
fft1d_controller_t fft1d_controller_i; fft1d_controller_t fft1d_controller_i;
alloc_fft1d_buffer(&fft1d_controller_i, tmp, controller->r.w); fft1d_alloc(&fft1d_controller_i, tmp, controller->r.w);
do_1dfft(&fft1d_controller_i); fft1d_run(&fft1d_controller_i);
memcpy(controller->data + (i * (2 << controller->w_pow2)), memcpy(controller->data + (i * (2 << controller->w_pow2)),
fft1d_controller_i.data, (2 << fft1d_controller_i.pow2) * sizeof(float)); fft1d_controller_i.data, (2 << fft1d_controller_i.pow2) * sizeof(float));
dealloc_fft1d_buffer(); fft1d_dealloc();
// Free image data buffer. // Free image data buffer.
fb_free(); fb_free();
} }
@ -491,7 +493,7 @@ void do_2dfft(fft2d_controller_t *controller)
} }
} }
void do_2difft(fft2d_controller_t *controller) void ifft2d_run(fft2d_controller_t *controller)
{ {
// Do columns... // Do columns...
for (int i = 0, ii = (2 << controller->w_pow2); i < ii; i += 2) { for (int i = 0, ii = (2 << controller->w_pow2); i < ii; i += 2) {
@ -502,10 +504,10 @@ void do_2difft(fft2d_controller_t *controller)
} }
// Do rows... // Do rows...
for (int i = 0, ii = (1 << controller->h_pow2); i < ii; i++) { for (int i = 0; i < controller->r.h; i++) {
fft1d_controller_t fft1d_controller_i; fft1d_controller_t fft1d_controller_i;
fft1d_controller_i.pow2 = controller->w_pow2; fft1d_controller_i.pow2 = controller->w_pow2;
fft1d_controller_i.data = controller->data + (i * (2 << controller->w_pow2)); fft1d_controller_i.data = controller->data + (i * (2 << controller->w_pow2));
do_1difft(&fft1d_controller_i); ifft1d_run(&fft1d_controller_i);
} }
} }

View File

@ -16,18 +16,18 @@ typedef struct fft1d_controller {
int pow2; int pow2;
float *data; float *data;
} fft1d_controller_t; } fft1d_controller_t;
void alloc_fft1d_buffer(fft1d_controller_t *controller, uint8_t *data, int len); void fft1d_alloc(fft1d_controller_t *controller, uint8_t *buf, int len);
void dealloc_fft1d_buffer(); void fft1d_dealloc();
void do_1dfft(fft1d_controller_t *controller); void fft1d_run(fft1d_controller_t *controller);
void do_1difft(fft1d_controller_t *controller); void ifft1d_run(fft1d_controller_t *controller);
typedef struct fft2d_controller { typedef struct fft2d_controller {
image_t *img; image_t *img;
rectangle_t r; rectangle_t r;
int w_pow2, h_pow2; int w_pow2, h_pow2;
float *data; float *data;
} fft2d_controller_t; } fft2d_controller_t;
void alloc_fft2d_buffer(fft2d_controller_t *controller, image_t *img, rectangle_t *r); void fft2d_alloc(fft2d_controller_t *controller, image_t *img, rectangle_t *r);
void dealloc_fft2d_buffer(); void fft2d_dealloc();
void do_2dfft(fft2d_controller_t *controller); void fft2d_run(fft2d_controller_t *controller);
void do_2difft(fft2d_controller_t *controller); void ifft2d_run(fft2d_controller_t *controller);
#endif /* __FFT_H__ */ #endif /* __FFT_H__ */

View File

@ -24,11 +24,11 @@ void imlib_phasecorrelate(image_t *img0, image_t *img1, int *x_offset, int *y_of
roi1.w = img1->w; roi1.w = img1->w;
roi1.h = img1->h; roi1.h = img1->h;
alloc_fft2d_buffer(&fft0, img0, &roi0); fft2d_alloc(&fft0, img0, &roi0);
alloc_fft2d_buffer(&fft1, img1, &roi1); fft2d_alloc(&fft1, img1, &roi1);
do_2dfft(&fft0); fft2d_run(&fft0);
do_2dfft(&fft1); fft2d_run(&fft1);
int w = (1 << fft0.w_pow2); int w = (1 << fft0.w_pow2);
int h = (1 << fft0.h_pow2); int h = (1 << fft0.h_pow2);
@ -45,17 +45,17 @@ void imlib_phasecorrelate(image_t *img0, image_t *img1, int *x_offset, int *y_of
fft0.data[i+1] = hp_i / mag; fft0.data[i+1] = hp_i / mag;
} }
do_2difft(&fft0); ifft2d_run(&fft0);
float max = 0; float max = 0;
int off_x = 0; int off_x = 0;
int off_y = 0; int off_y = 0;
for (int i = 0; i < img0->h; i++) { for (int i = 0; i < img0->h; i++) {
for (int j = 0; j < img0->w; j++) { for (int j = 0; j < img0->w; j++) {
int index = ((i * img0->w) + j) * 2; // Note that the output of the FFT is packed with real data in both
float f_r = fft0.data[index+0]; // the real and imaginary parts...
// float f_i = fft0.data[index+1]; int index = (i * (w * 2)) + j; // correct!
// float mag = fast_sqrtf((f_r*f_r)+(f_i*f_i)); float f_r = fft0.data[index];
if (f_r > max) { if (f_r > max) {
max = f_r; max = f_r;
off_x = j; off_x = j;
@ -65,17 +65,17 @@ void imlib_phasecorrelate(image_t *img0, image_t *img1, int *x_offset, int *y_of
} }
if (off_x > (img0->w/2)) { if (off_x > (img0->w/2)) {
*x_offset = off_x - img0->w; *x_offset = img0->w - off_x;
} else { } else {
*x_offset = off_x; *x_offset = -off_x;
} }
if (off_y > (img0->h/2)) { if (off_y > (img0->h/2)) {
*y_offset = off_y - img0->h; *y_offset = img0->h - off_y;
} else { } else {
*y_offset = off_y; *y_offset = -off_y;
} }
dealloc_fft2d_buffer(); // fft1 fft2d_dealloc(); // fft1
dealloc_fft2d_buffer(); // fft0 fft2d_dealloc(); // fft0
} }