100_fps_ir_led_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 = 1000
TRACKING_THRESHOLDS = [(128, 255)]  # 露光を下げると、全体が暗くなります。

SEARCHING_RESOLUTION = csi.VGA
SEARCHING_AREA_THRESHOLD = 16
SEARCHING_PIXEL_THRESHOLD = SEARCHING_AREA_THRESHOLD

TRACKING_RESOLUTION = csi.QQVGA
TRACKING_AREA_THRESHOLD = 256
TRACKING_PIXEL_THRESHOLD = TRACKING_AREA_THRESHOLD

TRACKING_EDGE_TOLERANCE = 0.05  # ブロブは中心から5%まで移動できます。

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()

    # 追跡対象のIR物体を見つける必要があります。おそらく非常に明るいはずです。
    blobs = img.find_blobs(
        TRACKING_THRESHOLDS,
        area_threshold=SEARCHING_AREA_THRESHOLD,
        pixels_threshold=SEARCHING_PIXEL_THRESHOLD,
    )

    if len(blobs):
        most_dense_blob = max(blobs, key=lambda x: x.density)
        img.draw_rectangle(most_dense_blob.rect)

        def get_mapped_centroid(b):
            # デフォルトでは、リードアウトウィンドウはセンサー全体の画素配列に設定され、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 = (b.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 = (b.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_blob(b, res):
            mapped_cx, mapped_cy = get_mapped_centroid(b)

            # 解像度を切り替えます（解像度が変わらない場合は何もしません）。
            csi0.framesize(res)

            # リードアウトウィンドウを構築します。x/yは中心からのオフセットです。
            x = int(mapped_cx - (sensor_w / 2.0))
            y = int(mapped_cy - (sensor_h / 2.0))
            w = csi0.width()
            h = csi0.height()

            # 重心にフォーカスを合わせます。
            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_blob(most_dense_blob, TRACKING_RESOLUTION)

        # このループでは、より高いリードアウト速度・低い解像度でブロブを追跡します。
        while True:
            clock.tick()
            img = csi0.snapshot()

            # 低解像度画像内でブロブを検出します。
            blobs = img.find_blobs(
                TRACKING_THRESHOLDS,
                area_threshold=TRACKING_AREA_THRESHOLD,
                pixels_threshold=TRACKING_PIXEL_THRESHOLD,
            )

            # ブロブを見失った場合は、新しいブロブを探す必要があります。
            if not len(blobs):
                # 解像度をリセットします。
                csi0.framesize(SEARCHING_RESOLUTION)
                csi0.ioctl(csi.IOCTL_SET_READOUT_WINDOW, (sensor_w, sensor_h))
                break

            # ブロブリストを絞り込み、対象のブロブをハイライトします。
            most_dense_blob = max(blobs, key=lambda x: x.density)
            img.draw_rectangle(most_dense_blob.rect)

            print(
                clock.fps(), "BLOB cx:%d, cy:%d" % get_mapped_centroid(most_dense_blob)
            )

            x_diff = most_dense_blob.cx - (csi0.width() / 2.0)
            y_diff = most_dense_blob.cy - (csi0.height() / 2.0)

            w_threshold = (csi0.width() / 2.0) * TRACKING_EDGE_TOLERANCE
            h_threshold = (csi0.height() / 2.0) * TRACKING_EDGE_TOLERANCE

            # ブロブが視野から外れそうになった場合は再センタリングします（FPSが低下します）。
            if abs(x_diff) > w_threshold or abs(y_diff) > h_threshold:
                center_on_blob(most_dense_blob, TRACKING_RESOLUTION)

    print(clock.fps())