/* * SPDX-License-Identifier: MIT * * Copyright (C) 2013-2024 OpenMV, LLC. * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. * * Phase correlation. */ #include "imlib.h" #include "fft.h" void imlib_logpolar_int(image_t *dst, image_t *src, rectangle_t *roi, bool linear, bool reverse) { int w = roi->w; // == dst_w int h = roi->h; // == dst_h int w_2 = w / 2; int h_2 = h / 2; float rho_scale = fast_sqrtf((w_2 * w_2) + (h_2 * h_2)); if (!linear) { rho_scale = fast_log(rho_scale); } const float m_pi_1_5 = 1.5f * M_PI; const float m_pi_1_5_d = IM_RAD2DEG(m_pi_1_5); const float m_pi_2_0 = 2.0f * M_PI; const float m_pi_2_0_d = IM_RAD2DEG(m_pi_2_0); const int m_pi_2_0_d_i = m_pi_2_0_d; float theta_scale_d = m_pi_2_0_d / (w - 2); float theta_scale_inv = w / m_pi_2_0; if (!reverse) { rho_scale /= h; switch (src->pixfmt) { case PIXFORMAT_BINARY: { uint32_t *tmp = (uint32_t *) src->data; int tmp_w = src->w, tmp_h = src->h, tmp_x = roi->x + w_2 - 1, tmp_y = roi->y + h_2; for (int y = 0, yy = h; y < yy; y++) { uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(dst, y); float rho = y * rho_scale; if (!linear) { rho = fast_expf(rho); } for (int x = 0, xx = w_2; x < xx; x++) { int theta = fast_roundf(m_pi_1_5_d - (x * theta_scale_d)); if (theta < 0) { theta += m_pi_2_0_d_i; // wrap for table access } int sourceX = tmp_x + fast_roundf(rho * cos_table[theta]); // rounding is necessary int sourceY = tmp_y + fast_roundf(rho * sin_table[theta]); // rounding is necessary if ((0 <= sourceX) && (0 <= sourceY) && (sourceY < tmp_h)) { // plot the 2 symmetrical pixels uint32_t *ptr, pixel; ptr = tmp + (((tmp_w + UINT32_T_MASK) >> UINT32_T_SHIFT) * sourceY); pixel = IMAGE_GET_BINARY_PIXEL_FAST(ptr, sourceX); IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr, x, pixel); pixel = IMAGE_GET_BINARY_PIXEL_FAST(ptr, tmp_w - 1 - sourceX); IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr, w - 1 - x, pixel); } } } break; } case PIXFORMAT_GRAYSCALE: { uint8_t *tmp = (uint8_t *) src->data; int tmp_w = src->w, tmp_h = src->h, tmp_x = roi->x + w_2 - 1, tmp_y = roi->y + h_2; for (int y = 0, yy = h; y < yy; y++) { uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(dst, y); float rho = y * rho_scale; if (!linear) { rho = fast_expf(rho); } for (int x = 0, xx = w_2; x < xx; x++) { int theta = fast_roundf(m_pi_1_5_d - (x * theta_scale_d)); if (theta < 0) { theta += m_pi_2_0_d_i; // wrap for table access } int sourceX = tmp_x + fast_roundf(rho * cos_table[theta]); // rounding is necessary int sourceY = tmp_y + fast_roundf(rho * sin_table[theta]); // rounding is necessary if ((0 <= sourceX) && (0 <= sourceY) && (sourceY < tmp_h)) { // plot the 2 symmetrical pixels uint8_t *ptr, pixel; ptr = tmp + (tmp_w * sourceY); pixel = ptr[sourceX]; row_ptr[x] = pixel; pixel = ptr[tmp_w - 1 - sourceX]; row_ptr[w - 1 - x] = pixel; } } } break; } case PIXFORMAT_RGB565: { uint16_t *tmp = (uint16_t *) src->data; int tmp_w = src->w, tmp_h = src->h, tmp_x = roi->x + w_2 - 1, tmp_y = roi->y + h_2; for (int y = 0, yy = h; y < yy; y++) { uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(dst, y); float rho = y * rho_scale; if (!linear) { rho = fast_expf(rho); } for (int x = 0, xx = w_2; x < xx; x++) { int theta = fast_roundf(m_pi_1_5_d - (x * theta_scale_d)); if (theta < 0) { theta += m_pi_2_0_d_i; // wrap for table access } int sourceX = tmp_x + fast_roundf(rho * cos_table[theta]); // rounding is necessary int sourceY = tmp_y + fast_roundf(rho * sin_table[theta]); // rounding is necessary if ((0 <= sourceX) && (0 <= sourceY) && (sourceY < tmp_h)) { // plot the 2 symmetrical pixels uint16_t *ptr, pixel; ptr = tmp + (tmp_w * sourceY); pixel = ptr[sourceX]; row_ptr[x] = pixel; pixel = ptr[tmp_w - 1 - sourceX]; row_ptr[w - 1 - x] = pixel; } } } break; } default: { break; } } } else { float rho_scale_inv = (h - 1) / rho_scale; switch (src->pixfmt) { case PIXFORMAT_BINARY: { uint32_t *tmp = (uint32_t *) src->data; int tmp_w = src->w, tmp_x = roi->x, tmp_y = roi->y; for (int y = 0, yy = h; y < yy; y++) { uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(dst, y); int y_2 = y - h_2; int y_2_2 = y_2 * y_2; for (int x = 0, xx = w_2; x < xx; x++) { int x_2 = x - w_2; int x_2_2 = x_2 * x_2; float rho = fast_sqrtf(x_2_2 + y_2_2); if (!linear) { rho = fast_log(rho); } float theta = m_pi_1_5 - fast_atan2f(y_2, x_2); int sourceX = tmp_x + fast_roundf(theta * theta_scale_inv); // rounding is necessary int sourceY = tmp_y + fast_roundf(rho * rho_scale_inv); // rounding is necessary // plot the 2 symmetrical pixels uint32_t *ptr, pixel; ptr = tmp + (((tmp_w + UINT32_T_MASK) >> UINT32_T_SHIFT) * sourceY); pixel = IMAGE_GET_BINARY_PIXEL_FAST(ptr, sourceX); IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr, x, pixel); pixel = IMAGE_GET_BINARY_PIXEL_FAST(ptr, tmp_w - 1 - sourceX); IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr, w - 1 - x, pixel); } } break; } case PIXFORMAT_GRAYSCALE: { uint8_t *tmp = (uint8_t *) src->data; int tmp_w = src->w, tmp_x = roi->x, tmp_y = roi->y; for (int y = 0, yy = h; y < yy; y++) { uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(dst, y); int y_2 = y - h_2; int y_2_2 = y_2 * y_2; for (int x = 0, xx = w_2; x < xx; x++) { int x_2 = x - w_2; int x_2_2 = x_2 * x_2; float rho = fast_sqrtf(x_2_2 + y_2_2); if (!linear) { rho = fast_log(rho); } float theta = m_pi_1_5 - fast_atan2f(y_2, x_2); int sourceX = tmp_x + fast_roundf(theta * theta_scale_inv); // rounding is necessary int sourceY = tmp_y + fast_roundf(rho * rho_scale_inv); // rounding is necessary // plot the 2 symmetrical pixels uint8_t *ptr, pixel; ptr = tmp + (tmp_w * sourceY); pixel = ptr[sourceX]; row_ptr[x] = pixel; pixel = ptr[tmp_w - 1 - sourceX]; row_ptr[w - 1 - x] = pixel; } } break; } case PIXFORMAT_RGB565: { uint16_t *tmp = (uint16_t *) src->data; int tmp_w = src->w, tmp_x = roi->x, tmp_y = roi->y; for (int y = 0, yy = h; y < yy; y++) { uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(dst, y); int y_2 = y - h_2; int y_2_2 = y_2 * y_2; for (int x = 0, xx = w_2; x < xx; x++) { int x_2 = x - w_2; int x_2_2 = x_2 * x_2; float rho = fast_sqrtf(x_2_2 + y_2_2); if (!linear) { rho = fast_log(rho); } float theta = m_pi_1_5 - fast_atan2f(y_2, x_2); int sourceX = tmp_x + fast_roundf(theta * theta_scale_inv); // rounding is necessary int sourceY = tmp_y + fast_roundf(rho * rho_scale_inv); // rounding is necessary // plot the 2 symmetrical pixels uint16_t *ptr, pixel; ptr = tmp + (tmp_w * sourceY); pixel = ptr[sourceX]; row_ptr[x] = pixel; pixel = ptr[tmp_w - 1 - sourceX]; row_ptr[w - 1 - x] = pixel; } } break; } default: { break; } } } } #if defined(IMLIB_ENABLE_LOGPOLAR) || defined(IMLIB_ENABLE_LINPOLAR) void imlib_logpolar(image_t *img, bool linear, bool reverse) { image_t img_2; img_2.w = img->w; img_2.h = img->h; img_2.pixfmt = img->pixfmt; rectangle_t rect; rect.x = 0; rect.y = 0; rect.w = img->w; rect.h = img->h; size_t size = image_size(img); img_2.data = fb_alloc(size, FB_ALLOC_NO_HINT); memcpy(img_2.data, img->data, size); memset(img->data, 0, size); imlib_logpolar_int(img, &img_2, &rect, linear, reverse); fb_free(); } #endif //defined(IMLIB_ENABLE_LOGPOLAR) || defined(IMLIB_ENABLE_LINPOLAR) #ifdef IMLIB_ENABLE_FIND_DISPLACEMENT // Note that both ROI widths and heights must be equal. void imlib_phasecorrelate(image_t *img0, image_t *img1, rectangle_t *roi0, rectangle_t *roi1, bool logpolar, bool fix_rotation_scale, float *x_translation, float *y_translation, float *rotation, float *scale, float *response) { // Step 1 - Get Rotation/Scale Differences if ((!logpolar) && fix_rotation_scale) { fft2d_controller_t fft0, fft1; fft2d_alloc(&fft0, img0, roi0); fft2d_alloc(&fft1, img1, roi1); fft2d_run(&fft0); fft2d_run(&fft1); fft2d_mag(&fft0); fft2d_mag(&fft1); fft2d_swap(&fft0); fft2d_swap(&fft1); fft2d_logpolar(&fft0); fft2d_logpolar(&fft1); fft2d_run_again(&fft0); fft2d_run_again(&fft1); int w = (1 << fft0.w_pow2); int h = (1 << fft0.h_pow2); for (int i = 0, j = h * w * 2; i < j; i += 2) { float ga_r = fft0.data[i + 0]; float ga_i = fft0.data[i + 1]; float gb_r = fft1.data[i + 0]; float gb_i = -fft1.data[i + 1]; // complex conjugate... float hp_r = (ga_r * gb_r) - (ga_i * gb_i); // hadamard product float hp_i = (ga_r * gb_i) + (ga_i * gb_r); // hadamard product float mag = 1 / fast_sqrtf((hp_r * hp_r) + (hp_i * hp_i)); // magnitude // Replace first fft with phase correlation... fft0.data[i + 0] = hp_r * mag; fft0.data[i + 1] = hp_i * mag; } ifft2d_run(&fft0); float sum = 0; float max = 0; int off_x = 0; int off_y = 0; for (int i = 0; i < h; i++) { for (int j = 0; j < w; j++) { // Note that the output of the FFT is packed with real data in both // the real and imaginary parts... (right side of the array is zero). float f_r = fft0.data[(i * w * 2) + j]; sum += f_r; if (f_r > max) { max = f_r; off_x = j; off_y = i; } } } float tmp_response = max / sum; // normalize this to [0:1]. float f_sum = 0; float f_off_x = 0; float f_off_y = 0; for (int i = -2; i < 2; i++) { for (int j = -2; j < 2; j++) { // Wrap around int new_x = off_x + j; if (new_x < 0) { new_x += w; } if (new_x >= w) { new_x -= w; } // Wrap around int new_y = off_y + i; if (new_y < 0) { new_y += h; } if (new_y >= h) { new_y -= h; } // Compute centroid. float f_r = fft0.data[(new_y * w * 2) + new_x]; f_off_x += (off_x + j) * f_r; // don't use new_x here f_off_y += (off_y + i) * f_r; // don't use new_y here f_sum += f_r; } } f_off_x /= f_sum; f_off_y /= f_sum; // FFT Shift X if (f_off_x >= (w / 2.0f)) { f_off_x = f_off_x - w; } else { f_off_x = f_off_x; } // FFT Shift Y if (f_off_y >= (h / 2.0f)) { f_off_y = -(f_off_y - h); } else { f_off_y = -f_off_y; } if ((f_off_x < (-w / 2.0f)) || ((w / 2.0f) <= f_off_x) || (f_off_y < (-h / 2.0f)) || ((h / 2.0f) <= f_off_y) || isnanf(f_off_x) || isinff(f_off_x) || isnanf(f_off_y) || isinff(f_off_y) || isnanf(tmp_response) || isinff(tmp_response)) { // Noise Filter f_off_x = 0; f_off_y = 0; tmp_response = 0; } fft2d_dealloc(); // fft1 fft2d_dealloc(); // fft0 float w_2 = roi0->w / 2.0f; float h_2 = roi0->h / 2.0f; float rho_scale = fast_log(fast_sqrtf((w_2 * w_2) + (h_2 * h_2))) / roi0->h; float theta_scale = (2 * M_PI) / roi0->w; *rotation = f_off_x * theta_scale; *scale = (f_off_y * rho_scale) + 1; } else { *rotation = 0; *scale = 0; } image_t img0_fixed; rectangle_t roi0_fixed; // Step 2 - Fix Rotation/Scale Differences if ((!logpolar) && fix_rotation_scale) { img0_fixed.w = roi0->w; img0_fixed.h = roi0->h; img0_fixed.pixfmt = img0->pixfmt; img0_fixed.pixels = fb_alloc(image_size(&img0_fixed), FB_ALLOC_NO_HINT); roi0_fixed.x = 0; roi0_fixed.y = 0; roi0_fixed.w = roi0->w; roi0_fixed.h = roi0->h; switch (img0->pixfmt) { case PIXFORMAT_BINARY: { for (int y = roi0->y, yy = roi0->y + roi0->h; y < yy; y++) { uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(img0, y); for (int x = roi0->x, xx = roi0->x + roi0->w; x < xx; x++) { IMAGE_PUT_BINARY_PIXEL(&img0_fixed, x, y, IMAGE_GET_BINARY_PIXEL_FAST(row_ptr, x)); } } break; } case PIXFORMAT_GRAYSCALE: { for (int y = roi0->y, yy = roi0->y + roi0->h; y < yy; y++) { uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(img0, y); for (int x = roi0->x, xx = roi0->x + roi0->w; x < xx; x++) { IMAGE_PUT_GRAYSCALE_PIXEL(&img0_fixed, x, y, IMAGE_GET_GRAYSCALE_PIXEL_FAST(row_ptr, x)); } } break; } case PIXFORMAT_RGB565: { for (int y = roi0->y, yy = roi0->y + roi0->h; y < yy; y++) { uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(img0, y); for (int x = roi0->x, xx = roi0->x + roi0->w; x < xx; x++) { IMAGE_PUT_RGB565_PIXEL(&img0_fixed, x, y, IMAGE_GET_RGB565_PIXEL_FAST(row_ptr, x)); } } break; } default: { memset(img0_fixed.data, 0, image_size(&img0_fixed)); break; } } imlib_rotation_corr(&img0_fixed, 0, 0, *rotation, 0, 0, *scale, 60, NULL); } else { memcpy(&img0_fixed, img0, sizeof(image_t)); memcpy(&roi0_fixed, roi0, sizeof(rectangle_t)); } // Step 3 - Get Translation Differences { image_t img0alt, img1alt; rectangle_t roi0alt, roi1alt; if (logpolar) { img0alt.w = roi0_fixed.w; img0alt.h = roi0_fixed.h; img0alt.pixfmt = img0_fixed.pixfmt; img0alt.data = fb_alloc0(image_size(&img0alt), FB_ALLOC_NO_HINT); imlib_logpolar_int(&img0alt, &img0_fixed, &roi0_fixed, false, false); roi0alt.x = 0; roi0alt.y = 0; roi0alt.w = roi0_fixed.w; roi0alt.h = roi0_fixed.h; img1alt.w = roi1->w; img1alt.h = roi1->h; img1alt.pixfmt = img1->pixfmt; img1alt.data = fb_alloc0(image_size(&img1alt), FB_ALLOC_NO_HINT); imlib_logpolar_int(&img1alt, img1, roi1, false, false); roi1alt.x = 0; roi1alt.y = 0; roi1alt.w = roi1->w; roi1alt.h = roi1->h; } fft2d_controller_t fft0, fft1; fft2d_alloc(&fft0, logpolar ? &img0alt : &img0_fixed, logpolar ? &roi0alt : &roi0_fixed); fft2d_alloc(&fft1, logpolar ? &img1alt : img1, logpolar ? &roi1alt : roi1); fft2d_run(&fft0); fft2d_run(&fft1); int w = (1 << fft0.w_pow2); int h = (1 << fft0.h_pow2); for (int i = 0, j = h * w * 2; i < j; i += 2) { float ga_r = fft0.data[i + 0]; float ga_i = fft0.data[i + 1]; float gb_r = fft1.data[i + 0]; float gb_i = -fft1.data[i + 1]; // complex conjugate... float hp_r = (ga_r * gb_r) - (ga_i * gb_i); // hadamard product float hp_i = (ga_r * gb_i) + (ga_i * gb_r); // hadamard product float mag = 1 / fast_sqrtf((hp_r * hp_r) + (hp_i * hp_i)); // magnitude fft0.data[i + 0] = hp_r * mag; fft0.data[i + 1] = hp_i * mag; } ifft2d_run(&fft0); float sum = 0; float max = 0; int off_x = 0; int off_y = 0; for (int i = 0; i < h; i++) { for (int j = 0; j < w; j++) { // Note that the output of the FFT is packed with real data in both // the real and imaginary parts... (right side of the array is zero). float f_r = fft0.data[(i * w * 2) + j]; sum += f_r; if (f_r > max) { max = f_r; off_x = j; off_y = i; } } } *response = max / sum; // normalize this to [0:1]. float f_sum = 0; float f_off_x = 0; float f_off_y = 0; for (int i = -2; i < 2; i++) { for (int j = -2; j < 2; j++) { // Wrap around int new_x = off_x + j; if (new_x < 0) { new_x += w; } if (new_x >= w) { new_x -= w; } // Wrap around int new_y = off_y + i; if (new_y < 0) { new_y += h; } if (new_y >= h) { new_y -= h; } // Compute centroid. float f_r = fft0.data[(new_y * w * 2) + new_x]; f_off_x += (off_x + j) * f_r; // don't use new_x here f_off_y += (off_y + i) * f_r; // don't use new_y here f_sum += f_r; } } f_off_x /= f_sum; f_off_y /= f_sum; // FFT Shift X if (f_off_x >= (w / 2.0f)) { *x_translation = f_off_x - w; } else { *x_translation = f_off_x; } // FFT Shift Y if (f_off_y >= (h / 2.0f)) { *y_translation = -(f_off_y - h); } else { *y_translation = -f_off_y; } if ((*x_translation < (-w / 2.0f)) || ((w / 2.0f) <= *x_translation) || (*y_translation < (-h / 2.0f)) || ((h / 2.0f) <= *y_translation) || isnanf(*x_translation) || isinff(*x_translation) || isnanf(*y_translation) || isinff(*y_translation) || isnanf(*response) || isinff(*response)) { // Noise Filter *x_translation = 0; *y_translation = 0; *response = 0; } fft2d_dealloc(); // fft1 fft2d_dealloc(); // fft0 if (logpolar) { fb_free(); // img1alt fb_free(); // img0alt float w_2 = roi0->w / 2.0f; float h_2 = roi0->h / 2.0f; float rho_scale = fast_log(fast_sqrtf((w_2 * w_2) + (h_2 * h_2))) / roi0->h; float theta_scale = (2 * M_PI) / roi0->w; *rotation = *x_translation * theta_scale; *scale = (*y_translation * rho_scale) + 1; *x_translation = 0; *y_translation = 0; } } if ((!logpolar) && fix_rotation_scale) { fb_free(); } } #endif //IMLIB_ENABLE_FIND_DISPLACEMENT