前回やったマーカー検出を発展させて今回はマーカーの姿勢を求めます。

  1. import numpy as np
  2. import cv2
  3. from cv2 import aruco

  5. def main():
  6.     cap = cv2.VideoCapture(0)
  7.     marker_length = 0.060
  8.     dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

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

  13.     while True:
  14.         ret, img = cap.read()
  15.         corners, ids, rejectedImgPoints = aruco.detectMarkers(img, dictionary)
  16.         aruco.drawDetectedMarkers(img, corners, ids, (0,255,255))

  18.         if len(corners) > 0:
  19.             for i, corner in enumerate(corners):
  20.                 rvec, tvec, _ = aruco.estimatePoseSingleMarkers(corner, marker_length, camera_matrix, distortion_coeff)
  21.                 
  22.                 tvec = np.squeeze(tvec)
  23.                 rvec = np.squeeze(rvec)
  24.                 rvec_matrix = cv2.Rodrigues(rvec)
  25.                 rvec_matrix = rvec_matrix[0]
  26.                 transpose_tvec = tvec[np.newaxis, :].T
  27.                 proj_matrix = np.hstack((rvec_matrix, transpose_tvec))
  28.                 euler_angle = cv2.decomposeProjectionMatrix(proj_matrix)[6]

  30.                 print("x : " + str(tvec[0]))
  31.                 print("y : " + str(tvec[1]))
  32.                 print("z : " + str(tvec[2]))
  33.                 print("roll : " + str(euler_angle[0]))
  34.                 print("pitch: " + str(euler_angle[1]))
  35.                 print("yaw  : " + str(euler_angle[2]))

  37.                 draw_pole_length = marker_length/2
  38.                 aruco.drawAxis(img, camera_matrix, distortion_coeff, rvec, tvec, draw_pole_length)

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

  44.     cv2.destroyAllWindows()

  46. if __name__ == '__main__':
  47.     main()

7行目でマーカーの一片の長さを[m]表記で指定します。
10,11行目でカメラのキャリブレーション結果をインポートします。
20行目でマーカーの情報を読み取っています。
22~28行目で読み取ったデータの調整をしています。

　詳しい動作についてはこのサイト(https://docs.opencv.org/master/d9/d6a/group__aruco.html):を見て解析して下さい。私も理解していない部分があります。

　実際に動作させると↓のようになります。

スクリーンショット 2020-10-19 212308