python - 姿势之间的相对旋转 (rvec)

Question

我想使用 python 和 cv2 找到两个 Aruco 标记之间的相对旋转角度。我将我的标记称为“测试”标记和“引用”标记。

我已经使用 cv2.aruco.estimatePoseSingleMarkers 成功检索了标记的姿势。这给了我一个用于测试标记的“test_rvec”和一个用于引用标记的“ref_rvec”。

据我了解，rvec(与 cv2.solvePnP 使用的格式相同，我相信 aruco 在幕后使用)是标记相对于相机的旋转。因此，为了获得测试标记相对于引用标记的旋转，我这样做:

R_ref_to_cam = cv2.Rodrigues(ref_rvec)[0] #reference to camera
R_test_to_cam = cv2.Rodrigues(test_rvec)[0] #test to camera
R_cam_to_ref = np.transpose(R_ref_to_cam) #inverse of reference to camera
R_test_to_ref = np.matmul(R_test_to_cam,R_cam_to_ref) #test to reference

然后我使用 cv2.decomposeProjectionMatrix 计算得到的矩阵 (R_test_to_ref) 的欧拉角。

在测试中，两个标记都平放在我的 table 上并具有相同的方向，相机垂直向下，我得到 X=0，Y=0，Z=0，正如预期的那样，因为标记之间的相对方向为零。

但是，如果我将一个标记沿“z”方向旋转 90 度(仍将其平放在我的 table 上)，我会得到 X=30、Y=30、Z=90。我希望看到其中两个轴报告为 90 度，第三个(旋转)轴报告为 0 度。我究竟做错了什么？

Answer 1

我不确定我是否理解正确。但与您不同的是，当我获得引用时，我使用的是基本向量数学:
AB = AC - BC

A和B是标记，C是相机。所以如果我反转引用 tvec&rvec，添加到第一个标记，我得到了相对点。为此，我写了两个函数:

def inversePerspective(rvec, tvec):
""" Applies perspective transform for given rvec and tvec. """
    R, _ = cv2.Rodrigues(rvec)
    R = np.matrix(R).T
    invTvec = np.dot(R, np.matrix(-tvec))
    invRvec, _ = cv2.Rodrigues(R)
    return invRvec, invTvec



def relativePosition(rvec1, tvec1, rvec2, tvec2):
""" Get relative position for rvec2 & tvec2. Compose the returned rvec & tvec to use composeRT with rvec2 & tvec2 """
    rvec1, tvec1 = rvec1.reshape((3, 1)), tvec1.reshape((3, 1))
    rvec2, tvec2 = rvec2.reshape((3, 1)), tvec2.reshape((3, 1))

    # Inverse the second marker, the right one in the image
    invRvec, invTvec = inversePerspective(rvec2, tvec2)

    info = cv2.composeRT(rvec1, tvec1, invRvec, invTvec)
    composedRvec, composedTvec = info[0], info[1]

    composedRvec = composedRvec.reshape((3, 1))
    composedTvec = composedTvec.reshape((3, 1))
    return composedRvec, composedTvec

我基本上是用来指点的。在这种情况下，我使用了其中一个标记旋转，但也许它可以帮助你。我实际上是在我的博客上写的，但它是土耳其语的。对不起，我没有时间写英文:
My blog post

您也可以查看我的源代码，希望它可以帮助您:
My source code.

对不起，如果我误解了你的问题。我没有测试我的代码以获得准确的角度引用，我测试了它的翻译。