마커의 검출로부터 실제의 거리로의 변환까지, 한가지의 실장이 심플하게 쓸 수 있었기 때문에 기재합니다.

이 프로그램은 카메라에서 마커까지의 거리와 각도를 얻을 수 있습니다.

덧붙여 자세 추정에서는 카메라 파라미터가 필요하게 되기(일단 시험하고 싶은 경우는 한층 더 아래에 방법을 기재) 때문에, 그것은 전회의 블로그 기사 참조해 주시면 고맙습니다.
htps : // 이 m / renagai / ms / 5, 95, 149c66d bd

구현

마커 크기는 각자 적절한 것으로 바꾸십시오.
마지막 블로그 기사에서 만든 카메라 매개 변수 인 카메라 행렬 mtx.npy와 왜곡 계수 dist.npy가 프로그램과 동일한 디렉토리에 있다고 가정합니다.

#!/usr/bin/env python
# -*- coding: utf-8 -*
import numpy as np
import cv2
from cv2 import aruco

def main():
    cap = cv2.VideoCapture(0)
    # マーカーサイズ
    marker_length = 0.056 # [m]
    # マーカーの辞書選択
    dictionary = aruco.getPredefinedDictionary(aruco.DICT_ARUCO_ORIGINAL)

    camera_matrix = np.load("mtx.npy")
    distortion_coeff = np.load("dist.npy")

    while True:
        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]

                print("x : " + str(tvec[0]))
                print("y : " + str(tvec[1]))
                print("z : " + str(tvec[2]))
                print("roll : " + str(euler_angle[0]))
                print("pitch: " + str(euler_angle[1]))
                print("yaw  : " + str(euler_angle[2]))

                # 可視化
                draw_pole_length = marker_length/2 # 現実での長さ[m]
                aruco.drawAxis(img, camera_matrix, distortion_coeff, rvec, tvec, draw_pole_length)

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

    cv2.destroyAllWindows()

if __name__ == '__main__':
    main()

또한, 카메라 파라미터를 취득하지 않은 경우

    camera_matrix = np.load("mtx.npy")
    distortion_coeff = np.load("dist.npy")

부분을

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

라고 변경하는 것으로, 우선은 움직입니다(나의 카메라 파라미터이므로 정밀도는 낮아진다고 생각합니다).

실행 결과

사용한 마커


프린터가 집에 없었기 때문에, 스마트 폰에 비친 마커의 자세 추정을하고 있습니다.
스마트 폰이라고 화면에서 조명이 반사되어 버려, 인식 정밀도가 떨어지므로 실제로 인쇄한 것을 사용하면 좋다고 생각합니다.

Z axis flipping 현상

마커의 z 축 각도 θ가 때때로 -θ가되는 현상이 일어날 수 있습니다.
조사하면, 이것은 "z axis flipping 현상"이라고 하는 것 같고, 단일의 마커를 사용하는 경우에는 올바른 대처법이 없는 것 같습니다.

따라서 일반적으로 여러 마커가 탑재 된 하나의 보드로 취급함으로써이 현상이 일어나지 않도록 할 수 있습니다.

Z axis flipping에 관해서는 이 사이트가 참고가 됩니다.
ht tp // 퐁g스케. 하테나아 ry. jp/엔트리/2018/04/19/102146

좌표 변환에 도움이 되는 사이트
htps : // 씹는 것. 하테나 bぉg. 코 m / 엔트리 / 로타리 온_에 xp 레시 온 s # 세 c1