下記内容にて、座標の取得はできましたが、arucoマーカID_0とID_1のx,y,zの距離を出す方法をアドバイスいただけたら助かります。
よろしくお願いいたします。
#https://qiita.com/ReoNagai/items/a8fdee89b1686ec31d10
#!/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.033 # [m]
# マーカーの辞書選択
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
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()
回答1件