diff --git a/src/omv/img/freak.c b/src/omv/img/freak.c index adb2f208b..ce32712b0 100644 --- a/src/omv/img/freak.c +++ b/src/omv/img/freak.c @@ -32,8 +32,8 @@ #include #include #include -#include "fmath.h" #include "arm_math.h" +#include "ff.h" #include "imlib.h" #include "xalloc.h" @@ -47,9 +47,10 @@ __typeof__ (b) _b = (b); \ _a > _b ? _a : _b; }) -#define kSQRT2 (1.4142135623731f) -#define kINV_SQRT2 (1.0f / 1.4142135623731f) -#define kLOG2 (0.693147180559945f) +#define PI (3.141592f) +#define kSQRT2 (1.414213f) +#define kINV_SQRT2 (1.0f / 1.414213f) +#define kLOG2 (0.693147f) #define kNB_SCALES (64) #define kNB_ORIENTATION (256) #define kNB_POINTS (43) @@ -63,8 +64,8 @@ #define SCALE_IDX (0) #define KP_SIZE (23) -#define SCALE_STEP (1.044273782f) // 2 ^ ( (nbOctaves-1) /nbScales) -#define SCALE_FACTOR (1.0f) // SCALE_STEP ^ SCALE_IDX +#define SCALE_STEP (1.044273f) // 2 ^ ( (nbOctaves-1) /nbScales) +#define SCALE_FACTOR (1.0f) // SCALE_STEP ^ SCALE_IDX #define SIZE_CST (kNB_SCALES/(kLOG2* NUM_OCTAVES)) #define PATTERN_SCALE (22.0f) @@ -92,7 +93,7 @@ const static float sigma[8] = { (BIG_R-18*UNIT_SPACE)/2.0f, (BIG_R-20*UNIT_SPACE)/2.0f, (SMALL_R)/2.0f, - 0.0f + (SMALL_R)/2.0f }; const static int ORIENTATION_PAIRS[45][4] = { @@ -191,7 +192,7 @@ const static uint8_t DESCRIPTION_PAIRS[512][2] = { // simply take average on a square patch, not even gaussian approx static uint8_t mean_intensity(image_t *image, i_image_t *i_image, - float kp_x, float kp_y, float scale_factor, uint32_t rot, uint32_t point) + int kp_x, int kp_y, float scale_factor, uint32_t rot, uint32_t point) { int pidx = (point/6)%8; float alpha, beta, theta = 0; @@ -255,7 +256,12 @@ void freak_find_keypoints(image_t *image, kp_t *kpts, int kpts_size, bool orient i_image = xalloc(sizeof(*i_image)); i_image->w = image->w; i_image->h = image->h; +#ifdef OPENMV2 i_image->data = xalloc(image->w*image->h*sizeof(*i_image->data)); +#else +#include "framebuffer.h" + i_image->data = (uint32_t*) (fb->pixels+(fb->w * fb->h)); +#endif imlib_integral_image(image, i_image); uint8_t *desc; @@ -288,9 +294,7 @@ void freak_find_keypoints(image_t *image, kp_t *kpts, int kpts_size, bool orient if (thetaIdx < 0) { thetaIdx += kNB_ORIENTATION; - } - - if (thetaIdx >= kNB_ORIENTATION) { + } else if (thetaIdx >= kNB_ORIENTATION) { thetaIdx -= kNB_ORIENTATION; } } @@ -305,7 +309,9 @@ void freak_find_keypoints(image_t *image, kp_t *kpts, int kpts_size, bool orient } } +#ifdef OPENMV2 xfree(i_image->data); +#endif xfree(i_image); } @@ -322,7 +328,7 @@ int16_t *freak_match_keypoints(kp_t *kpts1, int kpts1_size, kp_t *kpts2, int kpt kp_t *kp2 = &kpts2[y]; int dist = 0; - for (int m=0; m<16; m++) { + for (int m=0; m<4; m++) { //128 bits uint32_t v = ((uint32_t*)(kp1->desc))[m] ^ ((uint32_t*)(kp2->desc))[m]; while (v) { dist++; @@ -330,6 +336,21 @@ int16_t *freak_match_keypoints(kp_t *kpts1, int kpts1_size, kp_t *kpts2, int kpt } } + // FREAK descriptor works like a cascade, the first 128 bits are more + // important, if the distance is bigger than a threshold, discard descriptor. + if (dist > 32) { + continue; + } + + for (int m=4; m<16; m++) { + uint32_t v = ((uint32_t*)(kp1->desc))[m] ^ ((uint32_t*)(kp2->desc))[m]; + while (v) { + dist++; + v &= v - 1; + } + } + + if (dist < min_dist) { min_idx = y; min_dist = dist; @@ -345,3 +366,80 @@ int16_t *freak_match_keypoints(kp_t *kpts1, int kpts1_size, kp_t *kpts2, int kpt } return kpts_match; } + +int freak_save_descriptor(kp_t *kpts, int kpts_size, const char *path) +{ + FIL fp; + UINT bytes; + FRESULT res=FR_OK; + + res = f_open(&fp, path, FA_WRITE|FA_CREATE_ALWAYS); + if (res != FR_OK) { + return res; + } + + /* write number of keypoints */ + res = f_write(&fp, &kpts_size, sizeof(kpts_size), &bytes); + if (res != FR_OK || bytes != sizeof(kpts_size)) { + goto error; + } + + /* write keypoints */ + for (int i=0; iangle, sizeof(kp->angle), &bytes); + if (res != FR_OK || bytes != sizeof(kp->angle)) { + goto error; + } + + /* write descriptor */ + res = f_write(&fp, kp->desc, kNB_PAIRS/8, &bytes); + if (res != FR_OK || bytes != kNB_PAIRS/8) { + goto error; + } + } + +error: + f_close(&fp); + return res; +} + +int freak_load_descriptor(kp_t **kpts_out, int *kpts_size_out, const char *path) +{ + FIL fp; + UINT bytes; + FRESULT res=FR_OK; + + int kpts_size=0; + kp_t *kpts = NULL; + + res = f_open(&fp, path, FA_READ|FA_OPEN_EXISTING); + if (res != FR_OK) { + return res; + } + + /* read number of keypoints */ + res = f_read(&fp, &kpts_size, sizeof(kpts_size), &bytes); + if (res != FR_OK || bytes != sizeof(kpts_size)) { + goto error; + } + + kpts = xalloc(kpts_size*sizeof(*kpts)); + + /* read keypoints */ + for (int i=0; idesc, kNB_PAIRS/8, &bytes); + if (res != FR_OK || bytes != kNB_PAIRS/8) { + goto error; + } + } + + *kpts_out = kpts; + *kpts_size_out = kpts_size; +error: + f_close(&fp); + return res; +} diff --git a/src/omv/img/imlib.c b/src/omv/img/imlib.c index aa7505812..18818af7b 100644 --- a/src/omv/img/imlib.c +++ b/src/omv/img/imlib.c @@ -351,7 +351,20 @@ void imlib_subimage(struct image *src_img, struct image *dst_img, int x_off, int } } -void imlib_blit_gs(struct image *src, struct image *dst, int x_off, int y_off) +void imlib_blit_bytes(struct image *src, struct image *dst, int x_off, int y_off) +{ + int x, y; + typeof(*src->data) *srcp = src->data; + typeof(*dst->data) *dstp = dst->data; + + for (y=y_off; yh+y_off; y++) { + for (x=x_off; xw+x_off; x++) { + dstp[y*dst->w+x]=*srcp++; + } + } +} + +void imlib_blit_gs_to_rgb(struct image *src, struct image *dst, int x_off, int y_off) { int x, y; uint8_t *srcp = src->data; @@ -365,25 +378,14 @@ void imlib_blit_gs(struct image *src, struct image *dst, int x_off, int y_off) } } -void imlib_blit_rgb(struct image *src, struct image *dst, int x_off, int y_off) -{ - int x, y; - typeof(*src->data) *srcp = src->data; //TODO - typeof(*dst->data) *dstp = dst->data; - - for (y=y_off; yh+y_off; y++) { - for (x=x_off; xw+x_off; x++) { - dstp[y*dst->w+x]=*srcp++; - } - } -} - void imlib_blit(struct image *src, struct image *dst, int x_off, int y_off) { - if (src->bpp == 1) { - imlib_blit_gs(src, dst, x_off, y_off); - } else { - imlib_blit_rgb(src, dst, x_off, y_off); + if (src->bpp == dst->bpp) { + imlib_blit_bytes(src, dst, x_off, y_off); + } else if (src->bpp == 1 && dst->bpp == 2) { + imlib_blit_gs_to_rgb(src, dst, x_off, y_off); + } else if (src->bpp == 2 && dst->bpp == 1) { + //imlib_blit_rgb_to_gs(src, dst, x_off, y_off); } } @@ -604,11 +606,11 @@ void imlib_draw_rectangle(struct image *image, struct rectangle *r) } } -void imlib_draw_circle(struct image *image, int cx, int cy, int r) +void imlib_draw_circle(struct image *image, int cx, int cy, int r, color_t *color) { int x = r, y = 0; - uint8_t c = 0xff; int radiusError = 1-x; + uint16_t c = RGB565(color->r, color->g, color->b); if (cx+r >= image->w || cx-r < 0 || cy+r >= image->h || cy-r < 0) { return; diff --git a/src/omv/img/imlib.h b/src/omv/img/imlib.h index 8d7d88201..5d37c222b 100644 --- a/src/omv/img/imlib.h +++ b/src/omv/img/imlib.h @@ -191,10 +191,12 @@ struct array *imlib_detect_objects(struct image *image, struct cascade* cascade) kp_t *fast_detect(image_t *image, int threshold, int *ret_num_corners); void freak_find_keypoints(image_t *image, kp_t *kpts, int kpts_size, bool orient_normalized, bool scale_normalized); int16_t *freak_match_keypoints(kp_t *kpts1, int kpts1_size, kp_t *kpts2, int kpts2_size, int threshold); +int freak_save_descriptor(kp_t *kpts, int kpts_size, const char *path); +int freak_load_descriptor(kp_t **kpts_out, int *kpts_size_out, const char *path); /* Drawing functions */ void imlib_draw_rectangle(struct image *image, struct rectangle *r); -void imlib_draw_circle(struct image *image, int cx, int cy, int r); +void imlib_draw_circle(struct image *image, int cx, int cy, int r, color_t *c); void imlib_draw_line(image_t *src, int x0, int y0, int x1, int y1); void imlib_draw_string(image_t *src, int x, int y, const char *str, color_t *c); diff --git a/src/omv/main.c b/src/omv/main.c index b7ead8529..c2b632bbe 100644 --- a/src/omv/main.c +++ b/src/omv/main.c @@ -135,6 +135,8 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_0(py_cpu_freq_obj, py_cpu_freq); STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_image_load_image_obj, py_image_load_image); STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_image_load_cascade_obj, py_image_load_cascade); +STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_image_load_descriptor_obj, py_image_load_descriptor); +STATIC MP_DEFINE_CONST_FUN_OBJ_2(py_image_save_descriptor_obj, py_image_save_descriptor); static const char fresh_main_py[] = "# main.py -- put your code here!\n" @@ -278,6 +280,8 @@ soft_reset: mp_store_global(qstr_from_str("cpu_freq"), (mp_obj_t)&py_cpu_freq_obj); mp_store_global(qstr_from_str("Image"), (mp_obj_t)&py_image_load_image_obj); mp_store_global(qstr_from_str("HaarCascade"), (mp_obj_t)&py_image_load_cascade_obj); + mp_store_global(qstr_from_str("FreakDesc"), (mp_obj_t)&py_image_load_descriptor_obj); + mp_store_global(qstr_from_str("FreakDescSave"), (mp_obj_t)&py_image_save_descriptor_obj); mp_store_global(qstr_from_str("vcp_is_connected"), (mp_obj_t)&py_vcp_is_connected_obj); // try to mount the flash diff --git a/src/omv/py/py_image.c b/src/omv/py/py_image.c index 79abda338..3abceb2b6 100644 --- a/src/omv/py/py_image.c +++ b/src/omv/py/py_image.c @@ -6,6 +6,7 @@ #include "py_assert.h" #include "py_image.h" #include "py_file.h" +#include "arm_math.h" extern struct sensor_dev sensor; static const mp_obj_type_t py_cascade_type; @@ -111,7 +112,7 @@ static mp_obj_t py_image_size(mp_obj_t self_in) static mp_obj_t py_image_save(uint n_args, const mp_obj_t *args, mp_map_t *kw_args) { - rectangle_t r; + int res; image_t *image = py_image_cobj(args[0]); const char *path = mp_obj_str_get_str(args[1]); @@ -119,14 +120,20 @@ static mp_obj_t py_image_save(uint n_args, const mp_obj_t *args, mp_map_t *kw_ar if (kw_subimage != NULL) { mp_obj_t *array; mp_obj_get_array_fixed_n(kw_subimage->value, 4, &array); - r.x = mp_obj_get_int(array[0]); - r.y = mp_obj_get_int(array[1]); - r.w = mp_obj_get_int(array[2]); - r.h = mp_obj_get_int(array[3]); + + rectangle_t r = { + mp_obj_get_int(array[0]), + mp_obj_get_int(array[1]), + mp_obj_get_int(array[2]), + mp_obj_get_int(array[3]), + }; + + res = imlib_save_image(image, path, &r); + } else { + res = imlib_save_image(image, path, NULL); } - int res; - if ((res=imlib_save_image(image, path, &r)) != FR_OK) { + if (res != FR_OK) { nlr_raise(mp_obj_new_exception_msg(&mp_type_OSError, ffs_strerror(res))); } @@ -364,7 +371,9 @@ static mp_obj_t py_image_draw_circle(mp_obj_t image_obj, mp_obj_t c_obj, mp_obj_ { int cx, cy, r; mp_obj_t *array; + struct image *image; + color_t c = {.r=0xFF, .g=0xFF, .b=0xFF}; /* get image pointer */ image = py_image_cobj(image_obj); @@ -376,7 +385,7 @@ static mp_obj_t py_image_draw_circle(mp_obj_t image_obj, mp_obj_t c_obj, mp_obj_ /* radius */ r = mp_obj_get_int(r_obj); - imlib_draw_circle(image, cx, cy, r); + imlib_draw_circle(image, cx, cy, r, &c); return mp_const_none; } @@ -457,28 +466,24 @@ static mp_obj_t py_image_draw_rectangle(mp_obj_t image_obj, mp_obj_t rectangle_o return mp_const_none; } -static mp_obj_t py_image_draw_keypoints(mp_obj_t image_obj, mp_obj_t kp_obj) +static mp_obj_t py_image_draw_keypoints(mp_obj_t image_obj, mp_obj_t kpts_obj) { -// int r = 2; -// for (int i=0; ix&&kp->y) { -// if (((kp->x+r) < image->w) && ((kp->y+r) < image->h)) { -// imlib_draw_circle(image, kp->x, kp->y, r); -// } -// } -// } + image_t *image = NULL; + py_kp_obj_t *kpts=NULL; + + /* get pointer */ + image = py_image_cobj(image_obj); + kpts = (py_kp_obj_t*)kpts_obj; + color_t cl = {.r=0xFF, .g=0xFF, .b=0xFF}; + + for (int i=0; isize; i++) { + kp_t *kp = &kpts->kpts[i]; + float co = arm_cos_f32(kp->angle); + float si = arm_sin_f32(kp->angle); + imlib_draw_line(image, kp->x, kp->y, kp->x+(co*10), kp->y+(si*10)); + imlib_draw_circle(image, kp->x, kp->y, 4, &cl); + } -// kp_t *kp = NULL; -// image_t *image = NULL; -// -// /* get C image pointer */ -// image = py_image_cobj(image_obj); -// -// /* get C cascade pointer */ -// kp = py_kp_cobj(kp_obj); -// -// kp_draw_ipts(image, kp->ipts); return mp_const_none; } @@ -590,10 +595,6 @@ static mp_obj_t py_image_find_template(mp_obj_t image_obj, mp_obj_t template_obj } return obj; } -//#define get_kw_arg(kw_map, kw_val, kw_def) -// ({ mp_map_elem_t *kw_arg = mp_map_lookup(kw_map, -// MP_OBJ_NEW_QSTR(qstr_from_str(kw_val)), MP_MAP_LOOKUP); -// _a < _b ? _a : _b; }) static mp_obj_t py_image_find_keypoints(uint n_args, const mp_obj_t *args, mp_map_t *kw_args) { @@ -668,39 +669,41 @@ static mp_obj_t py_image_match_keypoints(mp_obj_t image_obj, mp_obj_t kpts_obj, // match the keypoint sets int16_t *kpts_match = freak_match_keypoints(kpts1, kpts1_size, kpts2, kpts2_size, m_thresh); -// // do something with the match -// for (int i=0; ix, kp1->y, kp2->x, kp2->y); -// //int r = 4; -// // if (((kp->x+r) < image->w) && ((kp->y+r) < image->h)) { -// // imlib_draw_circle(image, kp->x, kp->y, r); -// // } -// } -// } - - // do something with the match - int cx=0, cy=0, match=0; +#if 1 + // do something with the matches for (int i=0; ix; - cy += kp2->y; + imlib_draw_line(image, image->w/2, image->h/2, kp2->x, kp2->y); + } + } +#else + int match=0; + rectangle_t r = {image->w-1, image->h-1, 0, 0}; + for (int i=0; ix < r.x) { + r.x = kp2->x; + } + if (kp2->y < r.y) { + r.y = kp2->y; + } + if (kp2->x > r.w) { + r.w = kp2->x; + } + if (kp2->y > r.h) { + r.h = kp2->y; + } match ++; } } - if (match ==0) { - match++; - } - cx /= match; - cy /= match; - int r = 5; - if (((cx+r) < image->w) && ((cy+r) < image->h)) { - imlib_draw_line(image, image->w/2, image->h/2, cx, cy); - imlib_draw_circle(image, cx, cy, r); + if (match>=(kpts1_size/4)) { + r.w = r.w - r.x; + r.h = r.h - r.y; + imlib_draw_rectangle(image, &r); } +#endif return mp_const_none; } @@ -744,6 +747,40 @@ mp_obj_t py_image_load_cascade(mp_obj_t path_obj) return o; } +mp_obj_t py_image_load_descriptor(mp_obj_t path_obj) +{ + kp_t *kpts=NULL; + int kpts_size =0; + + py_kp_obj_t *kp_obj =NULL; + const char *path = mp_obj_str_get_str(path_obj); + + int res = freak_load_descriptor(&kpts, &kpts_size, path); + if (res != FR_OK) { + nlr_raise(mp_obj_new_exception_msg(&mp_type_OSError, ffs_strerror(res))); + } + /* return keypoints MP object */ + kp_obj = m_new_obj(py_kp_obj_t); + kp_obj->base.type = &py_kp_type; + kp_obj->kpts = kpts; + kp_obj->size = kpts_size; + kp_obj->threshold = 60; + kp_obj->normalized = false; + return kp_obj; +} + +mp_obj_t py_image_save_descriptor(mp_obj_t path_obj, mp_obj_t kpts_obj) +{ + py_kp_obj_t *kpts = ((py_kp_obj_t*)kpts_obj); + const char *path = mp_obj_str_get_str(path_obj); + + int res = freak_save_descriptor(kpts->kpts, kpts->size, path); + if (res != FR_OK) { + nlr_raise(mp_obj_new_exception_msg(&mp_type_OSError, ffs_strerror(res))); + } + return mp_const_true; +} + STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_image_size_obj, py_image_size); STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_save_obj, 2, py_image_save); STATIC MP_DEFINE_CONST_FUN_OBJ_2(py_image_scale_obj, py_image_scale); diff --git a/src/omv/py/py_image.h b/src/omv/py/py_image.h index 62b1be86d..96e934e0c 100644 --- a/src/omv/py/py_image.h +++ b/src/omv/py/py_image.h @@ -3,6 +3,8 @@ #include "imlib.h" mp_obj_t py_image_load_image(mp_obj_t path_obj); mp_obj_t py_image_load_cascade(mp_obj_t path_obj); +mp_obj_t py_image_load_descriptor(mp_obj_t path_obj); +mp_obj_t py_image_save_descriptor(mp_obj_t path_obj, mp_obj_t kpts_obj); mp_obj_t py_image(int width, int height, int bpp, void *pixels); mp_obj_t py_image_from_struct(image_t *image); void *py_image_cobj(mp_obj_t image);