相机Z轴与ArUco标记Z轴夹角计算异常问题排查

我的实现步骤 #

1. 检测图像中的ArUco标记：

corners, ids, rvec, tvec = aruco_utils.get_aruco_marker_info(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY), marker_length, camera_matrix, dist_coeffs)

2. 创建相机与标记坐标系间的转换矩阵：

M        = np.zeros((4,4))
M[:3,:3] = cv2.Rodrigues(rvec)[0]
M[:3,3]  = tvec[0].T
M[3,3]   = 1

marker_to_camera_matrix = np.matrix(M)
camera_to_marker_matrix = np.linalg.inv(marker_to_camera_matrix)

3. 将相机Z轴转换到标记坐标系：

coord_z = np.array([0,0,1, 1])
coord_z = np.dot(camera_to_marker_matrix, coord_z)

4. 计算转换后向量与标记Z轴的夹角：

camera_z_proj = np.array(coord_z)[0][:3]
camera_z_proj = camera_z_proj / np.linalg.norm(camera_z_proj)
marker_z_proj = np.array([0,0,1])

dot_product = np.dot(camera_z_proj, marker_z_proj)

# Calculate the angle in radians
angle_radians = np.arccos(dot_product)

# Convert the angle from radians to degrees
angle_degrees = np.degrees(angle_radians)

遇到的问题 #

对不同姿态的标记计算时，结果均近似相同；即使使用合成ArUco标记，角度始终约为70°。期望得到两个Z轴的真实夹角。

问题原因与修正方案 #

错误点分析 #

1. 齐次矩阵误用：向量的方向转换不需要考虑平移（平移仅作用于空间点，不影响方向）。你用齐次矩阵乘[0,0,1,1]，得到的是相机坐标系中(0,0,1)这个点在标记系中的位置，而非相机Z轴的方向向量，平移项干扰了结果。
2. 旋转矩阵逻辑混淆：OpenCV中ArUco返回的rvec对应的旋转矩阵R，是标记坐标系到相机坐标系的旋转（即把标记系的点/向量转换到相机系）。要将相机系的向量转到标记系，只需用R的转置（旋转矩阵是正交矩阵，逆等于转置），无需计算齐次矩阵的逆。

修正后的实现步骤 #

1. 检测ArUco标记（此步骤无需修改）：

corners, ids, rvec, tvec = aruco_utils.get_aruco_marker_info(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY), marker_length, camera_matrix, dist_coeffs)

2. 获取双向旋转矩阵：

# R：标记坐标系 → 相机坐标系的旋转矩阵
R, _ = cv2.Rodrigues(rvec)
# R_camera_to_marker：相机坐标系 → 标记坐标系的旋转矩阵（正交矩阵逆等于转置）
R_camera_to_marker = R.T

3. 转换相机Z轴方向到标记坐标系：

# 相机Z轴在自身坐标系中的方向向量
camera_z = np.array([0, 0, 1])
# 转换到标记坐标系并归一化
camera_z_in_marker = R_camera_to_marker @ camera_z
camera_z_in_marker = camera_z_in_marker / np.linalg.norm(camera_z_in_marker)

4. 计算两Z轴的夹角：

# 标记Z轴在自身坐标系中的方向向量
marker_z = np.array([0, 0, 1])
# 计算点积，限制范围避免数值误差导致arccos报错
dot_product = np.clip(np.dot(camera_z_in_marker, marker_z), -1.0, 1.0)
# 计算弧度与角度
angle_radians = np.arccos(dot_product)
angle_degrees = np.degrees(angle_radians)

关键说明 #

之前结果固定约70°，是因为你误将空间点的坐标转换当成了方向向量的转换，平移项的干扰导致计算出的并非真实的轴夹角。修正后仅用旋转矩阵处理方向向量，就能得到随标记姿态变化的真实夹角。

内容的提问来源于stack exchange，提问作者Alberto