理解cv2.recoverPose的坐标系变换

Question

我在理解

cv2.recoverPose(points1, points2)

的功能时遇到问题。根据我对文档的理解，这个函数应该返回从camera1到camera2的旋转矩阵以及从camera1到camera2的转换。

我正在使用一组人工点，

points_3d

，以及两个相机之间的变换，

T_1_0

，来恢复Python中的相机变换。为了获取相机上的点，我计算了 3D 点在相机传感器上的投影：

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.linalg.inv(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)

我希望结果等于

T_1_0

（截至断言），但结果是：

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

我在这里缺少什么？为什么它没有按预期工作？我做错了什么或者这里的预期行为是什么？

编辑

我用于投影的公式错误地引入了变换的逆过程。应该是以下内容：

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))

旁注

solvePnP

也解决了我一直试图在这里解决的问题（3D->2D投影）

_, 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)

但我还是不知道为什么

cv2.recoverPose

不起作用？查看文档，它还应该返回平移和旋转...

Answer 1

TL；博士

最重要的发现：

• 如果在相机 1 上找到点 1 并且在相机 2 上找到点 2，则

  cv2.recoverPose(points1, points2,...)

  函数返回从相机 1 到相机 2 的旋转。

• 返回的平移向量也是从camera1到camera2，但是在camera1的坐标系中。

• 将

  cameraMatrix

  设置为

  findEssentialMat

  非常重要。

描述

我现在可以使用

cv2.recoverPose

并恢复正确的旋转和平移。这是更新后的代码：

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

r, _ = cv2.Rodrigues(R)

结果是

# As expected
r = [[0.2]
 [0.1]
 [0.3]]

# expected: [0.2, 0.4, 0.1], but is:
t = [[0.43643578]
 [0.87287156]
 [0.21821789]]

但是！该文档表明，翻译的恢复只能达到一定规模！因此，在这种情况下，以下断言有效并且行为符合预期！：

factors = t.ravel() / t_0_10 
assert np.allclose(factors, factors[0])

评论： 有趣的是，如果我使用

K_c = np.eye(3)

，它有时有效，有时无效。但如果我使用真实相机的内在特性，这个断言总是正确的......

K_c = np.array([
    [485.0, 0.0, 320.0],
    [0.0, 485.0, 240.0],
    [0.0,0.0,1.0]
])