pythonでARマーカーの姿勢推定

マーカーの検出から実際の距離への変換まで，一通りの実装がシンプルに書けたため記載します．

本プログラムはカメラからマーカーまでの距離と角度を取得することができます．

なお，姿勢推定ではカメラパラメータが必要になる（とりあえず試したい場合はさらに下に方法を記載）ため、それは前回のブログ記事参照していただけるとありがたいです．
https://qiita.com/ReoNagai/items/5da95dea149c66ddbbdd

実装

マーカーサイズは各自で適切なものに変えてください．
前回のブログ記事で作ったカメラパラメータであるカメラ行列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に関してはこちらのサイトが参考になります．
http://pongsuke.hatenadiary.jp/entry/2018/04/19/102146

座標変換で参考になるサイト
https://kamino.hatenablog.com/entry/rotation_expressions#sec1