Misc Fixes, add load/save descriptor

This commit is contained in:
iabdalkader 2014-09-09 15:06:08 +02:00
parent 06c8f77662
commit 903f756a7c
6 changed files with 236 additions and 91 deletions

View File

@ -32,8 +32,8 @@
#include <string.h>
#include <stdint.h>
#include <stdbool.h>
#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; i<kpts_size; i++) {
kp_t *kp = &kpts[i];
/* write angle */
res = f_write(&fp, &kp->angle, 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; i<kpts_size; i++) {
kp_t *kp = &kpts[i];
res = f_read(&fp, kp->desc, 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;
}

View File

@ -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; y<src->h+y_off; y++) {
for (x=x_off; x<src->w+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; y<src->h+y_off; y++) {
for (x=x_off; x<src->w+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;

View File

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

View File

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

View File

@ -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; i<kpts_size; i++) {
// kp_t *kp = &kpts[i];
// if (kp->x&&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; i<kpts->size; 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; i<kpts1_size; i++) {
// if (kpts_match[i] != -1) {
// kp_t *kp1 = &kpts1[i];
// kp_t *kp2 = &kpts2[kpts_match[i]];
// imlib_draw_line(image, kp1->x, 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; i<kpts1_size; i++) {
if (kpts_match[i] != -1) {
kp_t *kp2 = &kpts2[kpts_match[i]];
cx += kp2->x;
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; i<kpts1_size; i++) {
if (kpts_match[i] != -1) {
kp_t *kp2 = &kpts2[kpts_match[i]];
if (kp2->x < 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);

View File

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