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

Question

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

我有一个相机在看一个区域。

1)我画了一张这个地区的地图。

2)我使用cv2.getPerspectiveTransform将4个图像点与地图上的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)绘制该区域的地图

2)使用cv2.findChessboardCorners的棋盘图像校准相机，得到相机矩阵和失真系数

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

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

5)摄像头的x，y，z位置为：cameraPosition = -np.matrix(rotM).T * np.matrix(tvec)