姿势之间的相对旋转（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）。

在测试中，两个标记都平放在我的桌子上且方向相同，相机笔直指向下方，我得到X = 0，Y = 0，Z = 0的预期结果，因为标记之间的相对方向是零。

但是，如果我将一个标记沿“ z”方向旋转90度（仍将其平放在桌面上），则会得到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.

对不起，如果我误解了您在那里的问题。我没有测试我的代码以获得正确的角度参考，而是测试了它的平移。

Answer 2

我怀疑您应该在R_test_to_ref上使用cv2.decomposeProjectionMatrix（），因为它是一个3x3旋转矩阵，而不是投影矩阵。在Python中将3x3旋转矩阵转换为Euler角的一种参考是here，代码复制如下：

# Code from https://www.learnopencv.com/rotation-matrix-to-euler-angles/
# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
def rotationMatrixToEulerAngles(R) :

    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])

    singular = sy < 1e-6

    if  not singular :
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], sy)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0

    return np.array([x, y, z])

您对欧拉角有什么用？您不需要OpenGL的Euler角，例如，您可以直接使用矩阵。欧拉角存在问题，在许多情况下，旋转矢量或四元数作为旋转的紧凑表示，会带来更多的运气。