python - 使用 OpenCV Python 计算相机世界位置

Question

我想计算我的相机在世界坐标中的位置。这应该相当容易，但我没有得到我期望的结果。我相信我已经阅读了有关该主题的所有内容，但我的代码无法正常工作。这是我的做法:

我有一个摄像头在观察一个区域。

1) 我画了一张该地区的 map 。

2) 我通过使用 cv2.getPerspectiveTransform

将 4 个图像点与 map 上的 4 个点进行匹配来计算单应性

3) H单应性将每个世界坐标转换为相机坐标；这是正常工作

4) 为了计算我遵循的相机矩阵 this :

translation = np.zeros((3,1)) 
translation[:,0] = homography[:,2]

rotation = np.zeros((3,3))
rotation[:,0] = homography[:,0]
rotation[:,1] = homography[:,1]
rotation[:,2] = np.cross(homography[0:3,0],homography[0:3,1])

cameraMatrix = np.zeros((3,4))
cameraMatrix[:,0:3] = rotation
cameraMatrix[:,3] = homography[:,2]

cameraMatrix = cameraMatrix/cameraMatrix[2][3] #normalize the matrix

5) 根据this ，相机的位置应该这样计算:

x,y,z = np.dot(-np.transpose(rotation),translation)

我得到的坐标是完全错误的。我猜问题应该出在第 4 步或第 5 步的某处。我的方法有什么问题吗？

Answer 1

我想我现在明白了。问题在于步骤 4 中描述的方法。不能仅从单应性矩阵计算相机位置。相机内在矩阵也是必要的。因此，正确的程序如下:

1)绘制区域 map

2) 使用带有 cv2.findChessboardCorners 的棋盘图像校准相机，这会产生相机矩阵和失真系数

3) 使用世界坐标 (3D) 和图像坐标 (2D) 解决 PnP。给定 4 个对应点和相机矩阵，solvePnP 返回对象在相机坐标系中的原点。

4) 现在我需要计算相机在世界坐标中的位置。旋转矩阵为:rotM = cv2.Rodrigues(rvec)[0]

5) 相机的 x,y,z 位置为:cameraPosition = -np.matrix(rotM).T * np.matrix(tvec)