apriltag_tracking.py

# この作品はMITライセンスの下で提供されています。
# Copyright (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)  # ピクセルフォーマットをGRAYSCALEに設定します
csi0.framesize(SEARCHING_RESOLUTION)
csi0.snapshot(time=1000)  # 設定が反映されるまで待ちます。
clock = time.clock()  # FPSを計測するためのclockオブジェクトを作成します。

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)

            # カメラドライバは、渡された解像度に合わせて、アスペクト比を保ちながら
            # センサーに収まる最大の幅/高さにスケーリングしようとします。
            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)

            # 解像度を切り替えます（解像度が変わらない場合は何もしません）。
            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())