teratail_273729への回答

sample_aruco.py

import numpy as np
import cv2
from cv2 import aruco
import sys, time
def main():
    # print(cv2.getBuildInformation())
    cap = cv2.VideoCapture(0)
    # マーカーサイズ

    marker_length = 0.056 # [m]
    # マーカーの辞書選択
    dictionary = aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL)

    camera_matrix = np.array([[639.87721705,   0.        , 330.12073612],
                              [  0.        , 643.69687408, 208.61588364],
                              [  0.        ,   0.        ,   1.        ]])
    distortion_coeff = np.array([ 5.66942769e-02, -6.05774927e-01, -7.42066667e-03, -3.09571466e-04, 1.92386974e+00])
    # distortion_coeff = np.load("dist.npy")

    while True:
        markers={} #  １フレーム内に存在するマーカーを格納する辞書：key: マーカーID、value: 座標・方向データ
        ret, img = cap.read()
        corners, ids, rejectedImgPoints = aruco.detectMarkers(img, dictionary)
        # 可視化
        aruco.drawDetectedMarkers(img, corners, ids, (0,255,255))

        if len(corners) > 0:
            # マーカーごとに処理
            for i, corner in enumerate(corners):
            
                # rvec -> rotation vector, tvec -> translation vector
                rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corner, marker_length, camera_matrix, distortion_coeff)

                # < rodoriguesからeuluerへの変換 >

                # 不要なaxisを除去
                tvec = np.squeeze(tvec)
                rvec = np.squeeze(rvec)
                # 回転ベクトルからrodoriguesへ変換
                rvec_matrix = cv2.Rodrigues(rvec)
                rvec_matrix = rvec_matrix[0] # rodoriguesから抜き出し
                # 並進ベクトルの転置
                transpose_tvec = tvec[np.newaxis, :].T
                # 合成
                proj_matrix = np.hstack((rvec_matrix, transpose_tvec))
                # # オイラー角への変換
                euler_angle = cv2.decomposeProjectionMatrix(proj_matrix)[6] # [deg]
                # マーカーIDをキーにして、マーカーID毎の座標・方向を辞書に格納：
                markers[ids[i][0]] = {
                    "x": str(tvec[0]),
                    "y": str(tvec[1]),
                    "z": str(tvec[2]),
                    "roll": str(euler_angle[0]),
                    "pitch": str(euler_angle[1]),
                    "yaw": str(euler_angle[2])
                }
                # 可視化
                draw_pole_length = marker_length/2 # 現実での長さ[m]
                aruco.drawAxis(img, camera_matrix, distortion_coeff, rvec, tvec, draw_pole_length)

        # 1フレーム内にある複数マーカーの座標・方向を出力する。
        if len(markers.keys()) > 0:
            print('='*20)
            for mid in markers.keys():  #  マーカーIDを列挙
                print("ID%s のマーカー=(x: %s, y: %s, z: %s, roll: %s, pitch: %s, yaw: %s)" % (
                    mid, markers[mid]["x"], markers[mid]["y"], markers[mid]["z"], markers[mid]["roll"], markers[mid]["pitch"], markers[mid]["yaw"]))
                sys.stdout.flush()
                time.sleep(0.01)
            print('='*20)

        cv2.imshow('drawDetectedMarkers', img)
        if cv2.waitKey(10) & 0xFF == ord('q'):
            break

    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()