我想我已经知道了。问题出在步骤4中所述的方法上。相机位置无法仅从单应矩阵计算。相机固有矩阵也是必需的。因此,正确的过程如下:
1)绘制区域地图
2)使用棋盘图像校准摄像机,从而cv2.findChessboardCorners
得出摄像机矩阵和失真系数
3)具有世界坐标(3D)和图像坐标(2D)的solvePnP。在给定4个对应点和相机矩阵的情况下,solvePnP在相机的坐标系中返回对象的origo。
4)现在,我需要计算摄像机在世界坐标中的位置。旋转矩阵为:rotM = cv2.Rodrigues(rvec)[0]
5)相机的x,y,z位置是:cameraPosition = -np.matrix(rotM).T * np.matrix(tvec)