AprilTags Fixes

* Made all memory allocations during the exhaustive time safe.
* Added 3D pose output from the AprilTags code.
This commit is contained in:
Kwabena W. Agyeman 2017-02-14 18:36:52 -05:00
parent b22aadc21f
commit cd07c0d5fa
7 changed files with 205 additions and 56 deletions

View File

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

View File

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

View File

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

View File

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

View File

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

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

View File

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