pytransform3d中A2B变换矩阵计算异常的技术咨询

核心问题解答 #

1. A2B变换矩阵的定义 #

A2B是将坐标系A中的点坐标转换到坐标系B中的点坐标的齐次变换矩阵。在齐次坐标下，对任意点P有：
P_B = A2B @ P_A
其中P_A是点P在A系中的坐标，P_B是点P在B系中的坐标。

2. 变换链式法则与正确推导 #

已知O2A是O系到A系的变换矩阵（P_A = O2A @ P_O），O2B是O系到B系的变换矩阵（P_B = O2B @ P_O）。要得到A2B，需通过O系中转：

1. 先将A系坐标转换为O系坐标：P_O = inv(O2A) @ P_A
2. 再将O系坐标转换为B系坐标：P_B = O2B @ P_O = O2B @ inv(O2A) @ P_A

因此，A2B的正确表达式为：A2B = O2B @ inv(O2A)

3. pytransform3d.concat的参数顺序逻辑 #

pytransform3d的pytr.concat(T1, T2)函数定义是：先执行T2变换，再执行T1变换，返回的复合矩阵为T1 @ T2。这和直觉中的“先传先执行”相反，参数顺序是「后执行的变换在前，先执行的变换在后」。

所以要实现O2B @ inv(O2A)，需要把后执行的O2B放在第一个参数位置，先执行的inv(O2A)放在第二个参数位置，即：
pytr.concat(O2B, pytr.invert_transform(O2A))
这就是你测试中得到正确结果的写法。

而你最初尝试的pytr.concat(pytr.invert_transform(O2A), O2B)对应的是inv(O2A) @ O2B，含义是“先将O系点转到B系，再转到A系”，完全不符合A2B的需求，因此结果错误。

4. 后处理需求 #

不需要对结果进行额外后处理，只要明确变换矩阵的定义和concat函数的参数顺序，即可得到正确的变换矩阵。

测试代码与输出 #

测试代码 #

import numpy as np
from math import degrees, radians
from numpy.linalg import inv
import pytransform3d.rotations as pyrot
import pytransform3d.transformations as pytr

def print_rp(X2Y):
    r = list(map(degrees, pyrot.intrinsic_euler_xyz_from_active_matrix(X2Y[0:3,0:3])))
    p = X2Y[0:3,3:4].ravel().tolist()
    print(f"intrinsic Euler XYZ rotations {r}")
    print(f"translations {p}")

print("# O2A")
O2A = pytr.transform_from(
    R = np.block([[1,0,0],[0,1,0],[0,0,1]]),
    p = [0.05,0.05,-0.05]
)
print_rp(O2A)

print("# O2B")
O2B = pytr.transform_from(
    R = np.block([[-1,-0,-0],[0,-1,0],[0,0,1]]),
    p = [0.05,-0.05,-0.05],
)
print_rp(O2B)

print("# A2B (should work but doesn't)")
A2B = pytr.concat(pytr.invert_transform(O2A), O2B)
print_rp(A2B)

print("# A2B (works but do not know why)")
A2B = pytr.concat(O2B, pytr.invert_transform(O2A))
print_rp(A2B)

print("# A2B (works as expected)")
A2B = inv(O2A) @ O2B
print_rp(A2B)

输出结果 #

# O2A
intrinsic Euler XYZ rotations [0.0, 0.0, 0.0]
translations [0.05, 0.05, -0.05]
# O2B
intrinsic Euler XYZ rotations [0.0, 0.0, 180.0]
translations [0.05, -0.05, -0.05]
# A2B (should work but doesn't)
intrinsic Euler XYZ rotations [0.0, 0.0, 180.0]
translations [0.1, 0.0, 0.0]
# A2B (works but do not know why)
intrinsic Euler XYZ rotations [0.0, 0.0, 180.0]
translations [0.0, -0.1, 0.0]
# A2B (works as expected)
intrinsic Euler XYZ rotations [0.0, 0.0, 180.0]
translations [0.0, -0.1, 0.0]

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