最近学到了"pitch,Roll,Yaw"几个专业术语,简单来说就是坐标系的绕轴旋转。
在几何测量里,坐标系作为测量基础,是重点关注对象。不同的坐标系会导致不同的测量的数值。坐标系通过旋转和平移进行转换:
绕X旋转
绕Y旋转
绕Z旋转
计算上面的角度就可以了。
在CALYPSO中建立两个坐标系“base”和“Alignment4”
还是一样通过PCM把坐标系差异写出
writeDiffCoordSysToFile("Alignment4","base","C:\temp\diff1.txt")
在下面程序里计算
import numpy as np
def get_transformation(filename):
diff = np.genfromtxt(filename)
r = diff[:9].reshape((3,3))
# t = diff[9:].reshape((3,1))
# print(r)
# print(t)
return r
rotate_matrix = get_transformation('diff1.txt')
RM = np.array(rotate_matrix)
# 旋转矩阵到欧拉角
def rotateMatrixToEulerAngles2(R):
theta_z = np.arctan2(RM[1, 0], RM[0, 0]) / np.pi * 180
theta_y = np.arctan2(-1 * RM[2, 0], np.sqrt(RM[2, 1] * RM[2, 1] + RM[2, 2] * RM[2, 2])) / np.pi * 180
theta_x = np.arctan2(RM[2, 1], RM[2, 2]) / np.pi * 180
print(f"Euler angles:\ntheta_x: {theta_x}\ntheta_y: {theta_y}\ntheta_z: {theta_z}")
return theta_x, theta_y, theta_z
if __name__ == '__main__':
rotateMatrixToEulerAngles2(RM)
得出结果:
Euler angles:
theta_x: 0.3315200000001608
theta_y: 0.22513000000007857
theta_z: 5.111249999999829