已知两相机位姿4x4矩阵，如何计算相对球坐标角度及平移半径？

假设你已经通过M = torch.inverse(p1).mm(p2)得到了相机1到相机2的相对变换矩阵，这个4x4矩阵的结构为：

M = [
    [R00, R01, R02, tx],
    [R10, R11, R12, ty],
    [R20, R21, R22, tz],
    [0,   0,   0,   1]
]

其中R是3x3的相对旋转矩阵，t = [tx, ty, tz]^T是相对平移向量。

一、计算平移半径 #

平移半径就是相机1到相机2的空间直线距离，直接计算平移向量的L2范数即可：

# 提取平移向量
t = M[:3, 3]
# 计算平移半径（欧氏距离）
translation_radius = torch.norm(t, p=2).item()

二、计算相对旋转角度（phi、theta） #

这里的phi（偏航角）和theta（俯仰角）通常对应Z-Y-X旋转顺序的欧拉角（航空航天常用顺序），我们可以从旋转矩阵中提取这两个角度（若无需滚转角可忽略）。

步骤1：提取旋转矩阵 #

R = M[:3, :3]

步骤2：推导欧拉角 #

根据Z-Y-X旋转顺序的矩阵推导，俯仰角theta和偏航角phi的计算方式如下：

• 俯仰角theta：对应绕Y轴的旋转角度，范围[-π/2, π/2]

  theta = torch.arcsin(-R[2, 0]).item()
  

• 偏航角phi：对应绕Z轴的旋转角度，范围[-π, π]
  需要处理奇点情况（当theta接近±π/2时）：

  if torch.abs(theta) < torch.pi/2 - 1e-6:
      phi = torch.atan2(R[1, 0], R[0, 0]).item()
  else:
      # 奇点场景下的替代计算方式（默认滚转角为0）
      phi = torch.atan2(-R[0, 1], R[1, 1]).item()
  

注意事项 #

• 欧拉角的旋转顺序直接影响计算结果，若你需要的是其他旋转顺序（如X-Y-Z），需对应调整推导公式。
• 上述代码返回弧度制角度，如需转换为角度制，乘以180/torch.pi即可。

内容的提问来源于stack exchange，提问作者K.XGai