diff --git a/src/omv/img/imlib.c b/src/omv/img/imlib.c index c10467dfe..771e48ee2 100644 --- a/src/omv/img/imlib.c +++ b/src/omv/img/imlib.c @@ -976,6 +976,118 @@ void imlib_histeq(image_t *img) fb_free(); } +// 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) +{ + zoom = 1 / zoom; + int halfWidth = img->w / 2; + int halfHeight = img->h / 2; + float lens_corr_radius = strength / fast_sqrtf((img->w * img->w) + (img->h * img->h)); + + switch(img->bpp) { + case IMAGE_BPP_BINARY: { + // Create a temp copy of the image to pull pixels from. + uint32_t *tmp = fb_alloc(((img->w + UINT32_T_MASK) >> UINT32_T_SHIFT) * img->h); + memcpy(tmp, img->data, ((img->w + UINT32_T_MASK) >> UINT32_T_SHIFT) * img->h); + + for (int y = 0, yy = img->h; y < yy; y++) { + uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(img, y); + int newY = y - halfHeight; + int newY2 = newY * newY; + float zoomedY = newY * zoom; + + for (int x = 0, xx = img->w; x < xx; x++) { + int newX = x - halfWidth; + int newX2 = newX * newX; + float zoomedX = newX * zoom; + + float r = lens_corr_radius * fast_sqrtf(newX2 + newY2); + float theta = (r < 0.0000001f) ? 1.0f : (fast_atanf(r) / r); + int sourceX = halfWidth + fast_roundf(theta * zoomedX); + int sourceY = halfHeight + fast_roundf(theta * zoomedY); + + if ((0 <= sourceX) && (sourceX < img->w) && (0 <= sourceY) && (sourceY < img->h)) { + uint32_t *ptr = tmp + (((img->w + UINT32_T_MASK) >> UINT32_T_SHIFT) * sourceY); + int pixel = IMAGE_GET_BINARY_PIXEL_FAST(ptr, sourceX); + IMAGE_PUT_BINARY_PIXEL_FAST(row_ptr, x, pixel); + } + } + } + + fb_free(); + break; + } + case IMAGE_BPP_GRAYSCALE: { + // Create a temp copy of the image to pull pixels from. + uint8_t *tmp = fb_alloc(img->w * img->h * sizeof(uint8_t)); + memcpy(tmp, img->data, img->w * img->h * sizeof(uint8_t)); + + for (int y = 0, yy = img->h; y < yy; y++) { + uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(img, y); + int newY = y - halfHeight; + int newY2 = newY * newY; + float zoomedY = newY * zoom; + + for (int x = 0, xx = img->w; x < xx; x++) { + int newX = x - halfWidth; + int newX2 = newX * newX; + float zoomedX = newX * zoom; + + float r = lens_corr_radius * fast_sqrtf(newX2 + newY2); + float theta = (r < 0.0000001f) ? 1.0f : (fast_atanf(r) / r); + int sourceX = halfWidth + fast_roundf(theta * zoomedX); + int sourceY = halfHeight + fast_roundf(theta * zoomedY); + + if ((0 <= sourceX) && (sourceX < img->w) && (0 <= sourceY) && (sourceY < img->h)) { + uint8_t *ptr = tmp + (img->w * sourceY); + int pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(ptr, sourceX); + IMAGE_PUT_GRAYSCALE_PIXEL_FAST(row_ptr, x, pixel); + } + } + } + + fb_free(); + break; + } + case IMAGE_BPP_RGB565: { + // Create a temp copy of the image to pull pixels from. + uint16_t *tmp = fb_alloc(img->w * img->h * sizeof(uint16_t)); + memcpy(tmp, img->data, img->w * img->h * sizeof(uint16_t)); + + for (int y = 0, yy = img->h; y < yy; y++) { + uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(img, y); + int newY = y - halfHeight; + int newY2 = newY * newY; + float zoomedY = newY * zoom; + + for (int x = 0, xx = img->w; x < xx; x++) { + int newX = x - halfWidth; + int newX2 = newX * newX; + float zoomedX = newX * zoom; + + float r = lens_corr_radius * fast_sqrtf(newX2 + newY2); + float theta = (r < 0.0000001f) ? 1.0f : (fast_atanf(r) / r); + int sourceX = halfWidth + fast_roundf(theta * zoomedX); + int sourceY = halfHeight + fast_roundf(theta * zoomedY); + + if ((0 <= sourceX) && (sourceX < img->w) && (0 <= sourceY) && (sourceY < img->h)) { + uint16_t *ptr = tmp + (img->w * sourceY); + int pixel = IMAGE_GET_RGB565_PIXEL_FAST(ptr, sourceX); + IMAGE_PUT_RGB565_PIXEL_FAST(row_ptr, x, pixel); + } + } + } + + fb_free(); + break; + } + default: { + break; + } + } +} + void imlib_mask_ellipse(image_t *img) { int h = img->w/2; @@ -1037,41 +1149,3 @@ int imlib_image_std(image_t *src) /* std */ return fast_sqrtf(v); } - -// Simple lens correction function. -// See http://www.tannerhelland.com/4743/simple-algorithm-correcting-lens-distortion/ -void imlib_lens_corr(image_t *src, float strength) -{ - float zoom = 1.0f; - int halfWidth = src->w / 2; - int halfHeight = src->h / 2; - float corr_radius = strength / fast_sqrtf(src->w*src->w + src->h*src->h); - - int buff_size = src->w * src->h / 2; - uint8_t *buff = fb_alloc(buff_size); - - for (int y=0; yh; y++) { - if (y == src->h/2) { - // Flush the first half of the image - memcpy(src->data, buff, buff_size); - } - for (int x=0; xw; x++) { - int newX = x - halfWidth; - int newY = y - halfHeight; - - float r = corr_radius * fast_sqrtf(newX*newX + newY*newY); - float theta = (abs(r) < 1e-6f) ? (atanf(r)/r) : 1.0f; - - newX = (int) (newX * theta * zoom + halfWidth ); - newY = (int) (newY * theta * zoom + halfHeight); - if (newX >= 0 && newX < src->w && newY >= 0 && newY < src->h) { - buff[(y %(src->h/2)) * src->w + x] = src->data[newY * src->w + newX]; - } else { - buff[(y %(src->h/2)) * src->w + x] = src->data[y * src->w + x]; - } - } - } - // Flush the second half of the image - memcpy(src->data + buff_size, buff, buff_size); - fb_free(); -} diff --git a/src/omv/img/imlib.h b/src/omv/img/imlib.h index 0a78244a1..c18286043 100644 --- a/src/omv/img/imlib.h +++ b/src/omv/img/imlib.h @@ -447,7 +447,7 @@ float IMAGE_Y_RATIO = ((float) _source_rect->s.h) / ((float) _target_rect->s.h); __typeof__ (row_ptr) _row_ptr = (row_ptr); \ __typeof__ (x) _x = (x); \ __typeof__ (v) _v = (v); \ - size_t _i = _x >> UINT32_T_SHIFT \ + size_t _i = _x >> UINT32_T_SHIFT; \ size_t _j = _x & UINT32_T_MASK; \ _row_ptr[_i] = (_row_ptr[_i] & (~(1 << _j))) | ((_v & 1) << _j); \ }) @@ -1057,8 +1057,7 @@ void imlib_edge_canny(image_t *src, rectangle_t *roi, int low_thresh, int high_t void imlib_find_hog(image_t *src, rectangle_t *roi, int cell_size); // Lens correction -void imlib_lens_corr(image_t *src, float strength); - +void imlib_lens_corr(image_t *img, float strength, float zoom); // Statistics void imlib_get_histogram(histogram_t *out, image_t *ptr, rectangle_t *roi); void imlib_get_percentile(percentile_t *out, image_bpp_t bpp, histogram_t *ptr, float percentile); diff --git a/src/omv/py/py_image.c b/src/omv/py/py_image.c index 3c8dffadc..828c862a6 100644 --- a/src/omv/py/py_image.c +++ b/src/omv/py/py_image.c @@ -850,16 +850,23 @@ static mp_obj_t py_image_histeq(mp_obj_t img_obj) return img_obj; } -static mp_obj_t py_image_lens_corr(mp_obj_t img_obj, mp_obj_t s_obj) +static mp_obj_t py_image_lens_corr(uint n_args, const mp_obj_t *args, mp_map_t *kw_args) { - image_t *arg_img = py_image_cobj(img_obj); - PY_ASSERT_FALSE_MSG(IM_IS_JPEG(arg_img), "Operation not supported on JPEG"); + image_t *arg_img = py_image_cobj(args[0]); + PY_ASSERT_FALSE_MSG(IM_IS_JPEG(arg_img), + "Operation not supported on JPEG"); - imlib_lens_corr(arg_img, mp_obj_get_float(s_obj)); - return img_obj; + float strength = py_helper_lookup_float(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_strength), + (n_args > 1) ? mp_obj_get_float(args[1]) : 1.8); + PY_ASSERT_TRUE_MSG(strength >= 0.0, "strength must be > 0"); + float zoom = py_helper_lookup_float(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_zoom), + (n_args > 2) ? mp_obj_get_float(args[2]) : 1.0); + PY_ASSERT_TRUE_MSG(zoom >= 1.0, "zoom must be > 1"); + + imlib_lens_corr(arg_img, strength, zoom); + return args[0]; } - static mp_obj_t py_image_mask_ellipse(mp_obj_t img_obj) { image_t *arg_img = py_image_cobj(img_obj); @@ -2329,7 +2336,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_2(py_image_mode_obj, py_image_mode); STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_median_obj, 2, py_image_median); STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_gaussian_obj, 1, py_image_gaussian); STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_image_histeq_obj, py_image_histeq); -STATIC MP_DEFINE_CONST_FUN_OBJ_2(py_image_lens_corr_obj, py_image_lens_corr); +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_lens_corr_obj, 1, py_image_lens_corr); STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_image_mask_ellipse_obj, py_image_mask_ellipse); /* Image Statistics */ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_get_histogram_obj, 1, py_image_get_histogram); diff --git a/src/omv/py/qstrdefsomv.h b/src/omv/py/qstrdefsomv.h index fabde8d8b..2b301214b 100644 --- a/src/omv/py/qstrdefsomv.h +++ b/src/omv/py/qstrdefsomv.h @@ -93,7 +93,6 @@ Q(add) Q(bias) Q(percentile) Q(normalized) -Q(lens_corr) Q(filter_outliers) Q(scale_factor) Q(max_keypoints) @@ -309,6 +308,11 @@ Q(CPUFREQ_216MHZ) Q(get_frequency) Q(set_frequency) +// Lens Correction +Q(lens_corr) +Q(strength) +Q(zoom) + // Get Histogram Q(get_hist) Q(get_histogram) diff --git a/usr/examples/09-Feature-Detection/qrcodes_with_lens_corr.py b/usr/examples/09-Feature-Detection/qrcodes_with_lens_corr.py index 4c85ba19a..a3aa67fb9 100644 --- a/usr/examples/09-Feature-Detection/qrcodes_with_lens_corr.py +++ b/usr/examples/09-Feature-Detection/qrcodes_with_lens_corr.py @@ -1,29 +1,21 @@ # QRCode Example # -# This example shows the power of the OpenMV Cam to detect QR Codes. -# -# On the new M7 OpenMV Cam you can detect QR codes at up to 320x240 in Grayscale or RGB565. -# We may be able to enable 640x480 operation in the future if we re-write the QR code library's front end code. -# -# On the M4 OpenMV Cam QR code detection should be done strictly at a maximum of 160x120 for Grayscale or RGB565. -# -# Lastly, reading QRCodes requires lens correction to improve the detection rate. -# Additionally, histogram equalization could be used to increase the contrast but is not required. +# This example shows the power of the OpenMV Cam to detect QR Codes +# using lens correction (see the qrcodes_with_lens_corr.py script for higher performance). import sensor, image, time -# For the new M7 OpenMV Cam... sensor.reset() -sensor.set_pixformat(sensor.GRAYSCALE) -sensor.set_framesize(sensor.QVGA) -sensor.skip_frames(10) -sensor.set_auto_gain(False) +sensor.set_pixformat(sensor.RGB565) +sensor.set_framesize(sensor.QQVGA) # can be QQVGA on M7... +sensor.skip_frames(30) +sensor.set_auto_gain(False) # must turn this off to prevent image washout... clock = time.clock() while(True): clock.tick() img = sensor.snapshot() - img.lens_corr(1.6) # Lens correction does not work on RGB565 images right now. This will be fixed ASAP. + img.lens_corr(1.8) # strength of 1.8 is good for the 2.8mm lens. for code in img.find_qrcodes(): print(code) print(clock.fps())