高精度タグ認識技術AprilTagの検証とその可能性

pip install pupil-apriltags
pip install opencv-python

import copy
import time
import argparse

import cv2 as cv
from pupil_apriltags import Detector

# コマンドライン引数を解析する関数
def get_args():
    parser = argparse.ArgumentParser()  # コマンドライン引数のパーサーを生成

    # 各種引数を設定
    parser.add_argument("--device", type=int, default=0)
    parser.add_argument("--width", help='cap width', type=int, default=1024)
    parser.add_argument("--height", help='cap height', type=int, default=576)
    parser.add_argument("--families", type=str, default='tag16h5')
    parser.add_argument("--nthreads", type=int, default=1)
    parser.add_argument("--quad_decimate", type=float, default=2.0)
    parser.add_argument("--quad_sigma", type=float, default=0.0)
    parser.add_argument("--refine_edges", type=int, default=1)
    parser.add_argument("--decode_sharpening", type=float, default=0.25)
    parser.add_argument("--debug", type=int, default=0)

    args = parser.parse_args()  # 引数を解析

    return args  # 解析結果を返す

def main():
    # 引数解析 #################################################################
    args = get_args()  # 引数を取得

    # 各種引数を変数に格納
    cap_device = args.device
    cap_width = args.width
    cap_height = args.height

    families = args.families
    nthreads = args.nthreads
    quad_decimate = args.quad_decimate
    quad_sigma = args.quad_sigma
    refine_edges = args.refine_edges
    decode_sharpening = args.decode_sharpening
    debug = args.debug

        # カメラ準備 ###############################################################
    # 指定されたデバイスのカメラをオープン
    cap = cv.VideoCapture(cap_device)
    # カメラの解像度を指定
    cap.set(cv.CAP_PROP_FRAME_WIDTH, cap_width)
    cap.set(cv.CAP_PROP_FRAME_HEIGHT, cap_height)

    # Detector準備 #############################################################
    # AprilTagの検出器を生成
    at_detector = Detector(
        families=families,
        nthreads=nthreads,
        quad_decimate=quad_decimate,
        quad_sigma=quad_sigma,
        refine_edges=refine_edges,
        decode_sharpening=decode_sharpening,
        debug=debug,
    )

    elapsed_time = 0

    # カメラからの映像を連続的に処理
    while True:
        start_time = time.time()  # 処理開始時間

        # カメラキャプチャ #####################################################
        # カメラからフレームを取得
        ret, image = cap.read()
        if not ret:
            break  # フレームが取得できなかった場合、ループを抜ける
        debug_image = copy.deepcopy(image)  # デバッグ用の画像を生成

        # 検出実施 #############################################################
        # 画像をグレースケールに変換し、AprilTagを検出
        image = cv.cvtColor(image, cv.COLOR_BGR2GRAY)

        # 画像を二値化
        _, image = cv.threshold(image, 0, 255, cv.THRESH_BINARY+cv.THRESH_OTSU)

        tags = at_detector.detect(
            image,
            estimate_tag_pose=False,
            camera_params=None,
            tag_size=None,
        )

        # 描画 ################################################################
        # 検出したAprilTagにマーキングを施した画像を生成
        debug_image = draw_tags(debug_image, tags, elapsed_time)

        elapsed_time = time.time() - start_time  # 処理時間を計算

        # キー処理(ESC：終了) #################################################
        # キーボードの入力をチェック
        key = cv.waitKey(1)
        if key == 27:  # ESCが押された場合、ループを抜ける
            break

        # 画面反映 #############################################################
        # 画像をウィンドウに表示
        cv.imshow('AprilTag Detect Demo', debug_image)

    cap.release()  # カメラをリリース
    cv.destroyAllWindows()  # ウィンドウを全て閉じる


# AprilTagにマーキングを施す関数
def draw_tags(
    image,  # 元画像
    tags,  # 検出したAprilTagのリスト
    elapsed_time,  # 処理時間
):
    # 各AprilTagに対して処理を行う
    for tag in tags:
        tag_family = tag.tag_family  # AprilTagのファミリー
        tag_id = tag.tag_id  # AprilTagのID
        center = tag.center  # AprilTagの中心座標
        corners = tag.corners  # AprilTagの四隅の座標

        # 各座標を整数に変換
        center = (int(center[0]), int(center[1]))
        corner_01 = (int(corners[0][0]), int(corners[0][1]))
        corner_02 = (int(corners[1][0]), int(corners[1][1]))
        corner_03 = (int(corners[2][0]), int(corners[2][1]))
        corner_04 = (int(corners[3][0]), int(corners[3][1]))

        # 中心を描画
        cv.circle(image, (center[0], center[1]), 5, (0, 0, 255), 2)

        # 各辺を描画
        cv.line(image, (corner_01[0], corner_01[1]),
                (corner_02[0], corner_02[1]), (255, 0, 0), 2)
        cv.line(image, (corner_02[0], corner_02[1]),
                (corner_03[0], corner_03[1]), (255, 0, 0), 2)
        cv.line(image, (corner_03[0], corner_03[1]),
                (corner_04[0], corner_04[1]), (0, 255, 0), 2)
        cv.line(image, (corner_04[0], corner_04[1]),
                (corner_01[0], corner_01[1]), (0, 255, 0), 2)

        # タグIDを描画
        cv.putText(image, str(tag_id), (center[0] - 10, center[1] - 10),
                   cv.FONT_HERSHEY_SIMPLEX, 0.75, (0, 0, 255), 2, cv.LINE_AA)

    # 処理時間を描画
    cv.putText(image,
               "Elapsed Time:" + '{:.1f}'.format(elapsed_time * 1000) + "ms",
               (10, 30), cv.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2,
               cv.LINE_AA)

    return image  # 描画した画像を返す


# スクリプトとして実行されたときの処理
if __name__ == '__main__':
    main()  # メインの処理を実行