diff --git a/src/Makefile b/src/Makefile index 1c06cd906..44f65a638 100755 --- a/src/Makefile +++ b/src/Makefile @@ -137,6 +137,7 @@ FIRM_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/, \ FIRM_OBJ += $(addprefix $(BUILD)/$(OMV_DIR)/img/,\ blob.o \ qrcode.o \ + apriltag.o \ fmath.o \ fsort.o \ fft.o \ diff --git a/src/omv/Makefile b/src/omv/Makefile index 054356c46..6fcbbb32c 100644 --- a/src/omv/Makefile +++ b/src/omv/Makefile @@ -19,6 +19,7 @@ SRCS += $(addprefix , \ SRCS += $(addprefix img/, \ blob.c \ qrcode.c \ + apriltag.c \ fmath.c \ fsort.c \ fft.c \ diff --git a/src/omv/boards/OPENMV3/omv_boardconfig.h b/src/omv/boards/OPENMV3/omv_boardconfig.h index 2bb149a08..1e82ed63a 100644 --- a/src/omv/boards/OPENMV3/omv_boardconfig.h +++ b/src/omv/boards/OPENMV3/omv_boardconfig.h @@ -36,6 +36,9 @@ #define OMV_MAX_BLOB_FRAME FRAMESIZE_VGA #define OMV_MAX_BLOB_FRAME_STR "VGA" +// Enable AprilTags (64 KB). +#define OMV_ENABLE_APRILTAGS + // Bootloader LED GPIO port/pin #define OMV_BOOTLDR_LED_PIN (GPIO_PIN_1) #define OMV_BOOTLDR_LED_PORT (GPIOC) diff --git a/src/omv/fb_alloc.c b/src/omv/fb_alloc.c index 03061cc52..533b1b749 100644 --- a/src/omv/fb_alloc.c +++ b/src/omv/fb_alloc.c @@ -13,7 +13,7 @@ extern char _fballoc; static char *pointer = &_fballoc; -NORETURN static void fb_alloc_fail() +NORETURN void fb_alloc_fail() { nlr_raise(mp_obj_new_exception_msg(&mp_type_MemoryError, "FB Alloc Collision!!!")); } diff --git a/src/omv/fb_alloc.h b/src/omv/fb_alloc.h index 639aa7b8d..3dd7669cd 100644 --- a/src/omv/fb_alloc.h +++ b/src/omv/fb_alloc.h @@ -9,6 +9,7 @@ #ifndef __FB_ALLOC_H__ #define __FB_ALLOC_H__ #include +void fb_alloc_fail(); void fb_alloc_init0(); uint32_t fb_avail(); void fb_alloc_mark(); diff --git a/src/omv/img/apriltag.c b/src/omv/img/apriltag.c index 2ee5ee340..10634c53d 100644 --- a/src/omv/img/apriltag.c +++ b/src/omv/img/apriltag.c @@ -10,10 +10,64 @@ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" +/* Copyright (C) 2013-2016, The Regents of The University of Michigan. +All rights reserved. + +This software was developed in the APRIL Robotics Lab under the +direction of Edwin Olson, ebolson@umich.edu. This software may be +available under alternative licensing terms; contact the address above. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the Regents of The University of Michigan. +*/ + //////////////////////////////////////////////////////////////////////////////////////////////////// //////// "umm_malloc.h" //////////////////////////////////////////////////////////////////////////////////////////////////// +//The MIT License (MIT) + +//Copyright (c) 2015 Ralph Hempel + +//Permission is hereby granted, free of charge, to any person obtaining a copy +//of this software and associated documentation files (the "Software"), to deal +//in the Software without restriction, including without limitation the rights +//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +//copies of the Software, and to permit persons to whom the Software is +//furnished to do so, subject to the following conditions: + +//The above copyright notice and this permission notice shall be included in all +//copies or substantial portions of the Software. + +//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +//SOFTWARE. + /* ---------------------------------------------------------------------------- * umm_malloc.h - a memory allocator for embedded systems (microcontrollers) * @@ -37,6 +91,28 @@ void umm_free( void *ptr ); //////// "umm_malloc.c" //////////////////////////////////////////////////////////////////////////////////////////////////// +//The MIT License (MIT) + +//Copyright (c) 2015 Ralph Hempel + +//Permission is hereby granted, free of charge, to any person obtaining a copy +//of this software and associated documentation files (the "Software"), to deal +//in the Software without restriction, including without limitation the rights +//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +//copies of the Software, and to permit persons to whom the Software is +//furnished to do so, subject to the following conditions: + +//The above copyright notice and this permission notice shall be included in all +//copies or substantial portions of the Software. + +//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +//SOFTWARE. + /* ---------------------------------------------------------------------------- * umm_malloc.c - a memory allocator for embedded systems (microcontrollers) * @@ -841,38 +917,6 @@ loop: SWAPINIT(a, es); #undef swap #undef vecswap -/* Copyright (C) 2013-2016, The Regents of The University of Michigan. -All rights reserved. - -This software was developed in the APRIL Robotics Lab under the -direction of Edwin Olson, ebolson@umich.edu. This software may be -available under alternative licensing terms; contact the address above. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. -2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -The views and conclusions contained in the software and documentation are those -of the authors and should not be interpreted as representing official policies, -either expressed or implied, of the Regents of The University of Michigan. -*/ - #define printf(format, ...) #define fprintf(format, ...) #define free(ptr) ({ umm_free(ptr); }) diff --git a/src/omv/img/imlib.h b/src/omv/img/imlib.h index c18286043..0d2db06c5 100644 --- a/src/omv/img/imlib.h +++ b/src/omv/img/imlib.h @@ -891,6 +891,26 @@ typedef struct find_qrcodes_list_lnk_data } find_qrcodes_list_lnk_data_t; +typedef enum apriltag_families +{ + TAG16H5 = 1, + TAG25H7 = 2, + TAG25H9 = 4, + TAG36H10 = 8, + TAG36H11 = 16, + ARTOOLKIT = 32 +} +apriltag_families_t; + +typedef struct find_apriltags_list_lnk_data +{ + rectangle_t rect; + uint16_t family, id; + point_t centroid; + float rotation, decision_margin; +} +find_apriltags_list_lnk_data_t; + /* Color space functions */ void imlib_rgb_to_lab(simple_color_t *rgb, simple_color_t *lab); void imlib_lab_to_rgb(simple_color_t *lab, simple_color_t *rgb); @@ -1068,5 +1088,6 @@ 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); #endif //__IMLIB_H__ diff --git a/src/omv/py/py_image.c b/src/omv/py/py_image.c index f597e2b5f..5fa83d7f5 100644 --- a/src/omv/py/py_image.c +++ b/src/omv/py/py_image.c @@ -18,6 +18,7 @@ #include "py_assert.h" #include "py_helper.h" #include "py_image.h" +#include "omv_boardconfig.h" static const mp_obj_type_t py_cascade_type; static const mp_obj_type_t py_image_type; @@ -1927,6 +1928,152 @@ static mp_obj_t py_image_find_qrcodes(uint n_args, const mp_obj_t *args, mp_map_ return objects_list; } +// AprilTag Object // +#define py_apriltag_obj_size 10 +typedef struct py_apriltag_obj { + mp_obj_base_t base; + mp_obj_t x, y, w, h, id, family, cx, cy, rotation, decision_margin; +} 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}", + mp_obj_get_int(self->x), + mp_obj_get_int(self->y), + mp_obj_get_int(self->w), + mp_obj_get_int(self->h), + mp_obj_get_int(self->id), + mp_obj_get_int(self->family), + 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)); +} + +static mp_obj_t py_apriltag_subscr(mp_obj_t self_in, mp_obj_t index, mp_obj_t value) +{ + if (value == MP_OBJ_SENTINEL) { // load + py_apriltag_obj_t *self = self_in; + if (MP_OBJ_IS_TYPE(index, &mp_type_slice)) { + mp_bound_slice_t slice; + if (!mp_seq_get_fast_slice_indexes(py_apriltag_obj_size, index, &slice)) { + mp_not_implemented("only slices with step=1 (aka None) are supported"); + } + mp_obj_tuple_t *result = mp_obj_new_tuple(slice.stop - slice.start, NULL); + mp_seq_copy(result->items, &(self->x) + slice.start, result->len, mp_obj_t); + return result; + } + switch (mp_get_index(self->base.type, py_apriltag_obj_size, index, false)) { + case 0: return self->x; + case 1: return self->y; + case 2: return self->w; + case 3: return self->h; + case 4: return self->id; + case 5: return self->family; + case 6: return self->cx; + case 7: return self->cy; + case 8: return self->rotation; + case 9: return self->decision_margin; + } + } + return MP_OBJ_NULL; // op not supported +} + +mp_obj_t py_apriltag_rect(mp_obj_t self_in) +{ + return mp_obj_new_tuple(4, (mp_obj_t []) {((py_apriltag_obj_t *) self_in)->x, + ((py_apriltag_obj_t *) self_in)->y, + ((py_apriltag_obj_t *) self_in)->w, + ((py_apriltag_obj_t *) self_in)->h}); +} + +mp_obj_t py_apriltag_x(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->x; } +mp_obj_t py_apriltag_y(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->y; } +mp_obj_t py_apriltag_w(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->w; } +mp_obj_t py_apriltag_h(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->h; } +mp_obj_t py_apriltag_id(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->id; } +mp_obj_t py_apriltag_family(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->family; } +mp_obj_t py_apriltag_cx(mp_obj_t self_in) { return ((py_apriltag_obj_t *) self_in)->cx; } +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; } + +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); +STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_y_obj, py_apriltag_y); +STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_w_obj, py_apriltag_w); +STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_h_obj, py_apriltag_h); +STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_id_obj, py_apriltag_id); +STATIC MP_DEFINE_CONST_FUN_OBJ_1(py_apriltag_family_obj, py_apriltag_family); +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 const mp_rom_map_elem_t py_apriltag_locals_dict_table[] = { + { MP_ROM_QSTR(MP_QSTR_rect), MP_ROM_PTR(&py_apriltag_rect_obj) }, + { MP_ROM_QSTR(MP_QSTR_x), MP_ROM_PTR(&py_apriltag_x_obj) }, + { MP_ROM_QSTR(MP_QSTR_y), MP_ROM_PTR(&py_apriltag_y_obj) }, + { MP_ROM_QSTR(MP_QSTR_w), MP_ROM_PTR(&py_apriltag_w_obj) }, + { MP_ROM_QSTR(MP_QSTR_h), MP_ROM_PTR(&py_apriltag_h_obj) }, + { MP_ROM_QSTR(MP_QSTR_id), MP_ROM_PTR(&py_apriltag_id_obj) }, + { MP_ROM_QSTR(MP_QSTR_family), MP_ROM_PTR(&py_apriltag_family_obj) }, + { MP_ROM_QSTR(MP_QSTR_cx), MP_ROM_PTR(&py_apriltag_cx_obj) }, + { 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) }, +}; + +STATIC MP_DEFINE_CONST_DICT(py_apriltag_locals_dict, py_apriltag_locals_dict_table); + +static const mp_obj_type_t py_apriltag_type = { + { &mp_type_type }, + .name = MP_QSTR_apriltag, + .print = py_apriltag_print, + .subscr = py_apriltag_subscr, + .locals_dict = (mp_obj_t) &py_apriltag_locals_dict, +}; + +static mp_obj_t py_image_find_apriltags(uint n_args, const mp_obj_t *args, mp_map_t *kw_args) +{ + image_t *arg_img = py_image_cobj(args[0]); + PY_ASSERT_FALSE_MSG(IM_IS_JPEG(arg_img), + "Operation not supported on JPEG"); + + rectangle_t roi; + py_helper_lookup_rectangle(kw_args, arg_img, &roi); + + 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)); + fb_alloc_free_till_mark(); + + mp_obj_list_t *objects_list = mp_obj_new_list(list_size(&out), NULL); + for (size_t i = 0; list_size(&out); i++) { + find_apriltags_list_lnk_data_t lnk_data; + list_pop_front(&out, &lnk_data); + + py_apriltag_obj_t *o = m_new_obj(py_apriltag_obj_t); + o->base.type = &py_apriltag_type; + o->x = mp_obj_new_int(lnk_data.rect.x); + o->y = mp_obj_new_int(lnk_data.rect.y); + o->w = mp_obj_new_int(lnk_data.rect.w); + o->h = mp_obj_new_int(lnk_data.rect.h); + o->id = mp_obj_new_int(lnk_data.id); + 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->decision_margin = mp_obj_new_float(lnk_data.decision_margin); + + objects_list->items[i] = o; + } + + return objects_list; +} + static mp_obj_t py_image_midpoint_pool(uint n_args, const mp_obj_t *args, mp_map_t *kw_args) { image_t *arg_img = py_image_cobj(args[0]); @@ -2352,6 +2499,7 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_get_statistics_obj, 1, py_image_get_s STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_find_blobs_obj, 2, py_image_find_blobs); /* Code Detection */ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_find_qrcodes_obj, 1, py_image_find_qrcodes); +STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_find_apriltags_obj, 1, py_image_find_apriltags); /* Template Matching */ STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_midpoint_pool_obj, 3, py_image_midpoint_pool); STATIC MP_DEFINE_CONST_FUN_OBJ_KW(py_image_midpoint_pooled_obj, 3, py_image_midpoint_pooled); @@ -2427,6 +2575,9 @@ static const mp_map_elem_t locals_dict_table[] = { {MP_OBJ_NEW_QSTR(MP_QSTR_find_blobs), (mp_obj_t)&py_image_find_blobs_obj}, /* Code Detection */ {MP_OBJ_NEW_QSTR(MP_QSTR_find_qrcodes), (mp_obj_t)&py_image_find_qrcodes_obj}, +#ifdef OMV_ENABLE_APRILTAGS + {MP_OBJ_NEW_QSTR(MP_QSTR_find_apriltags), (mp_obj_t)&py_image_find_apriltags_obj}, +#endif /* Template Matching */ {MP_OBJ_NEW_QSTR(MP_QSTR_midpoint_pool), (mp_obj_t)&py_image_midpoint_pool_obj}, {MP_OBJ_NEW_QSTR(MP_QSTR_midpoint_pooled), (mp_obj_t)&py_image_midpoint_pooled_obj}, @@ -2800,6 +2951,14 @@ static const mp_map_elem_t globals_dict_table[] = { {MP_OBJ_NEW_QSTR(MP_QSTR_EDGE_SIMPLE), MP_OBJ_NEW_SMALL_INT(EDGE_SIMPLE)}, {MP_OBJ_NEW_QSTR(MP_QSTR_CORNER_FAST), MP_OBJ_NEW_SMALL_INT(CORNER_FAST)}, {MP_OBJ_NEW_QSTR(MP_QSTR_CORNER_AGAST), MP_OBJ_NEW_SMALL_INT(CORNER_AGAST)}, +#ifdef OMV_ENABLE_APRILTAGS + {MP_OBJ_NEW_QSTR(MP_QSTR_TAG16H5), MP_OBJ_NEW_SMALL_INT(TAG16H5)}, + {MP_OBJ_NEW_QSTR(MP_QSTR_TAG25H7), MP_OBJ_NEW_SMALL_INT(TAG25H7)}, + {MP_OBJ_NEW_QSTR(MP_QSTR_TAG25H9), MP_OBJ_NEW_SMALL_INT(TAG25H9)}, + {MP_OBJ_NEW_QSTR(MP_QSTR_TAG36H10), MP_OBJ_NEW_SMALL_INT(TAG36H10)}, + {MP_OBJ_NEW_QSTR(MP_QSTR_TAG36H11), MP_OBJ_NEW_SMALL_INT(TAG36H11)}, + {MP_OBJ_NEW_QSTR(MP_QSTR_ARTOOLKIT), MP_OBJ_NEW_SMALL_INT(ARTOOLKIT)}, +#endif /* Color space functions */ {MP_OBJ_NEW_QSTR(MP_QSTR_rgb_to_lab), (mp_obj_t)&py_image_rgb_to_lab_obj}, diff --git a/src/omv/py/qstrdefsomv.h b/src/omv/py/qstrdefsomv.h index 57a27d55c..0be0d0429 100644 --- a/src/omv/py/qstrdefsomv.h +++ b/src/omv/py/qstrdefsomv.h @@ -416,3 +416,26 @@ Q(is_numeric) Q(is_alphanumeric) Q(is_binary) Q(is_kanji) + +// Find AprilTags +Q(find_apriltags) +Q(families) +// duplicate Q(roi) +Q(apriltag) +// duplicate Q(rect) +// duplicate Q(x) +// duplicate Q(y) +// duplicate Q(w) +// duplicate Q(h) +Q(id) +Q(family) +// duplicate Q(cx) +// duplicate Q(cy) +// duplicate Q(rotation) +Q(decision_margin) +Q(TAG16H5) +Q(TAG25H7) +Q(TAG25H9) +Q(TAG36H10) +Q(TAG36H11) +Q(ARTOOLKIT) diff --git a/usr/examples/16-Codes/find_apriltags.py b/usr/examples/16-Codes/find_apriltags.py new file mode 100644 index 000000000..ca8930236 --- /dev/null +++ b/usr/examples/16-Codes/find_apriltags.py @@ -0,0 +1,55 @@ +# 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 + +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. + +# 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 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. + +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" + +while(True): + clock.tick() + img = sensor.snapshot() + 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(clock.fps()) diff --git a/usr/examples/16-Codes/find_apriltags_w_lens_zoom.py b/usr/examples/16-Codes/find_apriltags_w_lens_zoom.py new file mode 100644 index 000000000..a96f3737c --- /dev/null +++ b/usr/examples/16-Codes/find_apriltags_w_lens_zoom.py @@ -0,0 +1,33 @@ +# 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 + +sensor.reset() +sensor.set_pixformat(sensor.RGB565) +sensor.set_framesize(sensor.VGA) # we run out of memory if the resolution is much bigger... +# Look at center 160x120 pixels of the VGA resolution. +sensor.set_windowing((160, 120)) +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. + +while(True): + clock.tick() + img = sensor.snapshot() + for tag in img.find_apriltags(): # defaults to TAG36H11 + img.draw_rectangle(tag.rect(), color = (255, 0, 0)) + img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0)) + print("Tag Family TAG36H11, Tag ID %d" % tag.id()) + print(clock.fps())