Python中4x4变换矩阵跨坐标系转换失败，如何排查修正？

问题背景 #

我正在尝试将变换矩阵从PyBullet仿真坐标系转换到另一个目标坐标系：

PyBullet仿真坐标系 #

• X轴：向前
• Y轴：远离相机
• Z轴：向上
  注：不确定XY平面朝向

目标坐标系 #

• X轴：向前
• Y轴：向上
• Z轴：朝向相机
  注：不确定XZ平面朝向

我推测坐标转换关系为 (X,Y,Z) → (X,-Z,Y)，并编写了如下Python代码实现该转换：

import numpy as np

def transform_matrix(self, transformation_matrix):
    # X maps to X (1,0,0)
    # Y maps to -Z (0,0,-1)
    # Z maps to -Y (0,-1,0)
    C = np.matrix([
        [1, 0, 0, 0],
        [0, 0, -1, 0],
        [0, -1, 0, 0],
        [0, 0, 0, 1]
    ])

    C_prime = np.transpose(C)

    return C @ transformation_matrix @ C_prime 

参考坐标系转换方法编写的代码未生效，不确定是映射关系错误还是代码本身问题，恳请帮助。

问题排查与解决方案 #

1. 修正轴映射关系 #

首先要明确：你的核心问题出在轴映射的矩阵构造错误。根据你给出的坐标系定义，正确的轴对应关系应该是：

转换矩阵C的每一列，应该是目标坐标系的基向量在PyBullet坐标系下的表示，所以正确的C矩阵应该是：

C = [
    [1, 0, 0, 0],  # 目标X轴 = PyBullet X轴
    [0, 0, 1, 0],  # 目标Y轴 = PyBullet Z轴
    [0, -1, 0, 0], # 目标Z轴 = -PyBullet Y轴
    [0, 0, 0, 1]
]

2. 变换矩阵的转换逻辑 #

你的代码中C_inv = C.T的逻辑是对的——因为正交矩阵的逆等于转置，但矩阵C的构造错误导致结果不符合预期。

修正后的代码 #

import numpy as np

def transform_matrix(self, transformation_matrix):
    # 修正后的轴映射：
    # 目标X = PyBullet X
    # 目标Y = PyBullet Z
    # 目标Z = -PyBullet Y
    C = np.matrix([
        [1, 0, 0, 0],
        [0, 0, 1, 0],
        [0, -1, 0, 0],
        [0, 0, 0, 1]
    ])
    C_inv = C.T  # 正交矩阵的逆等于转置
    return C @ transformation_matrix @ C_inv

3. 验证方法 #

可以用简单测试向量验证正确性：

• PyBullet坐标系下的点(0,1,0)（Y轴正方向，远离相机），转换后应对应目标坐标系的(0,0,-1)（Z轴负方向，朝向相机），代入修正后的矩阵计算结果符合预期。
• PyBullet坐标系下的点(0,0,1)（Z轴正方向，向上），转换后应对应目标坐标系的(0,1,0)（Y轴正方向，向上），计算结果正确。

如果修正后仍有问题，建议打印原始变换矩阵和转换后的矩阵，对比关键轴的变换结果，或者用PyBullet中已知姿态的物体做测试，手动验证映射关系是否完全匹配两个坐标系的实际方向。

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