openmv/lib/imlib/phasecorrelation.c
iabdalkader daf2bb30da misc: Restructure repo.
Signed-off-by: iabdalkader <i.abdalkader@gmail.com>
2025-04-13 08:28:34 +02:00

654 lines
24 KiB
C

/*
* 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