Speed up lens_corr.

Speed up the algorithm by fixing the abs() issue. Do not use that
function in any of your code. It by itself cut the speed of the code
in half. I don't know what's in that function but I'm guessing it does
ABS of a float using ints or something.

I made the zoom parameter functional now too so you can use lens_corr to
zoom in on the image. Argument parsing is handled too. Finally, I
updated the only script where this is used.

Note that I'm able to get more than 10 FPS at 160x120 on the M4 and 15
FPS at 160x120 on the M7. Previous this was at about 5 FPS and 7.5 FPS
respectively.
This commit is contained in:
Kwabena W. Agyeman 2017-01-15 21:32:55 -05:00
parent e650adb53a
commit d6b49adefa
5 changed files with 140 additions and 64 deletions

View File

@ -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; y<src->h; y++) {
if (y == src->h/2) {
// Flush the first half of the image
memcpy(src->data, buff, buff_size);
}
for (int x=0; x<src->w; 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();
}

View File

@ -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);

View File

@ -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);

View File

@ -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)

View File

@ -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())