如何从给定Z值的图片中计算三维坐标

import numpy as np

# 相机内参矩阵 (K)
K = np.array([[fx, 0, cx],
              [0, fy, cy],
              [0, 0, 1]])

# 相机外参矩阵 (R, t)
R = np.array([[...], [...], [...]])  # 旋转矩阵
t = np.array([[tx], [ty], [tz]])     # 平移向量

def pixel_to_world(pixel, depth):
    """
    Convert pixel coordinates (u, v) and depth to world coordinates (X, Y, Z).
    
    :param pixel: Tuple (u, v) representing pixel coordinates.
    :param depth: Depth value at the pixel.
    :return: World coordinates (X, Y, Z).
    """
    u, v = pixel
    
    # 归一化坐标
    x_n = (u - cx) / fx
    y_n = (v - cy) / fy
    
    # 旋转和平移
    P_n = np.array([[x_n], [y_n], [1]])
    P_w = np.dot(np.linalg.inv(K), np.dot(R, np.dot(K, P_n)) + depth * t)
    
    return P_w[:3]

# 示例使用
pixel = (320, 240)
depth = 1.5  # 假设深度值为1.5米
world_coords = pixel_to_world(pixel, depth)
print("World Coordinates:", world_coords)