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

Question

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

我有一个相机看着一个区域.

1)我画了一张该地区的地图.

2)我通过使用在我的地图上将4个图像点匹配到4个点来计算单应性 cv2.getPerspectiveTransform

3)H homography将每个世界坐标转换为摄像机坐标; 这是正常的

4)为了计算相机矩阵我遵循这个:

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)根据这个,相机的位置应该这样计算:

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

我得到的坐标是完全错误的.我想这个问题应该在步骤4或5的某个地方.我的方法有什么问题？

Answer 1

我想我现在已经有了.问题在于步骤4中描述的方法.不能仅从单应性矩阵计算相机位置.相机内在矩阵也是必要的.所以,正确的程序如下:

1)绘制该地区的地图

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

3)使用世界坐标(3D)和图像坐标(2D)求解P2P.给定4个对应点和摄像机矩阵,solvePnP在摄像机的坐标系中返回对象的原点.

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

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