了解 cv2.recoverPose

Question

我在理解

cv2.recoverPose(points1, points2)

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

我正在使用一组人工点，

points_3d

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

T_1_0

，来恢复Python中的相机变换：

import cv2
import numpy as np

def calc_reprojection(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))))
T_1_0 = np.array([
    [0.4, 0.3, 0.2, 1.2],
    [0.1, 0.5, 0.3, 0.3],
    [0.5, 0.2, 0.5, 0.0],
])
points1 = []
points2 = []

for pt_3d in points_3d:
    points1.append(calc_reprojection(K_c, T_0, pt_3d))
    points2.append(calc_reprojection(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)

print(np.hstack((R, t)))
assert np.allclose(np.hstack((R, t)), T_1_0)

我希望结果等于

T_1_0

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

[[ 0.37667463  0.57394222 -0.7271221  -0.5267588 ]
 [ 0.87801198 -0.4714365   0.08271998 -0.32395173]
 [-0.29531541 -0.66958043 -0.68150632  0.78586287]]

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

编辑

好吧，经过一番尝试和错误，我找到了一种在这个简单示例中正确恢复平移和旋转的方法。我的发现：

1. 我用于投影的公式错误地引入了变换的逆

2. solvePnP

   似乎实现了我一直在尝试在这里使用的功能（3D->2D投影）

import cv2
import numpy as np

def calc_reprojection(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_reprojection(K_c, T_0, pt_3d) + np.random.uniform(0,0.1,1))
    points2.append(calc_reprojection(K_c, T_1_0, pt_3d) + np.random.uniform(0,0.1,1))

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

_, r, t = cv2.solvePnP(points_3d, points2, K_c, None)

print(r)
print(t)
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]
])