如何利用归一化方向向量、实际距离与相机Matrix4x4计算物体世界坐标

已知相机朝向正前方，且拥有以下条件：相机局部空间的归一化方向向量、物体到相机的实际距离、相机的4x4位姿矩阵，可按以下步骤计算物体的世界坐标：

步骤1：提取相机的世界空间位置 #

相机的4x4位姿矩阵（用于将相机局部空间的点转换到世界空间）中，最后一列的前三个元素就是相机在世界空间中的坐标（记为C_world）。

注：若使用的是视图矩阵（将世界空间点转换到相机空间的矩阵，即位姿矩阵的逆），需先对视图矩阵求逆，得到相机的位姿矩阵后再提取位置。

步骤2：将局部方向向量转换为世界空间方向 #

已知的归一化方向向量是相机局部空间的（起点为相机），需要通过相机的旋转矩阵转换为世界空间的方向向量：

1. 从相机4x4位姿矩阵的左上3x3子矩阵提取旋转矩阵R，该矩阵描述相机局部空间到世界空间的旋转变换。
2. 用旋转矩阵与局部方向向量进行乘法运算，得到世界空间的归一化方向向量dir_world：
   • 若局部方向向量为列向量：dir_world = R × dir_local
   • 若局部方向向量为行向量：dir_world = dir_local × R^T（R^T为旋转矩阵的转置，旋转矩阵的逆等于其转置）

由于输入的dir_local已归一化，转换后的dir_world无需再次归一化。

步骤3：计算物体的世界坐标 #

物体相对于相机的位移为dir_world × distance（distance为物体到相机的实际距离），因此物体的世界坐标为：

P_world = C_world + dir_world × distance

示例代码（Python） #

# 假设相机位姿矩阵为列优先格式（图形学常用）
camera_pose = [
    [0.0, 0.0, 1.0, 5.0],   # 第一列：旋转分量 + 相机X坐标
    [0.0, 1.0, 0.0, 3.0],   # 第二列：旋转分量 + 相机Y坐标
    [-1.0, 0.0, 0.0, 2.0],  # 第三列：旋转分量 + 相机Z坐标
    [0.0, 0.0, 0.0, 1.0]    # 齐次坐标行
]

# 提取相机世界位置
camera_world_pos = [camera_pose[0][3], camera_pose[1][3], camera_pose[2][3]]

# 提取旋转矩阵（左上3x3）
rotation = [
    [camera_pose[0][0], camera_pose[0][1], camera_pose[0][2]],
    [camera_pose[1][0], camera_pose[1][1], camera_pose[1][2]],
    [camera_pose[2][0], camera_pose[2][1], camera_pose[2][2]]
]

# 相机局部空间的归一化方向向量（比如正前方为[0,0,1]）
local_dir = [0, 0, 1]

# 转换为世界空间方向向量
world_dir = [
    rotation[0][0]*local_dir[0] + rotation[0][1]*local_dir[1] + rotation[0][2]*local_dir[2],
    rotation[1][0]*local_dir[0] + rotation[1][1]*local_dir[1] + rotation[1][2]*local_dir[2],
    rotation[2][0]*local_dir[0] + rotation[2][1]*local_dir[1] + rotation[2][2]*local_dir[2]
]

# 物体到相机的实际距离
distance = 15.0

# 计算物体世界坐标
object_world_pos = [
    camera_world_pos[0] + world_dir[0] * distance,
    camera_world_pos[1] + world_dir[1] * distance,
    camera_world_pos[2] + world_dir[2] * distance
]

print("物体世界坐标：", object_world_pos)

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