Add offset correction to imlib lens correction. (#791)

* Add x/y correction to lens correction to adjust for lens offsets.
This commit is contained in:
Matt Dawson 2020-05-14 07:10:58 +12:00 committed by GitHub
parent 63ea06d75d
commit 5ec2e7b310
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 135 additions and 66 deletions

View File

@ -1200,54 +1200,89 @@ void imlib_zero(image_t *img, image_t *mask, bool invert)
#ifdef IMLIB_ENABLE_LENS_CORR
// A simple algorithm for correcting lens distortion.
// See http://www.tannerhelland.com/4743/simple-algorithm-correcting-lens-distortion/
void imlib_lens_corr(image_t *img, float strength, float zoom)
void imlib_lens_corr(image_t *img, float strength, float zoom, float x_corr, float y_corr)
{
int w = img->w;
int h = img->h;
int halfWidth = w / 2;
int halfHeight = h / 2;
float lens_corr_radius = strength / fast_sqrtf((w * w) + (h * h));
float maximum_diameter = fast_sqrtf((w * w) + (h * h));
float lens_corr_diameter = strength / maximum_diameter;
zoom = 1 / zoom;
// Convert percentage offset to pixels from center of image
int x_off = w * x_corr;
int y_off = h * y_corr;
// Create a tmp copy of the image to pull pixels from.
size_t size = image_size(img);
void *data = fb_alloc(size, FB_ALLOC_NO_HINT);
memcpy(data, img->data, size);
memset(img->data, 0, size);
int maximum_radius = fast_ceilf(maximum_diameter / 2) + 1; // +1 inclusive of final value
float *precalculated_table = fb_alloc(maximum_radius * sizeof(float), FB_ALLOC_NO_HINT);
for(int i=0; i < maximum_radius; i++) {
float r = lens_corr_diameter * i;
precalculated_table[i] = (fast_atanf(r) / r) * zoom;
}
int down_adj = halfHeight + y_off;
int up_adj = h - 1 - halfHeight + y_off;
int right_adj = halfWidth + x_off;
int left_adj = w - 1 - halfWidth + x_off;
switch(img->bpp) {
case IMAGE_BPP_BINARY: {
uint32_t *tmp = (uint32_t *) data;
for (int y = 0, yy = halfHeight; y < yy; y++) {
for (int y = 0; y < halfHeight; y++) {
uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(img, y);
uint32_t *row_ptr2 = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(img, h-1-y);
int newY = y - halfHeight;
int newY2 = newY * newY;
float zoomedY = newY * zoom;
for (int x = 0, xx = halfWidth; x < xx; x++) {
for (int x = 0; x < halfWidth; x++) {
int newX = x - halfWidth;
int newX2 = newX * newX;
float zoomedX = newX * zoom;
float precalculated = precalculated_table[(int)fast_sqrtf(newX2 + newY2)];
int sourceY = fast_roundf(precalculated * newY); // rounding is necessary
int sourceX = fast_roundf(precalculated * newX); // rounding is necessary
int sourceY_down = down_adj + sourceY;
int sourceY_up = up_adj - sourceY;
int sourceX_right = right_adj + sourceX;
int sourceX_left = left_adj - sourceX;
float r = lens_corr_radius * fast_sqrtf(newX2 + newY2);
float theta = fast_atanf(r) / r; // r is never 0
int sourceX = halfWidth + fast_roundf(theta * zoomedX); // rounding is necessary
int sourceY = halfHeight + fast_roundf(theta * zoomedY); // rounding is necessary
// plot the 4 symmetrical pixels
// top 2 pixels
if (sourceY_down >= 0 && sourceY_down < h) {
uint32_t *ptr = tmp + (((w + UINT32_T_MASK) >> UINT32_T_SHIFT) * sourceY_down);
if ((0 <= sourceX) && (0 <= sourceY)) { // plot the 4 symmetrical pixels
uint32_t *ptr, pixel;
ptr = 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, w-1-sourceX);
IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr, w-1-x, pixel);
ptr = tmp + (((w + UINT32_T_MASK) >> UINT32_T_SHIFT) * (h-1-sourceY));
pixel = IMAGE_GET_BINARY_PIXEL_FAST(ptr, sourceX);
IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr2, x, pixel);
pixel = IMAGE_GET_BINARY_PIXEL_FAST(ptr, w-1-sourceX);
IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr2, w-1-x, pixel);
if (sourceX_right >= 0 && sourceX_right < w) {
uint8_t pixel = IMAGE_GET_BINARY_PIXEL_FAST(ptr, sourceX_right);
IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr, x, pixel);
}
if (sourceX_left >= 0 && sourceX_left < w) {
uint8_t pixel = IMAGE_GET_BINARY_PIXEL_FAST(ptr, sourceX_left);
IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr, w - 1 - x, pixel);
}
}
// bottom 2 pixels
if (sourceY_up >= 0 && sourceY_up < h) {
uint32_t *ptr = tmp + (((w + UINT32_T_MASK) >> UINT32_T_SHIFT) * sourceY_up);
if (sourceX_right >= 0 && sourceX_right < w) {
uint8_t pixel = IMAGE_GET_BINARY_PIXEL_FAST(ptr, sourceX_right);
IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr2, x, pixel);
}
if (sourceX_left >= 0 && sourceX_left < w) {
uint8_t pixel = IMAGE_GET_BINARY_PIXEL_FAST(ptr, sourceX_left);
IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr2, w - 1 - x, pixel);
}
}
}
}
@ -1256,35 +1291,48 @@ void imlib_lens_corr(image_t *img, float strength, float zoom)
case IMAGE_BPP_GRAYSCALE: {
uint8_t *tmp = (uint8_t *) data;
for (int y = 0, yy = halfHeight; y < yy; y++) {
for (int y = 0; y < halfHeight; y++) {
uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(img, y);
uint8_t *row_ptr2 = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(img, h-1-y);
int newY = y - halfHeight;
int newY2 = newY * newY;
float zoomedY = newY * zoom;
for (int x = 0, xx = halfWidth; x < xx; x++) {
for (int x = 0; x < halfWidth; x++) {
int newX = x - halfWidth;
int newX2 = newX * newX;
float zoomedX = newX * zoom;
float precalculated = precalculated_table[(int)fast_sqrtf(newX2 + newY2)];
int sourceY = fast_roundf(precalculated * newY); // rounding is necessary
int sourceX = fast_roundf(precalculated * newX); // rounding is necessary
int sourceY_down = down_adj + sourceY;
int sourceY_up = up_adj - sourceY;
int sourceX_right = right_adj + sourceX;
int sourceX_left = left_adj - sourceX;
float r = lens_corr_radius * fast_sqrtf(newX2 + newY2);
float theta = fast_atanf(r) / r; // r is never 0
int sourceX = halfWidth + fast_roundf(theta * zoomedX); // rounding is necessary
int sourceY = halfHeight + fast_roundf(theta * zoomedY); // rounding is necessary
// plot the 4 symmetrical pixels
// top 2 pixels
if (sourceY_down >= 0 && sourceY_down < h) {
uint8_t *ptr = tmp + (w * sourceY_down);
if ((0 <= sourceX) && (0 <= sourceY)) { // plot the 4 symmetrical pixels
uint8_t *ptr, pixel;
ptr = tmp + (w * sourceY); // top 2 pixels
pixel = ptr[sourceX];
row_ptr[x] = pixel;
pixel = ptr[w - 1 - sourceX];
row_ptr[w - 1 - x] = pixel;
ptr = tmp + (w * (h - 1 - sourceY)); // bottom 2 pixels
pixel = ptr[sourceX];
row_ptr2[x] = pixel;
pixel = ptr[w - 1 - sourceX];
row_ptr2[w - 1 - x] = pixel;
if (sourceX_right >= 0 && sourceX_right < w) {
row_ptr[x] = ptr[sourceX_right];
}
if (sourceX_left >= 0 && sourceX_left < w) {
row_ptr[w - 1 - x] = ptr[sourceX_left];
}
}
// bottom 2 pixels
if (sourceY_up >= 0 && sourceY_up < h) {
uint8_t *ptr = tmp + (w * sourceY_up);
if (sourceX_right >= 0 && sourceX_right < w) {
row_ptr2[x] = ptr[sourceX_right];
}
if (sourceX_left >= 0 && sourceX_left < w) {
row_ptr2[w - 1 - x] = ptr[sourceX_left];
}
}
}
}
@ -1293,35 +1341,48 @@ void imlib_lens_corr(image_t *img, float strength, float zoom)
case IMAGE_BPP_RGB565: {
uint16_t *tmp = (uint16_t *) data;
for (int y = 0, yy = halfHeight; y < yy; y++) {
for (int y = 0; y < halfHeight; y++) {
uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(img, y);
uint16_t *row_ptr2 = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(img, h-1-y);
int newY = y - halfHeight;
int newY2 = newY * newY;
float zoomedY = newY * zoom;
for (int x = 0, xx = halfWidth; x < xx; x++) {
for (int x = 0; x < halfWidth; x++) {
int newX = x - halfWidth;
int newX2 = newX * newX;
float zoomedX = newX * zoom;
float precalculated = precalculated_table[(int)fast_sqrtf(newX2 + newY2)];
int sourceY = fast_roundf(precalculated * newY); // rounding is necessary
int sourceX = fast_roundf(precalculated * newX); // rounding is necessary
int sourceY_down = down_adj + sourceY;
int sourceY_up = up_adj - sourceY;
int sourceX_right = right_adj + sourceX;
int sourceX_left = left_adj - sourceX;
float r = lens_corr_radius * fast_sqrtf(newX2 + newY2);
float theta = fast_atanf(r) / r; // r is never 0
int sourceX = halfWidth + fast_roundf(theta * zoomedX); // rounding is necessary
int sourceY = halfHeight + fast_roundf(theta * zoomedY); // rounding is necessary
// plot the 4 symmetrical pixels
// top 2 pixels
if (sourceY_down >= 0 && sourceY_down < h) {
uint16_t *ptr = tmp + (w * sourceY_down);
if ((0 <= sourceX) && (0 <= sourceY)) { // plot the 4 symmetrical pixels
uint16_t *ptr, pixel;
ptr = tmp + (w * sourceY); // top 2 pixels
pixel = ptr[sourceX];
row_ptr[x] = pixel;
pixel = ptr[w - 1 - sourceX];
row_ptr[w - 1 - x] = pixel;
ptr = tmp + (w * (h - 1 - sourceY)); // bottom 2 pixels
pixel = ptr[sourceX];
row_ptr2[x] = pixel;
pixel = ptr[w - 1 - sourceX];
row_ptr2[w - 1 - x] = pixel;
if (sourceX_right >= 0 && sourceX_right < w) {
row_ptr[x] = ptr[sourceX_right];
}
if (sourceX_left >= 0 && sourceX_left < w) {
row_ptr[w - 1 - x] = ptr[sourceX_left];
}
}
// bottom 2 pixels
if (sourceY_up >= 0 && sourceY_up < h) {
uint16_t *ptr = tmp + (w * sourceY_up);
if (sourceX_right >= 0 && sourceX_right < w) {
row_ptr2[x] = ptr[sourceX_right];
}
if (sourceX_left >= 0 && sourceX_left < w) {
row_ptr2[w - 1 - x] = ptr[sourceX_left];
}
}
}
}
@ -1331,8 +1392,9 @@ void imlib_lens_corr(image_t *img, float strength, float zoom)
break;
}
}
fb_free();
fb_free(); // precalculated_table
fb_free(); // data
}
#endif //IMLIB_ENABLE_LENS_CORR

View File

@ -1338,7 +1338,7 @@ void imlib_remove_shadows(image_t *img, const char *path, image_t *other, int sc
void imlib_chrominvar(image_t *img);
void imlib_illuminvar(image_t *img);
// Lens/Rotation Correction
void imlib_lens_corr(image_t *img, float strength, float zoom);
void imlib_lens_corr(image_t *img, float strength, float zoom, float x_corr, float y_corr);
void imlib_rotation_corr(image_t *img, float x_rotation, float y_rotation,
float z_rotation, float x_translation, float y_translation,
float zoom, float fov, float *corners);

View File

@ -3273,8 +3273,13 @@ STATIC mp_obj_t py_image_lens_corr(uint n_args, const mp_obj_t *args, mp_map_t *
py_helper_keyword_float(n_args, args, 2, kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_zoom), 1.0f);
PY_ASSERT_TRUE_MSG(arg_zoom > 0.0f, "Zoom must be > 0!");
float arg_x_corr =
py_helper_keyword_float(n_args, args, 3, kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_x_corr), 0.0f);
float arg_y_corr =
py_helper_keyword_float(n_args, args, 4, kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_y_corr), 0.0f);
fb_alloc_mark();
imlib_lens_corr(arg_img, arg_strength, arg_zoom);
imlib_lens_corr(arg_img, arg_strength, arg_zoom, arg_x_corr, arg_y_corr);
fb_alloc_free_till_mark();
return args[0];
}

View File

@ -812,6 +812,8 @@ Q(logpolar)
Q(lens_corr)
Q(strength)
Q(zoom)
Q(x_corr)
Q(y_corr)
// Rotation Correction
Q(rotation_corr)