apriltag_tracking.py

# 本作品采用MIT许可证授权。
# 版权所有 (c) 2013-2023 OpenMV LLC。保留所有权利。
# https://github.com/openmv/openmv/blob/master/LICENSE
#
# 这个示例展示了如何使用读出窗口控制来读取相机的一小部分
# 以非常高的速度读取传感器像素数组并移动该读出窗口。

# 此示例是在使用OV5640传感器的OpenMV Cam H7 Plus上设计和测试的。

import csi
import time

# 此示例脚本在整个时间内将曝光强制设置为恒定值。然而，你可能
# 希望在读取窗口缩小到较小时动态调整曝光。
EXPOSURE_MICROSECONDS = 20000

SEARCHING_RESOLUTION = csi.QVGA
TRACKING_RESOLUTION = csi.QQVGA  # 或csi.QQQVGA

TRACKING_LOW_RATIO_THRESHOLD = (
    0.2  # 当标签边与分辨率相比较小时，转到较小的读取窗口。
)
TRACKING_HIGH_RATIO_THRESHOLD = (
    0.8  # 当标签边与分辨率相比较大时，转到较大的读取窗口。
)

csi0 = csi.CSI()
csi0.reset()  # 重置并初始化传感器。
csi0.pixformat(csi.GRAYSCALE)  # 将像素格式设置为灰度
csi0.framesize(SEARCHING_RESOLUTION)
csi0.snapshot(time=1000)  # 等待设置生效。
clock = time.clock()  # 创建一个时钟对象来跟踪FPS。

csi0.auto_gain(False)  # 关闭它，因为它会振荡。
csi0.auto_exposure(False, exposure_us=EXPOSURE_MICROSECONDS)
csi0.snapshot(time=1000)

# sensor_w和sensor_h是图像传感器的原始像素宽度/高度（x/y最初为0）。
x, y, sensor_w, sensor_h = csi0.ioctl(csi.IOCTL_GET_READOUT_WINDOW)

while True:
    clock.tick()
    img = csi0.snapshot()

    # 默认跟踪TAG36H11。
    tags = img.find_apriltags()

    if len(tags):
        best_tag = max(tags, key=lambda x: x.decision_margin)
        img.draw_rectangle(best_tag.rect)

        # 这需要小于传感器的默认输出，以便我们可以移动它。
        readout_window_w = ((sensor_w // csi0.width()) * csi0.width()) / 2
        readout_window_h = ((sensor_h // csi0.height()) * csi0.height()) / 2

        def get_mapped_centroid(t):
            # 默认情况下，读出窗口设置为整个传感器像素阵列，x/y==0。
            # 你看到的分辨率是通过从读出窗口中获取像素产生的
            # 相机。x/y 位置相对于传感器中心。
            x, y, w, h = csi0.ioctl(csi.IOCTL_GET_READOUT_WINDOW)

            # 相机驱动程序将尝试缩放以适应您传递给 max 的任何分辨率
            # 适合传感器并保持宽高比的宽度/高度。
            ratio = min(w / float(csi0.width()), h / float(csi0.height()))

            # 参考 cx() 到视口的中心，然后缩放到读数。
            mapped_cx = (t.cx - (csi0.width() / 2.0)) * ratio
            # 由于我们保持宽高比，x 方向上可能会有偏移。
            mapped_cx += (w - (csi0.width() * ratio)) / 2.0
            # 添加我们从传感器中心的位移。
            mapped_cx += x + (sensor_w / 2.0)

            # 将 cy() 引用到视口的中心，然后缩放到读数。
            mapped_cy = (t.cy - (csi0.height() / 2.0)) * ratio
            # 由于我们保持宽高比，y 方向可能会有偏移。
            mapped_cy += (h - (csi0.height() * ratio)) / 2.0
            # 添加我们从传感器中心的位移。
            mapped_cy += y + (sensor_h / 2.0)

            return (mapped_cx, mapped_cy)  # 传感器数组上的 X/Y 位置。

        def center_on_tag(t, res):
            global readout_window_w
            global readout_window_h
            mapped_cx, mapped_cy = get_mapped_centroid(t)

            # 切换到 res（如果 res 未更改，则此操作无效）。
            csi0.framesize(res)

            # 构建读取窗口。x/y 是从中心开始的偏移量。
            x = int(mapped_cx - (sensor_w / 2.0))
            y = int(mapped_cy - (sensor_h / 2.0))
            w = int(readout_window_w)
            h = int(readout_window_h)

            # 聚焦于质心。
            csi0.ioctl(csi.IOCTL_SET_READOUT_WINDOW, (x, y, w, h))

            # 检查是否触碰到边缘。
            new_x, new_y, w, h = csi0.ioctl(csi.IOCTL_GET_READOUT_WINDOW)

            # 你可以使用这些误差值来驱动伺服电机移动摄像头。
            x_error = x - new_x
            y_error = y - new_y

            if x_error < 0:
                print("-X Limit Reached ", end="")
            if x_error > 0:
                print("+X Limit Reached ", end="")
            if y_error < 0:
                print("-Y Limit Reached ", end="")
            if y_error > 0:
                print("+Y Limit Reached ", end="")

        center_on_tag(best_tag, TRACKING_RESOLUTION)

        loss_count = 0

        # 这个循环将以更高的读取速度和更低的分辨率跟踪标签。
        while True:
            clock.tick()
            img = csi0.snapshot()

            # 默认跟踪TAG36H11。
            tags = img.find_apriltags()

            # 如果我们丢失了标签，那么我们需要找到一个新的。
            if not len(tags):
                # 处理由于标签闪烁引起的几帧坏帧。
                if loss_count < 2:
                    loss_count += 1
                    continue
                # 重置分辨率。
                csi0.framesize(SEARCHING_RESOLUTION)
                csi0.ioctl(csi.IOCTL_SET_READOUT_WINDOW, (sensor_w, sensor_h))
                break

            loss_count = 0

            # 缩小斑点列表并突出显示斑点。
            best_tag = max(tags, key=lambda x: x.decision_margin)
            img.draw_rectangle(best_tag.rect)

            print(clock.fps(), "TAG cx:%d, cy:%d" % get_mapped_centroid(best_tag))

            w_ratio = best_tag.w / csi0.width()
            h_ratio = best_tag.h / csi0.height()

            # 缩小跟踪窗口，直到标签适合。
            while (w_ratio < TRACKING_LOW_RATIO_THRESHOLD) or (
                h_ratio < TRACKING_LOW_RATIO_THRESHOLD
            ):
                readout_window_w /= 2
                readout_window_h /= 2
                w_ratio *= 2
                h_ratio *= 2

            # 放大跟踪窗口，直到标签适合。
            while (TRACKING_HIGH_RATIO_THRESHOLD < w_ratio) or (
                TRACKING_HIGH_RATIO_THRESHOLD < h_ratio
            ):
                readout_window_w *= 2
                readout_window_h *= 2
                w_ratio /= 2
                h_ratio /= 2

            center_on_tag(best_tag, TRACKING_RESOLUTION)

    print(clock.fps())