如何基于trimesh相机参数与内外参计算像素对应真实世界坐标

注意：单像素无深度信息时仅能对应相机光心出发的一条射线，无法映射到唯一3D世界点，转换前必须先获取目标像素对应的沿相机光轴的深度值。

转换核心逻辑 #

坐标变换链路为：像素坐标系 → 相机坐标系 → 世界坐标系，对应计算规则如下：

• 像素坐标转相机坐标
  你持有的3x3内参矩阵K格式为[[fx, 0, cx], [0, fy, cy], [0, 0, 1]]，其中fx/fy为x/y方向焦距，cx/cy为光心在像素平面的偏移量。
  对于像素坐标(u, v)（原点在图像左上角，x轴向右、y轴向下，和trimesh默认像素坐标系对齐），对应深度值为d（沿相机光轴方向的垂直距离，单位与场景单位保持一致），相机坐标系下的3D点计算方式为：

  P_cam = [ (u - cx) * d / fx, (v - cy) * d / fy, d ]

• 相机坐标转世界坐标
  trimesh存储的旋转矩阵R（3x3）和平移向量t（3x1）默认是世界坐标到相机坐标的变换参数，满足P_cam = R @ P_world + t，因此反向求世界坐标时需要做逆变换。由于旋转矩阵是正交矩阵，逆矩阵等于转置矩阵，不需要做复杂的求逆运算：

  P_world = R.T @ (P_cam - t)
  如果你直接从trimesh相机属性中读取4x4的camera_to_world变换矩阵，可以直接用该矩阵做齐次坐标变换，不需要手动拆分R、t计算，出错概率更低。

可直接复用的实现代码 #

import numpy as np

def pixel2world(u: int, v: int, depth: float, intrinsic: np.ndarray, cam2world: np.ndarray) -> np.ndarray:
    """
    适配trimesh相机参数的像素转世界坐标函数
    Args:
        u: 像素横坐标，左上角为原点，向右递增
        v: 像素纵坐标，左上角为原点，向下递增
        depth: 像素对应沿相机光轴的Z向深度值
        intrinsic: 3x3相机内参矩阵，直接取scene.camera.K即可
        cam2world: 4x4相机到世界的变换矩阵，直接取scene.camera.camera_to_world即可
    Returns:
        长度为3的世界坐标数组 (x_w, y_w, z_w)
    """
    # 拆分内参
    fx, fy = intrinsic[0, 0], intrinsic[1, 1]
    cx, cy = intrinsic[0, 2], intrinsic[1, 2]
    # 构造相机坐标系下的齐次坐标
    p_cam_homo = np.array([
        (u - cx) * depth / fx,
        (v - cy) * depth / fy,
        depth,
        1.0
    ])
    # 直接乘相机到世界的变换矩阵得到世界坐标
    p_world_homo = cam2world @ p_cam_homo
    return p_world_homo[:3]

# 调用示例（已加载trimesh场景为scene，渲染得到的深度图为depth_map，shape为(H,W)）
# intrinsic = scene.camera.K
# cam2world = scene.camera.camera_to_world
# target_u, target_v = 320, 240  # 要转换的目标像素坐标
# target_depth = depth_map[target_v, target_u]  # 注意深度图索引顺序是(v,u)
# world_point = pixel2world(target_u, target_v, target_depth, intrinsic, cam2world)

常见踩坑说明 #

• 不要直接用R @ P_cam + t计算世界坐标，trimesh默认提供的R、t是世界转相机的正向变换，方向搞反会得到完全错误的结果。
• trimesh渲染输出的深度图默认存储的是沿光轴的Z向深度，刚好匹配转换需要的d参数，不需要额外转换为点到光心的欧氏距离。
• 批量转换大量像素点时，可以把所有像素坐标、深度值拼成(N,4)的齐次坐标数组，通过矩阵运算批量处理，比逐点循环效率高1~2个数量级。
• 转换前确认内参分辨率和当前图像分辨率完全一致，若图像做过缩放，需要对内参的fx、fy、cx、cy做同比例缩放。

内容的提问来源于stack exchange，提问作者Carlos