mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
AprilTags Fixes
* Made all memory allocations during the exhaustive time safe. * Added 3D pose output from the AprilTags code.
This commit is contained in:
parent
b22aadc21f
commit
cd07c0d5fa
@ -11363,22 +11363,32 @@ zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im)
|
||||
|
||||
////////////////////////////////////////////////////////
|
||||
// step 3. process each connected component.
|
||||
zarray_t *clusters = zarray_create(sizeof(zarray_t*)); //, uint64_zarray_hash_size(clustermap));
|
||||
if (1) {
|
||||
zarray_t *clusters = zarray_create_fail_ok(sizeof(zarray_t*)); //, uint64_zarray_hash_size(clustermap));
|
||||
if (clusters) {
|
||||
for (int i = 0; i < nclustermap; i++) {
|
||||
|
||||
for (struct uint64_zarray_entry *entry = clustermap[i]; entry; entry = entry->next) {
|
||||
// XXX reject clusters here?
|
||||
zarray_add(clusters, &entry->cluster);
|
||||
zarray_add_fail_ok(clusters, &entry->cluster);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int sz = clusters ? zarray_size(clusters) : 0;
|
||||
|
||||
if (1) {
|
||||
for (int i = 0; i < nclustermap; i++) {
|
||||
struct uint64_zarray_entry *entry = clustermap[i];
|
||||
while (entry) {
|
||||
// free any leaked cluster (zarray_add_fail_ok)
|
||||
bool leaked = true;
|
||||
for (int j = 0; j < sz; j++) {
|
||||
zarray_t *cluster;
|
||||
zarray_get(clusters, j, &cluster);
|
||||
leaked &= entry->cluster != cluster;
|
||||
}
|
||||
if (leaked) free(entry->cluster);
|
||||
struct uint64_zarray_entry *tmp = entry->next;
|
||||
free(entry);
|
||||
entry = tmp;
|
||||
@ -11392,48 +11402,52 @@ zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im)
|
||||
fb_free(); // threshim->buf
|
||||
fb_free(); // threshim
|
||||
|
||||
zarray_t *quads = zarray_create(sizeof(struct quad));
|
||||
zarray_t *quads = zarray_create_fail_ok(sizeof(struct quad));
|
||||
|
||||
int sz = zarray_size(clusters);
|
||||
if (quads) {
|
||||
for (int i = 0; i < sz; i++) {
|
||||
|
||||
for (int i = 0; i < sz; i++) {
|
||||
zarray_t *cluster;
|
||||
zarray_get(clusters, i, &cluster);
|
||||
|
||||
zarray_t *cluster;
|
||||
zarray_get(clusters, i, &cluster);
|
||||
if (zarray_size(cluster) < td->qtp.min_cluster_pixels)
|
||||
continue;
|
||||
|
||||
if (zarray_size(cluster) < td->qtp.min_cluster_pixels)
|
||||
continue;
|
||||
// a cluster should contain only boundary points around the
|
||||
// tag. it cannot be bigger than the whole screen. (Reject
|
||||
// large connected blobs that will be prohibitively slow to
|
||||
// fit quads to.) A typical point along an edge is added three
|
||||
// times (because it has 3 neighbors). The maximum perimeter
|
||||
// is 2w+2h.
|
||||
if (zarray_size(cluster) > 3*(2*w+2*h)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// a cluster should contain only boundary points around the
|
||||
// tag. it cannot be bigger than the whole screen. (Reject
|
||||
// large connected blobs that will be prohibitively slow to
|
||||
// fit quads to.) A typical point along an edge is added three
|
||||
// times (because it has 3 neighbors). The maximum perimeter
|
||||
// is 2w+2h.
|
||||
if (zarray_size(cluster) > 3*(2*w+2*h)) {
|
||||
continue;
|
||||
}
|
||||
struct quad quad;
|
||||
memset(&quad, 0, sizeof(struct quad));
|
||||
|
||||
struct quad quad;
|
||||
memset(&quad, 0, sizeof(struct quad));
|
||||
if (fit_quad(td, im, cluster, &quad)) {
|
||||
|
||||
if (fit_quad(td, im, cluster, &quad)) {
|
||||
|
||||
zarray_add(quads, &quad);
|
||||
zarray_add_fail_ok(quads, &quad);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// printf(" %d %d %d %d\n", indices[0], indices[1], indices[2], indices[3]);
|
||||
|
||||
for (int i = 0; i < zarray_size(clusters); i++) {
|
||||
for (int i = 0; i < sz; i++) {
|
||||
zarray_t *cluster;
|
||||
zarray_get(clusters, i, &cluster);
|
||||
zarray_destroy(cluster);
|
||||
}
|
||||
|
||||
zarray_destroy(clusters);
|
||||
if (clusters) zarray_destroy(clusters);
|
||||
|
||||
|
||||
if (!quads) {
|
||||
// we should have enough memory now
|
||||
quads = zarray_create(sizeof(struct quad));
|
||||
}
|
||||
return quads;
|
||||
}
|
||||
|
||||
@ -11608,11 +11622,13 @@ int popcount64c(uint64_t x)
|
||||
static void quick_decode_codeword(apriltag_family_t *tf, uint64_t rcode,
|
||||
struct quick_decode_entry *entry)
|
||||
{
|
||||
int threshold = imax(tf->h - tf->d - 1, 0);
|
||||
|
||||
for (int ridx = 0; ridx < 4; ridx++) {
|
||||
|
||||
for (int i = 0, j = tf->ncodes; i < j; i++) {
|
||||
int hamming = popcount64c(tf->codes[i] ^ rcode);
|
||||
if(hamming <= 2) {
|
||||
if(hamming <= threshold) {
|
||||
entry->rcode = rcode;
|
||||
entry->id = i;
|
||||
entry->hamming = hamming;
|
||||
@ -12489,7 +12505,8 @@ void apriltag_detections_destroy(zarray_t *detections)
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
void imlib_find_apriltags(list_t *out, image_t *ptr, rectangle_t *roi, apriltag_families_t families)
|
||||
void imlib_find_apriltags(list_t *out, image_t *ptr, rectangle_t *roi, apriltag_families_t families,
|
||||
float fx, float fy, float cx, float cy)
|
||||
{
|
||||
umm_init_x(roi->w, roi->h);
|
||||
apriltag_detector_t *td = apriltag_detector_create();
|
||||
@ -12576,6 +12593,7 @@ void imlib_find_apriltags(list_t *out, image_t *ptr, rectangle_t *roi, apriltag_
|
||||
rectangle_united(&(lnk_data.rect), &temp);
|
||||
}
|
||||
|
||||
lnk_data.id = det->id;
|
||||
lnk_data.family = 0;
|
||||
|
||||
if(det->family == &tag16h5) {
|
||||
@ -12602,29 +12620,23 @@ void imlib_find_apriltags(list_t *out, image_t *ptr, rectangle_t *roi, apriltag_
|
||||
lnk_data.family |= ARTOOLKIT;
|
||||
}
|
||||
|
||||
lnk_data.id = det->id;
|
||||
lnk_data.hamming = det->hamming;
|
||||
lnk_data.centroid.x = fast_roundf(det->c[0]) + roi->x;
|
||||
lnk_data.centroid.y = fast_roundf(det->c[1]) + roi->y;
|
||||
|
||||
float fx = (2.8 / 3.984) * 656; // 2.8mm Focal Length w/ OV7725 sensor for reference.
|
||||
float fy = (2.8 / 2.952) * 488; // 2.8mm Focal Length w/ OV7725 sensor for reference.
|
||||
float cx = ptr->w / 0.5; // Use the image versus the roi here since the image should be projected from the camera center.
|
||||
float cy = ptr->h / 0.5; // Use the image versus the roi here since the image should be projected from the camera center.
|
||||
lnk_data.goodness = det->goodness / 255.0; // scale to [0:1]
|
||||
lnk_data.decision_margin = det->decision_margin / 255.0; // scale to [0:1]
|
||||
|
||||
matd_t *pose = homography_to_pose(det->H, -fx, fy, cx, cy);
|
||||
|
||||
float transX = MATD_EL(pose, 0, 3);
|
||||
float transY = MATD_EL(pose, 1, 3);
|
||||
float transZ = MATD_EL(pose, 2, 3);
|
||||
float phetaX = fast_atan2f(MATD_EL(pose, 2, 1), MATD_EL(pose, 2, 2));
|
||||
float phetaY = fast_atan2f(-MATD_EL(pose, 2, 0), fast_sqrtf(sq(MATD_EL(pose, 2, 1)) + sq(MATD_EL(pose, 2, 2))));
|
||||
float phetaZ = fast_atan2f(MATD_EL(pose, 1, 0), MATD_EL(pose, 0, 0));
|
||||
lnk_data.x_translation = MATD_EL(pose, 0, 3);
|
||||
lnk_data.y_translation = MATD_EL(pose, 1, 3);
|
||||
lnk_data.z_translation = MATD_EL(pose, 2, 3);
|
||||
lnk_data.x_rotation = fast_atan2f(MATD_EL(pose, 2, 1), MATD_EL(pose, 2, 2));
|
||||
lnk_data.y_rotation = fast_atan2f(-MATD_EL(pose, 2, 0), fast_sqrtf(sq(MATD_EL(pose, 2, 1)) + sq(MATD_EL(pose, 2, 2))));
|
||||
lnk_data.z_rotation = fast_atan2f(MATD_EL(pose, 1, 0), MATD_EL(pose, 0, 0));
|
||||
|
||||
matd_destroy(pose);
|
||||
|
||||
lnk_data.rotation = phetaZ; // since we only use phetaZ above the scale of fx/fy/cx/cy don't matter and just need to be good ratios
|
||||
lnk_data.decision_margin = det->decision_margin / 255.0; // scale to [0:1]
|
||||
|
||||
list_push_back(out, &lnk_data);
|
||||
}
|
||||
|
||||
|
||||
@ -905,9 +905,12 @@ apriltag_families_t;
|
||||
typedef struct find_apriltags_list_lnk_data
|
||||
{
|
||||
rectangle_t rect;
|
||||
uint16_t family, id;
|
||||
uint16_t id;
|
||||
uint8_t family, hamming;
|
||||
point_t centroid;
|
||||
float rotation, decision_margin;
|
||||
float goodness, decision_margin;
|
||||
float x_translation, y_translation, z_translation;
|
||||
float x_rotation, y_rotation, z_rotation;
|
||||
}
|
||||
find_apriltags_list_lnk_data_t;
|
||||
|
||||
@ -1088,6 +1091,7 @@ void imlib_find_blobs(list_t *out, image_t *ptr, rectangle_t *roi, unsigned int
|
||||
bool merge, int margin);
|
||||
// 1/2D Bar Codes
|
||||
void imlib_find_qrcodes(list_t *out, image_t *ptr, rectangle_t *roi);
|
||||
void imlib_find_apriltags(list_t *out, image_t *ptr, rectangle_t *roi, apriltag_families_t families);
|
||||
void imlib_find_apriltags(list_t *out, image_t *ptr, rectangle_t *roi, apriltag_families_t families,
|
||||
float fx, float fy, float cx, float cy);
|
||||
|
||||
#endif //__IMLIB_H__
|
||||
|
||||
@ -1929,17 +1929,21 @@ static mp_obj_t py_image_find_qrcodes(uint n_args, const mp_obj_t *args, mp_map_
|
||||
}
|
||||
|
||||
// AprilTag Object //
|
||||
#define py_apriltag_obj_size 10
|
||||
#define py_apriltag_obj_size 18
|
||||
typedef struct py_apriltag_obj {
|
||||
mp_obj_base_t base;
|
||||
mp_obj_t x, y, w, h, id, family, cx, cy, rotation, decision_margin;
|
||||
mp_obj_t x, y, w, h, id, family, cx, cy, rotation, decision_margin, hamming, goodness;
|
||||
mp_obj_t x_translation, y_translation, z_translation;
|
||||
mp_obj_t x_rotation, y_rotation, z_rotation;
|
||||
} py_apriltag_obj_t;
|
||||
|
||||
static void py_apriltag_print(const mp_print_t *print, mp_obj_t self_in, mp_print_kind_t kind)
|
||||
{
|
||||
py_apriltag_obj_t *self = self_in;
|
||||
mp_printf(print,
|
||||
"{x:%d, y:%d, w:%d, h:%d, id:%d, family:%d, cx:%d, cy:%d, rotation:%f, decision_margin:%f}",
|
||||
"{x:%d, y:%d, w:%d, h:%d, id:%d, family:%d, cx:%d, cy:%d, rotation:%f, decision_margin:%f, hamming:%d, goodness:%f,"
|
||||
" x_translation:%f, y_translation:%f, z_translation:%f,"
|
||||
" x_rotation:%f, y_rotation:%f, z_rotation:%f}",
|
||||
mp_obj_get_int(self->x),
|
||||
mp_obj_get_int(self->y),
|
||||
mp_obj_get_int(self->w),
|
||||
@ -1949,7 +1953,15 @@ static void py_apriltag_print(const mp_print_t *print, mp_obj_t self_in, mp_prin
|
||||
mp_obj_get_int(self->cx),
|
||||
mp_obj_get_int(self->cy),
|
||||
(double) mp_obj_get_float(self->rotation),
|
||||
(double) mp_obj_get_float(self->decision_margin));
|
||||
(double) mp_obj_get_float(self->decision_margin),
|
||||
mp_obj_get_int(self->hamming),
|
||||
(double) mp_obj_get_float(self->goodness),
|
||||
(double) mp_obj_get_float(self->x_translation),
|
||||
(double) mp_obj_get_float(self->y_translation),
|
||||
(double) mp_obj_get_float(self->z_translation),
|
||||
(double) mp_obj_get_float(self->x_rotation),
|
||||
(double) mp_obj_get_float(self->y_rotation),
|
||||
(double) mp_obj_get_float(self->z_rotation));
|
||||
}
|
||||
|
||||
static mp_obj_t py_apriltag_subscr(mp_obj_t self_in, mp_obj_t index, mp_obj_t value)
|
||||
@ -1976,6 +1988,14 @@ static mp_obj_t py_apriltag_subscr(mp_obj_t self_in, mp_obj_t index, mp_obj_t va
|
||||
case 7: return self->cy;
|
||||
case 8: return self->rotation;
|
||||
case 9: return self->decision_margin;
|
||||
case 10: return self->hamming;
|
||||
case 11: return self->goodness;
|
||||
case 12: return self->x_translation;
|
||||
case 13: return self->y_translation;
|
||||
case 14: return self->z_translation;
|
||||
case 15: return self->x_rotation;
|
||||
case 16: return self->y_rotation;
|
||||
case 17: return self->z_rotation;
|
||||
}
|
||||
}
|
||||
return MP_OBJ_NULL; // op not supported
|
||||
@ -1999,6 +2019,14 @@ mp_obj_t py_apriltag_cx(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_i
|
||||
mp_obj_t py_apriltag_cy(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->cy; }
|
||||
mp_obj_t py_apriltag_rotation(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->rotation; }
|
||||
mp_obj_t py_apriltag_decision_margin(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->decision_margin; }
|
||||
mp_obj_t py_apriltag_hamming(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->hamming; }
|
||||
mp_obj_t py_apriltag_goodness(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->goodness; }
|
||||
mp_obj_t py_apriltag_x_translation(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->x_translation; }
|
||||
mp_obj_t py_apriltag_y_translation(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->y_translation; }
|
||||
mp_obj_t py_apriltag_z_translation(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->z_translation; }
|
||||
mp_obj_t py_apriltag_x_rotation(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->x_rotation; }
|
||||
mp_obj_t py_apriltag_y_rotation(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->y_rotation; }
|
||||
mp_obj_t py_apriltag_z_rotation(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->z_rotation; }
|
||||
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_rect_obj, py_apriltag_rect);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_x_obj, py_apriltag_x);
|
||||
@ -2011,6 +2039,14 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_cx_obj, py_apriltag_cx);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_cy_obj, py_apriltag_cy);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_rotation_obj, py_apriltag_rotation);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_decision_margin_obj, py_apriltag_decision_margin);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_hamming_obj, py_apriltag_hamming);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_goodness_obj, py_apriltag_goodness);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_x_translation_obj, py_apriltag_x_translation);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_y_translation_obj, py_apriltag_y_translation);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_z_translation_obj, py_apriltag_z_translation);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_x_rotation_obj, py_apriltag_x_rotation);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_y_rotation_obj, py_apriltag_y_rotation);
|
||||
STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_z_rotation_obj, py_apriltag_z_rotation);
|
||||
|
||||
STATIC const mp_rom_map_elem_t py_apriltag_locals_dict_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR_rect), MP_ROM_PTR(&py_apriltag_rect_obj) },
|
||||
@ -2024,6 +2060,14 @@ STATIC const mp_rom_map_elem_t py_apriltag_locals_dict_table[] = {
|
||||
{ MP_ROM_QSTR(MP_QSTR_cy), MP_ROM_PTR(&py_apriltag_cy_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_rotation), MP_ROM_PTR(&py_apriltag_rotation_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_decision_margin), MP_ROM_PTR(&py_apriltag_decision_margin_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_hamming), MP_ROM_PTR(&py_apriltag_hamming_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_goodness), MP_ROM_PTR(&py_apriltag_goodness_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_x_translation), MP_ROM_PTR(&py_apriltag_x_translation_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_y_translation), MP_ROM_PTR(&py_apriltag_y_translation_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_z_translation), MP_ROM_PTR(&py_apriltag_z_translation_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_x_rotation), MP_ROM_PTR(&py_apriltag_x_rotation_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_y_rotation), MP_ROM_PTR(&py_apriltag_y_rotation_obj) },
|
||||
{ MP_ROM_QSTR(MP_QSTR_z_rotation), MP_ROM_PTR(&py_apriltag_z_rotation_obj) },
|
||||
};
|
||||
|
||||
STATIC MP_DEFINE_CONST_DICT(py_apriltag_locals_dict, py_apriltag_locals_dict_table);
|
||||
@ -2045,9 +2089,19 @@ static mp_obj_t py_image_find_apriltags(uint n_args, const mp_obj_t *args, mp_ma
|
||||
rectangle_t roi;
|
||||
py_helper_lookup_rectangle(kw_args, arg_img, &roi);
|
||||
|
||||
apriltag_families_t families = py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_families), TAG36H11);
|
||||
// 2.8mm Focal Length w/ OV7725 sensor for reference.
|
||||
float fx = py_helper_lookup_float(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_fx), (2.8 / 3.984) * 656);
|
||||
// 2.8mm Focal Length w/ OV7725 sensor for reference.
|
||||
float fy = py_helper_lookup_float(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_fy), (2.8 / 2.952) * 488);
|
||||
// Use the image versus the roi here since the image should be projected from the camera center.
|
||||
float cx = py_helper_lookup_float(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_cx), arg_img->w * 0.5);
|
||||
// Use the image versus the roi here since the image should be projected from the camera center.
|
||||
float cy = py_helper_lookup_float(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_cy), arg_img->h * 0.5);
|
||||
|
||||
list_t out;
|
||||
fb_alloc_mark();
|
||||
imlib_find_apriltags(&out, arg_img, &roi, py_helper_lookup_int(kw_args, MP_OBJ_NEW_QSTR(MP_QSTR_families), TAG36H11));
|
||||
imlib_find_apriltags(&out, arg_img, &roi, families, fx, fy, cx, cy);
|
||||
fb_alloc_free_till_mark();
|
||||
|
||||
mp_obj_list_t *objects_list = mp_obj_new_list(list_size(&out), NULL);
|
||||
@ -2065,8 +2119,16 @@ static mp_obj_t py_image_find_apriltags(uint n_args, const mp_obj_t *args, mp_ma
|
||||
o->family = mp_obj_new_int(lnk_data.family);
|
||||
o->cx = mp_obj_new_int(lnk_data.centroid.x);
|
||||
o->cy = mp_obj_new_int(lnk_data.centroid.y);
|
||||
o->rotation = mp_obj_new_float(lnk_data.rotation);
|
||||
o->rotation = mp_obj_new_float(lnk_data.z_rotation);
|
||||
o->decision_margin = mp_obj_new_float(lnk_data.decision_margin);
|
||||
o->hamming = mp_obj_new_int(lnk_data.hamming);
|
||||
o->goodness = mp_obj_new_float(lnk_data.goodness);
|
||||
o->x_translation = mp_obj_new_float(lnk_data.x_translation);
|
||||
o->y_translation = mp_obj_new_float(lnk_data.y_translation);
|
||||
o->z_translation = mp_obj_new_float(lnk_data.z_translation);
|
||||
o->x_rotation = mp_obj_new_float(lnk_data.x_rotation);
|
||||
o->y_rotation = mp_obj_new_float(lnk_data.y_rotation);
|
||||
o->z_rotation = mp_obj_new_float(lnk_data.z_rotation);
|
||||
|
||||
objects_list->items[i] = o;
|
||||
}
|
||||
|
||||
@ -419,8 +419,12 @@ Q(is_kanji)
|
||||
|
||||
// Find AprilTags
|
||||
Q(find_apriltags)
|
||||
Q(families)
|
||||
// duplicate Q(roi)
|
||||
Q(families)
|
||||
Q(fx)
|
||||
Q(fy)
|
||||
// duplicate Q(cx)
|
||||
// duplicate Q(cy)
|
||||
Q(apriltag)
|
||||
// duplicate Q(rect)
|
||||
// duplicate Q(x)
|
||||
@ -429,10 +433,18 @@ Q(apriltag)
|
||||
// duplicate Q(h)
|
||||
Q(id)
|
||||
Q(family)
|
||||
Q(hamming)
|
||||
// duplicate Q(cx)
|
||||
// duplicate Q(cy)
|
||||
// duplicate Q(rotation)
|
||||
Q(goodness)
|
||||
Q(decision_margin)
|
||||
Q(x_translation)
|
||||
Q(y_translation)
|
||||
Q(z_translation)
|
||||
Q(x_rotation)
|
||||
Q(y_rotation)
|
||||
Q(z_rotation)
|
||||
Q(TAG16H5)
|
||||
Q(TAG25H7)
|
||||
Q(TAG25H9)
|
||||
|
||||
@ -3,7 +3,7 @@
|
||||
# This example shows the power of the OpenMV Cam to detect April Tags
|
||||
# on the OpenMV Cam M7. The M4 versions cannot detect April Tags.
|
||||
|
||||
import sensor, image, time
|
||||
import sensor, image, time, math
|
||||
|
||||
sensor.reset()
|
||||
sensor.set_pixformat(sensor.RGB565)
|
||||
@ -17,6 +17,7 @@ clock = time.clock()
|
||||
|
||||
# The apriltag code supports up to 6 tag families which can be processed at the same time.
|
||||
# Returned tag objects will have their tag family and id within the tag family.
|
||||
|
||||
tag_families = 0
|
||||
tag_families |= image.TAG16H5 # comment out to disable this family
|
||||
tag_families |= image.TAG25H7 # comment out to disable this family
|
||||
@ -51,5 +52,6 @@ while(True):
|
||||
for tag in img.find_apriltags(families=tag_families): # defaults to TAG36H11 without "families".
|
||||
img.draw_rectangle(tag.rect(), color = (255, 0, 0))
|
||||
img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
|
||||
print("Tag Family %s, Tag ID %d" % (family_name(tag), tag.id()))
|
||||
print_args = (family_name(tag), tag.id(), (180 * tag.rotation()) / math.pi)
|
||||
print("Tag Family %s, Tag ID %d, rotation %f (degrees)" % print_args)
|
||||
print(clock.fps())
|
||||
|
||||
57
usr/examples/16-Codes/find_apriltags_3d_pose.py
Normal file
57
usr/examples/16-Codes/find_apriltags_3d_pose.py
Normal file
@ -0,0 +1,57 @@
|
||||
# AprilTags Example
|
||||
#
|
||||
# This example shows the power of the OpenMV Cam to detect April Tags
|
||||
# on the OpenMV Cam M7. The M4 versions cannot detect April Tags.
|
||||
|
||||
import sensor, image, time, math
|
||||
|
||||
sensor.reset()
|
||||
sensor.set_pixformat(sensor.RGB565)
|
||||
sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
|
||||
sensor.skip_frames(30)
|
||||
sensor.set_auto_gain(False) # must turn this off to prevent image washout...
|
||||
sensor.set_auto_whitebal(False) # must turn this off to prevent image washout...
|
||||
clock = time.clock()
|
||||
|
||||
# Note! Unlike find_qrcodes the find_apriltags method does not need lens correction on the image to work.
|
||||
|
||||
# What's the difference between tag families? Well, for example, the TAG16H5 family is effectively
|
||||
# a 4x4 square tag. So, this means it can be seen at a longer distance than a TAG36H11 tag which
|
||||
# is a 6x6 square tag. However, the lower H value (H5 versus H11) means that the false positve
|
||||
# rate for the 4x4 tag is much, much, much, higher than the 6x6 tag. So, unless you have a
|
||||
# reason to use the other tags families just use TAG36H11 which is the default family.
|
||||
|
||||
# The AprilTags library outputs the pose information for tags. This is the x/y/z translation and
|
||||
# x/y/z rotation. The x/y/z rotation is in radians and can be converted to degrees. As for
|
||||
# translation the units are dimensionless and you must apply a conversion function.
|
||||
|
||||
# f_x is the x focal length of the camera. It should be equal to the lens focal length in mm
|
||||
# divided by the x sensor size in mm times the number of pixels for the sensor length.
|
||||
# The below values are for the OV7725 camera with a 2.8 mm lens.
|
||||
|
||||
# f_y is the y focal length of the camera. It should be equal to the lens focal length in mm
|
||||
# divided by the y sensor size in mm times the number of pixels for the sensor length.
|
||||
# The below values are for the OV7725 camera with a 2.8 mm lens.
|
||||
|
||||
# c_x is the image x center position in pixels.
|
||||
# c_y is the image y center position in pixels.
|
||||
|
||||
f_x = (2.8 / 3.984) * 656 # find_apriltags defaults to this if not set
|
||||
f_y = (2.8 / 2.952) * 488 # find_apriltags defaults to this if not set
|
||||
c_x = 160 * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)
|
||||
c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)
|
||||
|
||||
def degrees(radians):
|
||||
return (180 * radians) / math.pi
|
||||
|
||||
while(True):
|
||||
clock.tick()
|
||||
img = sensor.snapshot()
|
||||
for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # defaults to TAG36H11
|
||||
img.draw_rectangle(tag.rect(), color = (255, 0, 0))
|
||||
img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
|
||||
print_args = (tag.x_translation(), tag.y_translation(), tag.z_translation(), \
|
||||
degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
|
||||
# Translation units are unknown. Rotation units are in degrees.
|
||||
print("Tx: %f, Ty %f, Tz %f, Rx %f, Ry %f, Rz %f" % print_args)
|
||||
print(clock.fps())
|
||||
@ -29,5 +29,5 @@ while(True):
|
||||
img.draw_rectangle(tag.rect(), color = (255, 0, 0))
|
||||
img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
|
||||
print_args = (tag.id(), (180 * tag.rotation()) / math.pi)
|
||||
print("Tag Family TAG36H11, Tag ID %d - rotation %f (degrees)" % print_args)
|
||||
print("Tag Family TAG36H11, Tag ID %d, rotation %f (degrees)" % print_args)
|
||||
print(clock.fps())
|
||||
|
||||
Loading…
Reference in New Issue
Block a user