From 57ea4e326c8f201cd5c002f31c220994adc14500 Mon Sep 17 00:00:00 2001 From: "Kwabena W. Agyeman" Date: Tue, 12 May 2020 19:35:24 -0700 Subject: [PATCH] Fix and speedup rotation correction --- src/omv/img/apriltag.c | 198 ++++++++++++++++++++++++++++------------- 1 file changed, 135 insertions(+), 63 deletions(-) diff --git a/src/omv/img/apriltag.c b/src/omv/img/apriltag.c index f231168dc..319432056 100644 --- a/src/omv/img/apriltag.c +++ b/src/omv/img/apriltag.c @@ -12329,7 +12329,13 @@ void imlib_rotation_corr(image_t *img, float x_rotation, float y_rotation, float float x_translation, float y_translation, float zoom, float fov, float *corners) { - umm_init_x(8000); // 400 20 byte heap blocks... + // 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); + + umm_init_x(fb_avail()); int w = img->w; int h = img->h; @@ -12378,7 +12384,7 @@ void imlib_rotation_corr(image_t *img, float x_rotation, float y_rotation, float matd_t *T3 = matd_op("M*M", A2, T2); matd_t *T4 = matd_inverse(T3); - if (corners) { + if (T4 && corners) { float corr[4]; zarray_t *correspondences = zarray_create(sizeof(float[4])); @@ -12395,7 +12401,7 @@ void imlib_rotation_corr(image_t *img, float x_rotation, float y_rotation, float zarray_add(correspondences, &corr); corr[0] = w - 1; - corr[1] = h- 1; + corr[1] = h - 1; corr[2] = corners[4]; corr[3] = corners[5]; zarray_add(correspondences, &corr); @@ -12406,9 +12412,13 @@ void imlib_rotation_corr(image_t *img, float x_rotation, float y_rotation, float corr[3] = corners[7]; zarray_add(correspondences, &corr); - matd_t *H = homography_compute(correspondences, HOMOGRAPHY_COMPUTE_FLAG_SVD); + matd_t *H = homography_compute(correspondences, HOMOGRAPHY_COMPUTE_FLAG_INVERSE); - if (T4 && H) { + if (!H) { // try again... + H = homography_compute(correspondences, HOMOGRAPHY_COMPUTE_FLAG_SVD); + } + + if (H) { matd_t *T5 = matd_op("M*M", H, T4); matd_destroy(H); matd_destroy(T4); @@ -12418,91 +12428,151 @@ void imlib_rotation_corr(image_t *img, float x_rotation, float y_rotation, float zarray_destroy(correspondences); } - // 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); - if (T4) { float T4_00 = MATD_EL(T4, 0, 0), T4_01 = MATD_EL(T4, 0, 1), T4_02 = MATD_EL(T4, 0, 2); float T4_10 = MATD_EL(T4, 1, 0), T4_11 = MATD_EL(T4, 1, 1), T4_12 = MATD_EL(T4, 1, 2); float T4_20 = MATD_EL(T4, 2, 0), T4_21 = MATD_EL(T4, 2, 1), T4_22 = MATD_EL(T4, 2, 2); - switch(img->bpp) { - case IMAGE_BPP_BINARY: { - uint32_t *tmp = (uint32_t *) data; + if ((fast_fabsf(T4_20) < MATD_EPS) && (fast_fabsf(T4_21) < MATD_EPS)) { // warp affine + T4_00 /= T4_22; + T4_01 /= T4_22; + T4_02 /= T4_22; + T4_10 /= T4_22; + T4_11 /= T4_22; + T4_12 /= T4_22; + switch(img->bpp) { + case IMAGE_BPP_BINARY: { + uint32_t *tmp = (uint32_t *) data; - for (int y = 0, yy = h; y < yy; y++) { - uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(img, y); - for (int x = 0, xx = w; x < xx; x++) { - float xxx = T4_00*x + T4_01*y + T4_02; - float yyy = T4_10*x + T4_11*y + T4_12; - float zzz = T4_20*x + T4_21*y + T4_22; - int sourceX = fast_roundf(xxx / zzz); - int sourceY = fast_roundf(yyy / zzz); + for (int y = 0, yy = h; y < yy; y++) { + uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(img, y); + for (int x = 0, xx = w; x < xx; x++) { + int sourceX = fast_roundf(T4_00*x + T4_01*y + T4_02); + int sourceY = fast_roundf(T4_10*x + T4_11*y + T4_12); - if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { - uint32_t *ptr = tmp + (((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); + if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { + uint32_t *ptr = tmp + (((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); + } } } + break; } - break; - } - case IMAGE_BPP_GRAYSCALE: { - uint8_t *tmp = (uint8_t *) data; + case IMAGE_BPP_GRAYSCALE: { + uint8_t *tmp = (uint8_t *) data; - for (int y = 0, yy = h; y < yy; y++) { - uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(img, y); - for (int x = 0, xx = w; x < xx; x++) { - float xxx = T4_00*x + T4_01*y + T4_02; - float yyy = T4_10*x + T4_11*y + T4_12; - float zzz = T4_20*x + T4_21*y + T4_22; - int sourceX = fast_roundf(xxx / zzz); - int sourceY = fast_roundf(yyy / zzz); + for (int y = 0, yy = h; y < yy; y++) { + uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(img, y); + for (int x = 0, xx = w; x < xx; x++) { + int sourceX = fast_roundf(T4_00*x + T4_01*y + T4_02); + int sourceY = fast_roundf(T4_10*x + T4_11*y + T4_12); - if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { - uint8_t *ptr = tmp + (w * sourceY); - int pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(ptr, sourceX); - IMAGE_PUT_GRAYSCALE_PIXEL_FAST(row_ptr, x, pixel); + if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { + uint8_t *ptr = tmp + (w * sourceY); + int pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(ptr, sourceX); + IMAGE_PUT_GRAYSCALE_PIXEL_FAST(row_ptr, x, pixel); + } } } + break; } - break; - } - case IMAGE_BPP_RGB565: { - uint16_t *tmp = (uint16_t *) data; + case IMAGE_BPP_RGB565: { + uint16_t *tmp = (uint16_t *) data; - for (int y = 0, yy = h; y < yy; y++) { - uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(img, y); - for (int x = 0, xx = w; x < xx; x++) { - float xxx = T4_00*x + T4_01*y + T4_02; - float yyy = T4_10*x + T4_11*y + T4_12; - float zzz = T4_20*x + T4_21*y + T4_22; - int sourceX = fast_roundf(xxx / zzz); - int sourceY = fast_roundf(yyy / zzz); + for (int y = 0, yy = h; y < yy; y++) { + uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(img, y); + for (int x = 0, xx = w; x < xx; x++) { + int sourceX = fast_roundf(T4_00*x + T4_01*y + T4_02); + int sourceY = fast_roundf(T4_10*x + T4_11*y + T4_12); - if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { - uint16_t *ptr = tmp + (w * sourceY); - int pixel = IMAGE_GET_RGB565_PIXEL_FAST(ptr, sourceX); - IMAGE_PUT_RGB565_PIXEL_FAST(row_ptr, x, pixel); + if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { + uint16_t *ptr = tmp + (w * sourceY); + int pixel = IMAGE_GET_RGB565_PIXEL_FAST(ptr, sourceX); + IMAGE_PUT_RGB565_PIXEL_FAST(row_ptr, x, pixel); + } } } + break; + } + default: { + break; } - break; } - default: { - break; + } else { // warp persepective + switch(img->bpp) { + case IMAGE_BPP_BINARY: { + uint32_t *tmp = (uint32_t *) data; + + for (int y = 0, yy = h; y < yy; y++) { + uint32_t *row_ptr = IMAGE_COMPUTE_BINARY_PIXEL_ROW_PTR(img, y); + for (int x = 0, xx = w; x < xx; x++) { + float xxx = T4_00*x + T4_01*y + T4_02; + float yyy = T4_10*x + T4_11*y + T4_12; + float zzz = T4_20*x + T4_21*y + T4_22; + int sourceX = fast_roundf(xxx / zzz); + int sourceY = fast_roundf(yyy / zzz); + + if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { + uint32_t *ptr = tmp + (((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); + } + } + } + break; + } + case IMAGE_BPP_GRAYSCALE: { + uint8_t *tmp = (uint8_t *) data; + + for (int y = 0, yy = h; y < yy; y++) { + uint8_t *row_ptr = IMAGE_COMPUTE_GRAYSCALE_PIXEL_ROW_PTR(img, y); + for (int x = 0, xx = w; x < xx; x++) { + float xxx = T4_00*x + T4_01*y + T4_02; + float yyy = T4_10*x + T4_11*y + T4_12; + float zzz = T4_20*x + T4_21*y + T4_22; + int sourceX = fast_roundf(xxx / zzz); + int sourceY = fast_roundf(yyy / zzz); + + if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { + uint8_t *ptr = tmp + (w * sourceY); + int pixel = IMAGE_GET_GRAYSCALE_PIXEL_FAST(ptr, sourceX); + IMAGE_PUT_GRAYSCALE_PIXEL_FAST(row_ptr, x, pixel); + } + } + } + break; + } + case IMAGE_BPP_RGB565: { + uint16_t *tmp = (uint16_t *) data; + + for (int y = 0, yy = h; y < yy; y++) { + uint16_t *row_ptr = IMAGE_COMPUTE_RGB565_PIXEL_ROW_PTR(img, y); + for (int x = 0, xx = w; x < xx; x++) { + float xxx = T4_00*x + T4_01*y + T4_02; + float yyy = T4_10*x + T4_11*y + T4_12; + float zzz = T4_20*x + T4_21*y + T4_22; + int sourceX = fast_roundf(xxx / zzz); + int sourceY = fast_roundf(yyy / zzz); + + if ((0 <= sourceX) && (sourceX < w) && (0 <= sourceY) && (sourceY < h)) { + uint16_t *ptr = tmp + (w * sourceY); + int pixel = IMAGE_GET_RGB565_PIXEL_FAST(ptr, sourceX); + IMAGE_PUT_RGB565_PIXEL_FAST(row_ptr, x, pixel); + } + } + } + break; + } + default: { + break; + } } } matd_destroy(T4); } - fb_free(); - matd_destroy(T3); matd_destroy(T2); matd_destroy(T1); @@ -12515,6 +12585,8 @@ void imlib_rotation_corr(image_t *img, float x_rotation, float y_rotation, float matd_destroy(A1); fb_free(); // umm_init_x(); + + fb_free(); } #endif //IMLIB_ENABLE_ROTATION_CORR #pragma GCC diagnostic pop