AprilTags示例

# 本作品采用MIT许可证授权。
# 版权所有 (c) 2013-2023 OpenMV LLC。保留所有权利。
# https://github.com/openmv/openmv/blob/master/LICENSE
#
# AprilTags示例
#
# 本示例展示OpenMV Cam检测AprilTags的强大能力
# 在OpenMV Cam M7上。M4版本无法检测April标签。

import csi
import time
import math

csi0 = csi.CSI()
csi0.reset()
csi0.pixformat(csi.RGB565)
csi0.framesize(csi.QQVGA)
csi0.snapshot(time=2000)
csi0.auto_gain(False)  # 必须关闭此功能以防止图像褪色...
csi0.auto_whitebal(False)  # 必须关闭此功能以防止图像褪色...

clock = time.clock()

# 注意！与find_qrcodes不同，find_apriltags方法不需要对图像进行镜头校正即可工作。

# 标签家族之间有什么区别？例如，TAG16H5家族实际上是
# 4x4的方形标签。这意味着它比6x6方形的TAG36H11标签
# 能在更远的距离被识别。但更低的H值（H5对比H11）意味着
# 4x4标签的误报率远远高于6x6标签。所以，除非您有
# 使用其他标签家族的理由，否则请使用默认的TAG36H11家族。

# AprilTags库会输出标签的姿态信息，即x/y/z平移和
# x/y/z旋转。x/y/z旋转以弧度为单位，可以转换为角度。至于
# 平移，其单位是无量纲的，您必须自行应用换算函数。

# f_x是相机在x方向的焦距。它应等于镜头焦距（毫米）
# 除以传感器x方向尺寸（毫米），再乘以图像的像素数。
# 以下值适用于配备2.8毫米镜头的OV7725摄像头。

# f_y是相机在y方向的焦距。它应等于镜头焦距（毫米）
# 除以传感器y方向尺寸（毫米），再乘以图像的像素数。
# 以下值适用于配备2.8毫米镜头的OV7725摄像头。

# c_x是图像x方向中心位置（像素）。
# c_y是图像y方向中心位置（像素）。

f_x = (2.8 / 3.984) * 160  # 如未设置，find_apriltags默认使用此值
f_y = (2.8 / 2.952) * 120  # 如未设置，find_apriltags默认使用此值
c_x = 160 * 0.5  # 如未设置，find_apriltags默认使用此值（image.w * 0.5）
c_y = 120 * 0.5  # 如未设置，find_apriltags默认使用此值（image.h * 0.5）


def degrees(radians):
    return (180 * radians) / math.pi


while True:
    clock.tick()
    img = csi0.snapshot()
    for tag in img.find_apriltags(
        fx=f_x, fy=f_y, cx=c_x, cy=c_y
    ):  # 默认为TAG36H11
        img.draw_detection(tag, color1=(255, 0, 0), color2=(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),
        )
        # 平移单位未知。旋转单位为度。
        print("Tx: %f, Ty %f, Tz %f, Rx %f, Ry %f, Rz %f" % print_args)
    print(clock.fps())