使用 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）求解PnP。给定 4 个对应点和相机矩阵，solvePnP 返回对象在相机坐标系中的原点。

4）现在我需要计算相机在世界坐标中的位置。旋转矩阵为：

rotM = cv2.Rodrigues(rvec)[0]

5) 相机的x,y,z位置为：

cameraPosition = -np.matrix(rotM).T * np.matrix(tvec)

Answer 2

我在Python中创建了一个函数，可以从图像坐标获取XZ平面上的世界点（我稍后用Codeium添加了注释）：

def image2worldY0Position(u :int, v :int, mtx :np.ndarray, dist:np.ndarray, rvec:np.ndarray, tvec:np.ndarray):
    """
    Converts the pixel coordinates of a point in an image to its corresponding 
    world coordinates on XZ plane.
    
    Args:
        u (int): x-coordinate of the point in the image.
        v (int): y-coordinate of the point in the image.
        mtx (ndarray): Camera matrix.
        dist (ndarray): Distortion coefficients.
        rvec (ndarray): Rotation vector.
        tvec (ndarray): Translation vector.
    
    Returns:
        ndarray: The world coordinates of the point.
    """
    
    # Extract rotation matrix and its inverse
    R, _ = cv2.Rodrigues(rvec)
    R_inv = R.T
    
    # Compute optimal camera matrix and its inverse
    optimalMtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (ww, hh), 0, (ww, hh))
    mtx_inv = np.linalg.inv(optimalMtx)
    
    # Compute intermediate matrices
    tempMat = R_inv @ np.linalg.inv(mtx) @ np.array([[u],[v],[1]])
    tempMat2 = R_inv @ tvec
    
    # Compute the scaling factor
    s = tempMat2[1,0] / tempMat[1,0]
    
    # Undistort the pixel coordinates
    uv_1 = np.array([[u, v]], dtype=np.float32)
    uv_undistorted = cv2.undistortPoints(uv_1, mtx, dist, None, optimalMtx)
    uv_undistorted = np.array([[uv_undistorted[0][0][0], uv_undistorted[0][0][1],1]], dtype=np.float32).T
    
    # Compute camera coordinates
    camera_point = mtx_inv @ (s*uv_undistorted)
    
    # Compute world coordinates
    world_point = R_inv @ (camera_point - tvec)
    
    # Project world coordinates to image coordinates
    points_2d, _ = cv2.projectPoints(world_point, rvec, tvec, mtx, dist)
    
    return world_point