arucoマーカ同士の距離を図りたい

Question

下記内容にて、座標の取得はできましたが、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()

Comments

[fana]

何が本件の 不明点/問題点 なのでしょうか？

estimatePoseSingleMarkers　なるメソッドでマーカの３次元位置姿勢が得られ，且つ，その値が（精度的に）信頼できるのだとしたら，
マーカ間の距離を算出することに関して現在何が障害となっているのでしょうか？
コレの精度が悪すぎて使えない的な話なのでしょうか？

[ryu]

アドバイスありがとうございます
現状困っていることは、複数のarucoマーカー0から4のIDがあり、そのIDの座標をしゅとくしたいというところです、現状、左から順番にえらばれているので、特定して取得したいです

Answer 1

アドバイス

estimatePoseSingleMarkers で得られる　rvec, tvec　とは何なのか？
すなわち
これらをどう使えば何を求めることができるのか？ というのを把握してください．

より具体的に言えば，

３次元空間上のある点の座標値が

• カメラ座標系で書いたら C_p
• マーカ座標系（各マーカ毎に定義されていると思われる直交基底）書いたら C_m

であるとき，C_p と C_m との間の変換を rvec, tvec を用いてどのように表すのか？　というのを把握してください．

そうすれば，「マーカ間の距離はどう求めるか？」という話は自明となるのではないでしょうか．

Comments

[fana]

リファレンスによると

> The returned transformation is the one that transforms points from each marker coordinate system to the camera coordinate system.

とのことですが，具体的な数式までは提示されていない様子．
あなたがやるべきことは，その「具体的な数式」を把握（というか「まぁこうだろうな」っていうのが本当に合ってるのかどうかを軽く確認）することでしょう．