From 15aed91efdd3ff407f61d0079fc228af972231b2 Mon Sep 17 00:00:00 2001 From: "Kwabena W. Agyeman" Date: Tue, 14 Jun 2016 00:00:07 -0400 Subject: [PATCH] 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. --- src/omv/img/fft.c | 40 ++++++++++++++++++---------------- src/omv/img/fft.h | 16 +++++++------- src/omv/img/phasecorrelation.c | 30 ++++++++++++------------- 3 files changed, 44 insertions(+), 42 deletions(-) diff --git a/src/omv/img/fft.c b/src/omv/img/fft.c index 7fb0ed943..af74f94a4 100644 --- a/src/omv/img/fft.c +++ b/src/omv/img/fft.c @@ -382,33 +382,35 @@ static void do_ifft(float *inout, int N_pow2, int stride) } } } + float div = 1.0 / (N >> 1); - for (int i = 0; i < N; i++) { - inout[i] *= div; + for (int i = 0; i < N; i += 2) { + 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->pow2 = int_clog2(len); controller->data = fb_alloc((2 << controller->pow2) * sizeof(float)); } -void dealloc_fft1d_buffer() +void fft1d_dealloc() { 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 // 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, h_buffer, controller->pow2 - 1); do_fft(h_buffer, controller->pow2 - 1, 1); @@ -416,24 +418,24 @@ void do_1dfft(fft1d_controller_t *controller) 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 // 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); prepare_complex_input(h_buffer, h_buffer, controller->pow2 - 1, 1); do_ifft(h_buffer, controller->pow2 - 1, 1); 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(); } /////////////////////////////////////////////////////////////////////////////// -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; 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)); } -void dealloc_fft2d_buffer() +void fft2d_dealloc() { fb_free(); } @@ -453,7 +455,7 @@ void dealloc_fft2d_buffer() // RGB565 to YUV conversion 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 // 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. fft1d_controller_t fft1d_controller_i; - alloc_fft1d_buffer(&fft1d_controller_i, tmp, controller->r.w); - do_1dfft(&fft1d_controller_i); + fft1d_alloc(&fft1d_controller_i, tmp, controller->r.w); + fft1d_run(&fft1d_controller_i); memcpy(controller->data + (i * (2 << controller->w_pow2)), fft1d_controller_i.data, (2 << fft1d_controller_i.pow2) * sizeof(float)); - dealloc_fft1d_buffer(); + fft1d_dealloc(); // Free image data buffer. 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... 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... - 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_i.pow2 = controller->w_pow2; fft1d_controller_i.data = controller->data + (i * (2 << controller->w_pow2)); - do_1difft(&fft1d_controller_i); + ifft1d_run(&fft1d_controller_i); } } diff --git a/src/omv/img/fft.h b/src/omv/img/fft.h index b7784c2a8..7eee92f89 100644 --- a/src/omv/img/fft.h +++ b/src/omv/img/fft.h @@ -16,18 +16,18 @@ typedef struct fft1d_controller { int pow2; float *data; } fft1d_controller_t; -void alloc_fft1d_buffer(fft1d_controller_t *controller, uint8_t *data, int len); -void dealloc_fft1d_buffer(); -void do_1dfft(fft1d_controller_t *controller); -void do_1difft(fft1d_controller_t *controller); +void fft1d_alloc(fft1d_controller_t *controller, uint8_t *buf, int len); +void fft1d_dealloc(); +void fft1d_run(fft1d_controller_t *controller); +void ifft1d_run(fft1d_controller_t *controller); typedef struct fft2d_controller { image_t *img; rectangle_t r; int w_pow2, h_pow2; float *data; } fft2d_controller_t; -void alloc_fft2d_buffer(fft2d_controller_t *controller, image_t *img, rectangle_t *r); -void dealloc_fft2d_buffer(); -void do_2dfft(fft2d_controller_t *controller); -void do_2difft(fft2d_controller_t *controller); +void fft2d_alloc(fft2d_controller_t *controller, image_t *img, rectangle_t *r); +void fft2d_dealloc(); +void fft2d_run(fft2d_controller_t *controller); +void ifft2d_run(fft2d_controller_t *controller); #endif /* __FFT_H__ */ diff --git a/src/omv/img/phasecorrelation.c b/src/omv/img/phasecorrelation.c index c04cd63b4..5e34f3c37 100644 --- a/src/omv/img/phasecorrelation.c +++ b/src/omv/img/phasecorrelation.c @@ -24,11 +24,11 @@ void imlib_phasecorrelate(image_t *img0, image_t *img1, int *x_offset, int *y_of roi1.w = img1->w; roi1.h = img1->h; - alloc_fft2d_buffer(&fft0, img0, &roi0); - alloc_fft2d_buffer(&fft1, img1, &roi1); + fft2d_alloc(&fft0, img0, &roi0); + fft2d_alloc(&fft1, img1, &roi1); - do_2dfft(&fft0); - do_2dfft(&fft1); + fft2d_run(&fft0); + fft2d_run(&fft1); int w = (1 << fft0.w_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; } - do_2difft(&fft0); + ifft2d_run(&fft0); float max = 0; int off_x = 0; int off_y = 0; for (int i = 0; i < img0->h; i++) { for (int j = 0; j < img0->w; j++) { - int index = ((i * img0->w) + j) * 2; - float f_r = fft0.data[index+0]; - // float f_i = fft0.data[index+1]; - // float mag = fast_sqrtf((f_r*f_r)+(f_i*f_i)); + // Note that the output of the FFT is packed with real data in both + // the real and imaginary parts... + int index = (i * (w * 2)) + j; // correct! + float f_r = fft0.data[index]; if (f_r > max) { max = f_r; 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)) { - *x_offset = off_x - img0->w; + *x_offset = img0->w - off_x; } else { - *x_offset = off_x; + *x_offset = -off_x; } if (off_y > (img0->h/2)) { - *y_offset = off_y - img0->h; + *y_offset = img0->h - off_y; } else { - *y_offset = off_y; + *y_offset = -off_y; } - dealloc_fft2d_buffer(); // fft1 - dealloc_fft2d_buffer(); // fft0 + fft2d_dealloc(); // fft1 + fft2d_dealloc(); // fft0 }