mirror of
https://github.com/openmv/openmv.git
synced 2025-11-04 14:49:50 +08:00
imlib/apriltag: Disable unused TAG families by default.
This commit is contained in:
parent
d4a456edff
commit
effe12f2cc
@ -8,7 +8,6 @@
|
||||
# on the OpenMV Cam M7. The M4 versions cannot detect April Tags.
|
||||
|
||||
import sensor
|
||||
import image
|
||||
import time
|
||||
import math
|
||||
|
||||
@ -22,47 +21,15 @@ clock = time.clock()
|
||||
|
||||
# Note! Unlike find_qrcodes the find_apriltags method does not need lens correction on the image to work.
|
||||
|
||||
# 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
|
||||
tag_families |= image.TAG25H9 # comment out to disable this family
|
||||
tag_families |= image.TAG36H10 # comment out to disable this family
|
||||
tag_families |= image.TAG36H11 # comment out to disable this family (default family)
|
||||
tag_families |= image.ARTOOLKIT # comment out to disable this family
|
||||
|
||||
# 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 positive
|
||||
# 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.
|
||||
|
||||
|
||||
def family_name(tag):
|
||||
if tag.family() == image.TAG16H5:
|
||||
return "TAG16H5"
|
||||
if tag.family() == image.TAG25H7:
|
||||
return "TAG25H7"
|
||||
if tag.family() == image.TAG25H9:
|
||||
return "TAG25H9"
|
||||
if tag.family() == image.TAG36H10:
|
||||
return "TAG36H10"
|
||||
if tag.family() == image.TAG36H11:
|
||||
return "TAG36H11"
|
||||
if tag.family() == image.ARTOOLKIT:
|
||||
return "ARTOOLKIT"
|
||||
# Please use the TAG36H11 tag family for this script - it's the recommended tag family to use.
|
||||
|
||||
|
||||
while True:
|
||||
clock.tick()
|
||||
img = sensor.snapshot()
|
||||
for tag in img.find_apriltags(
|
||||
families=tag_families
|
||||
): # defaults to TAG36H11 without "families".
|
||||
for tag in img.find_apriltags():
|
||||
img.draw_rectangle(tag.rect(), color=(255, 0, 0))
|
||||
img.draw_cross(tag.cx(), tag.cy(), color=(0, 255, 0))
|
||||
print_args = (family_name(tag), tag.id(), (180 * tag.rotation()) / math.pi)
|
||||
print_args = ("TAG36H11", tag.id(), (180 * tag.rotation()) / math.pi)
|
||||
print("Tag Family %s, Tag ID %d, rotation %f (degrees)" % print_args)
|
||||
print(clock.fps())
|
||||
|
||||
@ -8,7 +8,6 @@
|
||||
# on the OpenMV Cam M7. The M4 versions cannot detect April Tags.
|
||||
|
||||
import sensor
|
||||
import image
|
||||
import time
|
||||
import math
|
||||
import omv
|
||||
@ -31,47 +30,15 @@ clock = time.clock()
|
||||
|
||||
# Note! Unlike find_qrcodes the find_apriltags method does not need lens correction on the image to work.
|
||||
|
||||
# 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
|
||||
tag_families |= image.TAG25H9 # comment out to disable this family
|
||||
tag_families |= image.TAG36H10 # comment out to disable this family
|
||||
tag_families |= image.TAG36H11 # comment out to disable this family (default family)
|
||||
tag_families |= image.ARTOOLKIT # comment out to disable this family
|
||||
|
||||
# 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 positive
|
||||
# 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.
|
||||
|
||||
|
||||
def family_name(tag):
|
||||
if tag.family() == image.TAG16H5:
|
||||
return "TAG16H5"
|
||||
if tag.family() == image.TAG25H7:
|
||||
return "TAG25H7"
|
||||
if tag.family() == image.TAG25H9:
|
||||
return "TAG25H9"
|
||||
if tag.family() == image.TAG36H10:
|
||||
return "TAG36H10"
|
||||
if tag.family() == image.TAG36H11:
|
||||
return "TAG36H11"
|
||||
if tag.family() == image.ARTOOLKIT:
|
||||
return "ARTOOLKIT"
|
||||
# Please use the TAG36H11 tag family for this script - it's the recommended tag family to use.
|
||||
|
||||
|
||||
while True:
|
||||
clock.tick()
|
||||
img = sensor.snapshot()
|
||||
for tag in img.find_apriltags(
|
||||
families=tag_families
|
||||
): # defaults to TAG36H11 without "families".
|
||||
for tag in img.find_apriltags():
|
||||
img.draw_rectangle(tag.rect(), color=127)
|
||||
img.draw_cross(tag.cx(), tag.cy(), color=127)
|
||||
print_args = (family_name(tag), tag.id(), (180 * tag.rotation()) / math.pi)
|
||||
print_args = ("TAG36H11", tag.id(), (180 * tag.rotation()) / math.pi)
|
||||
print("Tag Family %s, Tag ID %d, rotation %f (degrees)" % print_args)
|
||||
print(clock.fps())
|
||||
|
||||
@ -13,8 +13,9 @@
|
||||
# pass the thresholding test... otherwise, you don't get a distance
|
||||
# benefit.
|
||||
|
||||
# Please use the TAG36H11 tag family for this script - it's the recommended tag family to use.
|
||||
|
||||
import sensor
|
||||
import image
|
||||
import time
|
||||
import omv
|
||||
|
||||
@ -34,16 +35,6 @@ sensor.set_auto_gain(False) # must be turned off for color tracking
|
||||
sensor.set_auto_whitebal(False) # must be turned off for color tracking
|
||||
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
|
||||
tag_families |= image.TAG25H9 # comment out to disable this family
|
||||
tag_families |= image.TAG36H10 # comment out to disable this family
|
||||
tag_families |= image.TAG36H11 # comment out to disable this family (default family)
|
||||
tag_families |= image.ARTOOLKIT # comment out to disable this family
|
||||
|
||||
while True:
|
||||
clock.tick()
|
||||
img = sensor.snapshot()
|
||||
@ -68,7 +59,7 @@ while True:
|
||||
# Since we constrict the roi size apriltags shouldn't run out of ram.
|
||||
# But, if it does we handle it...
|
||||
try:
|
||||
tag_list.extend(img.find_apriltags(roi=(x, y, w, h), families=tag_families))
|
||||
tag_list.extend(img.find_apriltags(roi=(x, y, w, h)))
|
||||
except (
|
||||
MemoryError
|
||||
): # Don't catch all exceptions otherwise you can't stop the script.
|
||||
|
||||
@ -96,6 +96,7 @@
|
||||
|
||||
// Enable find_apriltags() (64 KB)
|
||||
#define IMLIB_ENABLE_APRILTAGS
|
||||
#define IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
|
||||
// Enable fine find_apriltags() - (8-way connectivity versus 4-way connectivity)
|
||||
// #define IMLIB_ENABLE_FINE_APRILTAGS
|
||||
|
||||
@ -96,6 +96,7 @@
|
||||
|
||||
// Enable find_apriltags() (64 KB)
|
||||
#define IMLIB_ENABLE_APRILTAGS
|
||||
#define IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
|
||||
// Enable fine find_apriltags() - (8-way connectivity versus 4-way connectivity)
|
||||
// #define IMLIB_ENABLE_FINE_APRILTAGS
|
||||
|
||||
@ -96,6 +96,7 @@
|
||||
|
||||
// Enable find_apriltags() (64 KB)
|
||||
#define IMLIB_ENABLE_APRILTAGS
|
||||
#define IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
|
||||
// Enable fine find_apriltags() - (8-way connectivity versus 4-way connectivity)
|
||||
// #define IMLIB_ENABLE_FINE_APRILTAGS
|
||||
|
||||
@ -96,6 +96,7 @@
|
||||
|
||||
// Enable find_apriltags() (64 KB)
|
||||
#define IMLIB_ENABLE_APRILTAGS
|
||||
#define IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
|
||||
// Enable fine find_apriltags() - (8-way connectivity versus 4-way connectivity)
|
||||
// #define IMLIB_ENABLE_FINE_APRILTAGS
|
||||
|
||||
@ -96,6 +96,7 @@
|
||||
|
||||
// Enable find_apriltags() (64 KB)
|
||||
#define IMLIB_ENABLE_APRILTAGS
|
||||
#define IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
|
||||
// Enable fine find_apriltags() - (8-way connectivity versus 4-way connectivity)
|
||||
// #define IMLIB_ENABLE_FINE_APRILTAGS
|
||||
|
||||
@ -96,6 +96,7 @@
|
||||
|
||||
// Enable find_apriltags() (64 KB)
|
||||
#define IMLIB_ENABLE_APRILTAGS
|
||||
#define IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
|
||||
// Enable fine find_apriltags() - (8-way connectivity versus 4-way connectivity)
|
||||
// #define IMLIB_ENABLE_FINE_APRILTAGS
|
||||
|
||||
@ -96,6 +96,7 @@
|
||||
|
||||
// Enable find_apriltags() (64 KB)
|
||||
#define IMLIB_ENABLE_APRILTAGS
|
||||
#define IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
|
||||
// Enable fine find_apriltags() - (8-way connectivity versus 4-way connectivity)
|
||||
// #define IMLIB_ENABLE_FINE_APRILTAGS
|
||||
|
||||
@ -96,6 +96,7 @@
|
||||
|
||||
// Enable find_apriltags() (64 KB)
|
||||
#define IMLIB_ENABLE_APRILTAGS
|
||||
#define IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
|
||||
// Enable fine find_apriltags() - (8-way connectivity versus 4-way connectivity)
|
||||
// #define IMLIB_ENABLE_FINE_APRILTAGS
|
||||
|
||||
@ -95,6 +95,12 @@
|
||||
|
||||
// Enable find_apriltags() (64 KB)
|
||||
#define IMLIB_ENABLE_APRILTAGS
|
||||
#define IMLIB_ENABLE_APRILTAGS_TAG16H5
|
||||
#define IMLIB_ENABLE_APRILTAGS_TAG25H7
|
||||
#define IMLIB_ENABLE_APRILTAGS_TAG25H9
|
||||
#define IMLIB_ENABLE_APRILTAGS_TAG36H10
|
||||
#define IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
#define IMLIB_ENABLE_APRILTAGS_ARTOOLKIT
|
||||
|
||||
// Enable fine find_apriltags() - (8-way connectivity versus 4-way connectivity)
|
||||
// #define IMLIB_ENABLE_FINE_APRILTAGS
|
||||
|
||||
@ -5313,6 +5313,7 @@ void apriltag_detections_destroy(zarray_t *detections);
|
||||
//////// "tag16h5"
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG16H5
|
||||
const apriltag_family_t tag16h5 = {
|
||||
.ncodes = 30,
|
||||
.black_border = 1,
|
||||
@ -5351,11 +5352,13 @@ const apriltag_family_t tag16h5 = {
|
||||
0x000000000000af2eUL
|
||||
}
|
||||
};
|
||||
#endif // IMLIB_ENABLE_APRILTAGS_TAG16H5
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////// "tag25h7"
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG25H7
|
||||
const apriltag_family_t tag25h7 = {
|
||||
.ncodes = 242,
|
||||
.black_border = 1,
|
||||
@ -5606,11 +5609,13 @@ const apriltag_family_t tag25h7 = {
|
||||
0x0000000001233a70UL
|
||||
}
|
||||
};
|
||||
#endif // IMLIB_ENABLE_APRILTAGS_TAG25H7
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////// "tag25h9"
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG25H9
|
||||
const apriltag_family_t tag25h9 = {
|
||||
.ncodes = 35,
|
||||
.black_border = 1,
|
||||
@ -5654,11 +5659,13 @@ const apriltag_family_t tag25h9 = {
|
||||
0x0000000000b991ceUL
|
||||
}
|
||||
};
|
||||
#endif // IMLIB_ENABLE_APRILTAGS_TAG25H9
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////// "tag36h10"
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG36H10
|
||||
const apriltag_family_t tag36h10 = {
|
||||
.ncodes = 2320,
|
||||
.black_border = 1,
|
||||
@ -7987,12 +7994,13 @@ const apriltag_family_t tag36h10 = {
|
||||
0x0000000de5bc4369UL
|
||||
}
|
||||
};
|
||||
|
||||
#endif // IMLIB_ENABLE_APRILTAGS_TAG36H10
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////// "tag36h11"
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
const apriltag_family_t tag36h11 = {
|
||||
.ncodes = 587,
|
||||
.black_border = 1,
|
||||
@ -8588,11 +8596,13 @@ const apriltag_family_t tag36h11 = {
|
||||
0x0000000e83be4b73UL
|
||||
}
|
||||
};
|
||||
#endif // IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////// "artoolkit"
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_ARTOOLKIT
|
||||
const apriltag_family_t artoolkit = {
|
||||
.ncodes = 512,
|
||||
.black_border = 1,
|
||||
@ -9113,6 +9123,7 @@ const apriltag_family_t artoolkit = {
|
||||
0x000923d963d8UL
|
||||
}
|
||||
};
|
||||
#endif // IMLIB_ENABLE_APRILTAGS_ARTOOLKIT
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////// "union_find.h"
|
||||
@ -11949,29 +11960,41 @@ void imlib_find_apriltags(list_t *out, image_t *ptr, rectangle_t *roi, apriltag_
|
||||
umm_init_x(((fb_avail() - fb_alloc_need) / resolution) * resolution);
|
||||
apriltag_detector_t *td = apriltag_detector_create();
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG16H5
|
||||
if (families & TAG16H5) {
|
||||
apriltag_detector_add_family(td, (apriltag_family_t *) &tag16h5);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG25H7
|
||||
if (families & TAG25H7) {
|
||||
apriltag_detector_add_family(td, (apriltag_family_t *) &tag25h7);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG25H9
|
||||
if (families & TAG25H9) {
|
||||
apriltag_detector_add_family(td, (apriltag_family_t *) &tag25h9);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG36H10
|
||||
if (families & TAG36H10) {
|
||||
apriltag_detector_add_family(td, (apriltag_family_t *) &tag36h10);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
if (families & TAG36H11) {
|
||||
apriltag_detector_add_family(td, (apriltag_family_t *) &tag36h11);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_ARTOOLKIT
|
||||
if (families & ARTOOLKIT) {
|
||||
apriltag_detector_add_family(td, (apriltag_family_t *) &artoolkit);
|
||||
}
|
||||
#endif
|
||||
|
||||
image_t img;
|
||||
img.w = roi->w;
|
||||
@ -12015,29 +12038,41 @@ void imlib_find_apriltags(list_t *out, image_t *ptr, rectangle_t *roi, apriltag_
|
||||
lnk_data.id = det->id;
|
||||
lnk_data.family = 0;
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG16H5
|
||||
if(det->family == &tag16h5) {
|
||||
lnk_data.family |= TAG16H5;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG25H7
|
||||
if(det->family == &tag25h7) {
|
||||
lnk_data.family |= TAG25H7;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG25H9
|
||||
if(det->family == &tag25h9) {
|
||||
lnk_data.family |= TAG25H9;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG36H10
|
||||
if(det->family == &tag36h10) {
|
||||
lnk_data.family |= TAG36H10;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
if(det->family == &tag36h11) {
|
||||
lnk_data.family |= TAG36H11;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_ARTOOLKIT
|
||||
if(det->family == &artoolkit) {
|
||||
lnk_data.family |= ARTOOLKIT;
|
||||
}
|
||||
#endif
|
||||
|
||||
lnk_data.hamming = det->hamming;
|
||||
lnk_data.centroid_x = det->c[0] + roi->x;
|
||||
|
||||
@ -7118,13 +7118,25 @@ static const mp_rom_map_elem_t globals_dict_table[] = {
|
||||
{MP_ROM_QSTR(MP_QSTR_CORNER_FAST), MP_ROM_INT(CORNER_FAST)},
|
||||
{MP_ROM_QSTR(MP_QSTR_CORNER_AGAST), MP_ROM_INT(CORNER_AGAST)},
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG16H5
|
||||
{MP_ROM_QSTR(MP_QSTR_TAG16H5), MP_ROM_INT(TAG16H5)},
|
||||
#endif
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG25H7
|
||||
{MP_ROM_QSTR(MP_QSTR_TAG25H7), MP_ROM_INT(TAG25H7)},
|
||||
#endif
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG25H9
|
||||
{MP_ROM_QSTR(MP_QSTR_TAG25H9), MP_ROM_INT(TAG25H9)},
|
||||
#endif
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG36H10
|
||||
{MP_ROM_QSTR(MP_QSTR_TAG36H10), MP_ROM_INT(TAG36H10)},
|
||||
#endif
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG36H11
|
||||
{MP_ROM_QSTR(MP_QSTR_TAG36H11), MP_ROM_INT(TAG36H11)},
|
||||
#endif
|
||||
#ifdef IMLIB_ENABLE_APRILTAGS_TAG36ARTOOLKIT
|
||||
{MP_ROM_QSTR(MP_QSTR_ARTOOLKIT), MP_ROM_INT(ARTOOLKIT)},
|
||||
#endif
|
||||
#endif
|
||||
#ifdef IMLIB_ENABLE_BARCODES
|
||||
{MP_ROM_QSTR(MP_QSTR_EAN2), MP_ROM_INT(BARCODE_EAN2)},
|
||||
{MP_ROM_QSTR(MP_QSTR_EAN5), MP_ROM_INT(BARCODE_EAN5)},
|
||||
|
||||
Loading…
Reference in New Issue
Block a user