关于cv2.recoverPose坐标系转换功能的疑问

问题描述 #

测试cv2.recoverPose时发现返回的旋转矩阵与平移向量和预设参数差异较大，但solvePnP可正常恢复位姿。测试逻辑为：人工生成3D点，通过已知相机变换得到两个相机的2D投影点，再用cv2.findEssentialMat+cv2.recoverPose恢复位姿，结果不符合预期。

测试代码：

import cv2
import numpy as np

def calc_projection(K_c, transform, pt_3d):
    p_projected = np.hstack((K_c, np.zeros((3,1)))) @ np.vstack((transform, [0,0,0,1])) @ np.vstack((pt_3d.reshape(3,1), 1))
    p_projected = p_projected[:2,:] / p_projected[2,:]
    p_projected = p_projected.ravel()
    return p_projected

points_3d = np.random.rand(100, 3)

K_c = np.eye(3)

T_0 = np.hstack((np.eye(3), np.zeros((3,1))))
rot_vec = np.array([0.2, 0.1, 0.3])
R_1_0, _ = cv2.Rodrigues(np.array(rot_vec))
t_0_10 = np.array([0.2, 0.4, 0.1])
T_1_0 = np.hstack((R_1_0, t_0_10.reshape(3,1)))
points1 = []
points2 = []

for pt_3d in points_3d:
    points1.append(calc_projection(K_c, T_0, pt_3d))
    points2.append(calc_projection(K_c, T_1_0, pt_3d))

points1 = np.array(points1)
points2 = np.array(points2)

E, mask = cv2.findEssentialMat(points1, points2, method=cv2.RANSAC)
inliers1 = points1[mask]
inliers2 = points2[mask]
_, R, t, _ = cv2.recoverPose(E, inliers1, inliers2)

r, _ = cv2.Rodrigues(R)
assert np.allclose(r, rot_vec)
assert np.allclose(t, t_0_10)

实际返回结果：

r = [[0.20329041]
 [0.15711541]
 [0.37188371]]
t = [[0.50969714]
 [0.79593836]
 [0.32663581]]

solvePnP验证结果正常：

_, r, t = cv2.solvePnP(points_3d, points2, K_c, None)
assert np.allclose(r, rot_vec.reshape(3, 1), rtol=0.15)
assert np.allclose(t, t_0_10.reshape(3, 1), rtol=0.15)

原因分析 #

1. 平移向量的尺度不确定性 #

本质矩阵E仅编码旋转信息和平移的方向，无法恢复平移的绝对尺度。cv2.recoverPose返回的t是单位化后的向量，而预设的t_0_10是带有实际尺度的平移量，直接对比数值必然不匹配。正确验证方式是检查方向一致性：

# 验证平移方向一致
assert np.allclose(t / np.linalg.norm(t), t_0_10 / np.linalg.norm(t_0_10), rtol=1e-2)

2. 旋转矩阵的误差来源 #

• 本质矩阵估计噪声：cv2.findEssentialMat用RANSAC估计本质矩阵，若测试3D点分布不理想（如随机点存在共面、分布集中等情况），会导致本质矩阵估计偏差，进而影响位姿恢复精度。
• 多解性选择误差：本质矩阵分解会得到4种位姿解，cv2.recoverPose通过三角化点检查是否在相机前方选择正确解，但如果点深度分布不合理，可能引入微小误差。

3. 变换逻辑的一致性 #

cv2.recoverPose返回的R和t满足：相机2坐标系下的点P2 = R * P1 + t（P1为相机1坐标系下的空间点），与预设的T_1_0（相机1相对于相机0的变换）逻辑一致，但需确保投影过程无坐标系混淆。

修正后的验证示例 #

修改断言逻辑，允许微小误差并验证方向：

# 验证旋转向量（允许5%以内的相对误差）
assert np.allclose(r, rot_vec.reshape(3,1), rtol=5e-2)
# 验证平移方向一致
assert np.allclose(t / np.linalg.norm(t), t_0_10 / np.linalg.norm(t_0_10), rtol=1e-2)

solvePnP能正常恢复位姿是因为直接使用3D点与2D投影的对应关系，无需依赖本质矩阵分解，且能恢复带尺度的平移量（输入了真实3D坐标）。

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