python - 2 个 ArUco 标记平面之间的角度

Question

我想测量 ArUco 标记与第二个引用 ArUco 标记定义的平面之间的角度偏差。

引用 ArUco 标记 (M1) 固定在平坦的墙壁上，第二个 ArUco 标记 (M2) 位于同一墙前面几厘米处。我想知道标记 M2 何时偏离 M1 的 xy 平面超过 10 度。

以下是配置说明:

为此，我认为应该计算姿势 rvec 之间的相对旋转，如本文所述:

Relative rotation between pose (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

计算composedRvec，我得到以下结果:

两个 ArUco 标记位于同一平面(composedRvec 值位于右上角):

两个 ArUco 标记呈 90 度角:

我不太明白结果:

当标记位于同一平面时，使用 0,0,0 组成的Rvec 即可。

但是为什么在第二种情况下是 0,1.78,0 呢？

当 2 个标记之间的角度超过 10 度时，我应该对生成的composedRvec 有什么一般条件来告诉我？

我是否遵循了composeRvec的正确策略？

**** 编辑 ***

2 个标记在同一 xy 平面且呈 40° 角时的结果:

||composedRvec||= sqrt(0.619^2+0.529^2+0.711^2)=1.08 rad = 61.87°

**** 编辑 2 ***

通过在 40° 角配置下重新进行测量，我发现即使不修改设置或照明，这些值也会波动很大。有时，我会遵循正确的值(value)观:

||composedRvec||= sqrt(0.019^2+0.012^2+0.74^2)=0.74 rad = 42.4° 这是相当准确的。

**** 编辑 3 ***

这是我基于 @Gilles-Philippe Paillé 编辑后的答案的最终代码:

import numpy as np
import cv2
import cv2.aruco as aruco


cap = cv2.VideoCapture(0, cv2.CAP_DSHOW)  # Get the camera source
img_path='D:/your_path/'
# FILE_STORAGE_READ
cv_file = cv2.FileStorage(img_path+"camera.yml", cv2.FILE_STORAGE_READ)
matrix_coefficients = cv_file.getNode("K").mat()
distortion_coefficients = cv_file.getNode("D").mat()

nb_markers=2

def track(matrix_coefficients, distortion_coefficients):
    while True:

        ret, frame = cap.read()
        # operations on the frame come here
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)  # Change grayscale
        aruco_dict = aruco.custom_dictionary(nb_markers, 5)  

        parameters = aruco.DetectorParameters_create()  # Marker detection parameters
        # lists of ids and the corners beloning to each id
        corners, ids, rejected_img_points = aruco.detectMarkers(gray, 
aruco_dict,parameters=parameters,cameraMatrix=matrix_coefficients,distCoeff=distortion_coefficients)
                                                          

    # store rz1 and rz2
    R_list=[]

    if np.all(ids is not None):  # If there are markers found by detector
        for i in range(0, len(ids)):  # Iterate in markers

        # Estimate pose of each marker and return the values rvec and tvec---different from camera coefficients
            rvec, tvec, markerPoints = aruco.estimatePoseSingleMarkers(corners[i], 0.02, matrix_coefficients,
                                                                   distortion_coefficients)
            (rvec - tvec).any()  # get rid of that nasty numpy value array error



            aruco.drawDetectedMarkers(frame, corners)  # Draw A square around the markers

            aruco.drawAxis(frame, matrix_coefficients, distortion_coefficients, rvec, tvec, 0.01)  # Draw Axis


            R, _ = cv2.Rodrigues(rvec)
            # convert (np.matrix(R).T) matrix to array using np.squeeze(np.asarray()) to get rid off the ValueError: shapes (1,3) and (1,3) not aligned
            R = np.squeeze(np.asarray(np.matrix(R).T))
            R_list.append(R[2])


        # Display the resulting frame


    if len(R_list) == 2:


        print('##############')
        angle_radians = np.arccos(np.dot(R_list[0], R_list[1]))
        angle_degrees=angle_radians*180/np.pi
        print(angle_degrees)


    cv2.imshow('frame', frame)
#   Wait 3 milisecoonds for an interaction. Check the key and do the corresponding job.
    key = cv2.waitKey(3000) & 0xFF
    if key == ord('q'):
     break



track(matrix_coefficients, distortion_coefficients)

以下是一些结果:

红色 -> 真实角度，白色 -> 测量角度

这超出了问题的范围，但我发现姿势估计波动很大。例如，当 2 个标记靠墙时，值会轻松从 9° 跳到 37°，而无需接触系统。

Answer 1

结果使用 Angle-axis representation ，即向量的范数是旋转角度(你想要的)，向量的方向是旋转轴。

您正在寻找θ = ||composedRvec||。请注意，结果以弧度为单位。条件将是 ||composedRvec|| > 10*π/180.

编辑:要仅考虑两个平面 Z 轴之间的角度，请将两个旋转向量 rvec1 和 rvec2 转换为矩阵并提取第三列。角度为 angle_radians = np.arccos(np.dot(rz1, rz2))