image-patches-absolute-rotation-scale.py

# この作品はMITライセンスの下で提供されています。
# Copyright (c) 2013-2023 OpenMV LLC. 全著作権所有。
# https://github.com/openmv/openmv/blob/master/LICENSE
#
# 画像パッチによる絶対オプティカルフロー 回転/スケール
#
# この例では、OpenMV Camを使って
# 現在の画像と直前の画像を比較することで回転/スケールを計測します
# 回転/スケールのみが扱われ、
# このモードではX・Y方向の平行移動は扱われません。
#
# ただし、この例では画像全体に対して一度にオプティカルフローを計算するのではなく、
# 画像内のピクセルのグループごとに処理を分割して行います。
# これにより結果の「新しい」画像が得られます。
#
# 注意: アルゴリズムが機能するには、対象の表面に何らかの「エッジ」が
# 必要です。特徴のない表面では結果がめちゃくちゃになります。
#
# このデモを効果的に実行するには、OpenMV Camを安定した
# 台に固定し、レンズを中心にゆっくりとカメラを回転させたり、前後に
# 動かしたりして数値の変化を確認してください。
# つまりZ方向のみの変化です。
#
# 注意: find_displacement()を使用する場合は、小さい2のべき乗の
# 解像度を使用する必要があります。これは、このアルゴリズムが
# 位相相関と呼ばれる手法によって動作しており、FFTを使って
# 画像比較を行うためです。2のべき乗でない解像度は、2のべき乗に
# パディングする必要があり、アルゴリズムの結果の有用性が低下します。
# B128X128やB128X64（2倍高速）のような解像度を使用してください。
#
# OpenMV Camは64x32、64x64、128x64、128x128の
# 2のべき乗解像度に対応しています。32x32の解像度が必要な場合は、
# 64x64画像に対して"img.scale(x_scale=0.5, y_scale=0.5, hint=image.AREA)"を実行することで作成できます。

import csi
import image
import time
import math

BLOCK_W = 16  # 2のべき乗
BLOCK_H = 16  # 2のべき乗

csi0 = csi.CSI()
csi0.reset()  # センサーをリセットして初期化します。
csi0.pixformat(csi.GRAYSCALE)  # ピクセルフォーマットをGRAYSCALEに設定します
csi0.framesize((128, 128))  # フレームサイズを128x128（または64x64）に設定します……
csi0.snapshot(time=2000)  # 設定が反映されるまで待ちます。
clock = time.clock()  # FPSを計測するためのclockオブジェクトを作成します。

# ヒープ上に2つ目のフレームバッファを作成します。
extra_fb = image.Image(csi0.width(), csi0.height(), csi0.pixformat())
extra_fb.draw_image(csi0.snapshot())

while True:
    clock.tick()  # snapshot()呼び出し間の経過ミリ秒を計測します。
    img = csi0.snapshot()  # 画像を撮影して返します。

    for y in range(0, csi0.height(), BLOCK_H):
        for x in range(0, csi0.width(), BLOCK_W):
            # この例では、絶対的な変化を計測するために古い画像を更新しません。
            displacement = extra_fb.find_displacement(
                img,
                logpolar=True,
                roi=(x, y, BLOCK_W, BLOCK_H),
                template_roi=(x, y, BLOCK_W, BLOCK_H),
            )

            # 0.1程度を下回ると（環境によります）、結果は単なるノイズになります。
            if displacement.response > 0.1:
                rotation_change = displacement.rotation
                zoom_amount = displacement.scale
                pixel_x = (
                    x
                    + (BLOCK_W // 2)
                    + int(math.sin(rotation_change) * zoom_amount * (BLOCK_W // 4))
                )
                pixel_y = (
                    y
                    + (BLOCK_H // 2)
                    + int(math.cos(rotation_change) * zoom_amount * (BLOCK_H // 4))
                )
                img.draw_line(
                    (x + BLOCK_W // 2, y + BLOCK_H // 2, pixel_x, pixel_y), color=255
                )
            else:
                img.draw_line(
                    (
                        x + BLOCK_W // 2,
                        y + BLOCK_H // 2,
                        x + BLOCK_W // 2,
                        y + BLOCK_H // 2,
                    ),
                    color=0,
                )

    print(clock.fps())