首页>>人工智能->根据相机旋转矩阵求解三个轴的旋转角/欧拉角/姿态角 或 旋转矩阵与欧拉角(Euler Angles)之间的相互转换,以及python和C++代码实现

根据相机旋转矩阵求解三个轴的旋转角/欧拉角/姿态角 或 旋转矩阵与欧拉角(Euler Angles)之间的相互转换,以及python和C++代码实现

时间:2023-11-29 本站 点击:0

相机标定过程中,我们会得到一个3x3旋转矩阵,下面是我们把旋转矩阵欧拉角之间的相互转换:

1 旋转矩阵转换为欧拉角(Euler Angles)

1、旋转矩阵是一个3x3的矩阵,如下:

$$ R=\left(\begin{array}{ccc} r{11} & r{12} & r{13} \ r{21} & r{22} & r{23} \ r{31} & r{32} & r_{33} \end{array}\right) $$

刚体旋转的旋转矩阵是由三个基本旋转矩阵复合而成的。

2、欧拉角(Euler Angles)

欧拉角来描述刚体在三维欧几里得空间取向

3、旋转矩阵转换为欧拉角的公式:

Z轴对应的欧拉角

$$ \theta{z}=\arctan 2\left(-r{31},  r_{11}\right) $$

Y轴对应的欧拉角

$$ \theta{y}=\arctan 2\left(-r{31}, \sqrt{r{31}{ }^{2}+r{33}{ }^{2}}\right) $$

X轴对应的欧拉角

$$ \theta{x}=\arctan 2\left(-r{32},  r_{33}\right) $$

注意:

上面公式计算测的欧拉角是弧度制

上面公式的意思是,相机坐标系想要转到与世界坐标系完全平行(即$x_c$平行于$x_w$,$y_c$平行于$y_w$,$z_c$平行于$z_w$,且他们的方向都是相同的),需要旋转3次,设原始相机坐标系C0

1、C0绕其z轴旋转,得到新的坐标系C1;

2、C1绕其y轴旋转,得到新的坐标系C2(注意旋转轴为C1的y轴,而非C0的y轴);

3、C2绕其x轴旋转,得到新的坐标系C3。此时C3与世界坐标系W完全平行。

特别注意:旋转顺序为z y x,切记不能调换

4、python实现:旋转矩阵转换为欧拉角

import numpy as nprotate_matrix = [[-0.0174524064372832, -0.999847695156391, 0.0],                 [0.308969929589947, -0.00539309018185907, -0.951056516295153],                 [0.950911665781176, -0.0165982248672099, 0.309016994374948]]RM = np.array(rotate_matrix)# 旋转矩阵到欧拉角(弧度值)def rotateMatrixToEulerAngles(R):    theta_z = np.arctan2(RM[1, 0], RM[0, 0])    theta_y = np.arctan2(-1 * RM[2, 0], np.sqrt(RM[2, 1] * RM[2, 1] + RM[2, 2] * RM[2, 2]))    theta_x = np.arctan2(RM[2, 1], RM[2, 2])    print(f"Euler angles:\ntheta_x: {theta_x}\ntheta_y: {theta_y}\ntheta_z: {theta_z}")    return theta_x, theta_y, theta_z# 旋转矩阵到欧拉角(角度制)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_zif __name__ == '__main__':    rotateMatrixToEulerAngles(RM)    rotateMatrixToEulerAngles2(RM)

输出结果如下:

Euler angles:theta_x: -0.05366141770874149theta_y: -1.2561686529408898theta_z: 1.6272221428848495Euler angles:theta_x: -3.0745727573994635theta_y: -71.97316217014685theta_z: 93.23296111753567

5、C++实现:旋转矩阵转换为欧拉角

//计算出相机坐标系的三轴旋转欧拉角,旋转后可以转出世界坐标系。//旋转顺序为z、y、xconst double PI = 3.141592653;double thetaz = atan2(r21, r11) / PI * 180;double thetay = atan2(-1 * r31, sqrt(r32*r32 + r33*r33)) / PI * 180;double thetax = atan2(r32, r33) / PI * 180;

2 欧拉角转换为旋转矩阵

欧拉角转换为旋转矩阵,就是沿XYZ三个轴进行旋转,参考旋转矩阵

1、利用上面生成的弧度值的欧拉角,再转换为旋转矩阵

# 欧拉角转换为旋转矩阵# 输入为欧拉角为 弧度制# euler_angles = [-0.05366141770874149, -1.2561686529408898, 1.6272221428848495]def eulerAnglesToRotationMatrix(theta):    R_x = np.array([[1, 0, 0],                    [0, np.cos(theta[0]), -np.sin(theta[0])],                    [0, np.sin(theta[0]), np.cos(theta[0])]                    ])    R_y = np.array([[np.cos(theta[1]), 0, np.sin(theta[1])],                    [0, 1, 0],                    [-np.sin(theta[1]), 0, np.cos(theta[1])]                    ])    R_z = np.array([[np.cos(theta[2]), -np.sin(theta[2]), 0],                    [np.sin(theta[2]), np.cos(theta[2]), 0],                    [0, 0, 1]                    ])    R = np.dot(R_z, np.dot(R_y, R_x))    print(f"Rotate matrix:\n{R}")    return Rif __name__ == '__main__':    euler_angles = [-0.05366141770874149, -1.2561686529408898, 1.6272221428848495]    eulerAnglesToRotationMatrix(euler_angles)

输出结果:

Rotate matrix:[[-1.74524064e-02 -9.99847695e-01 -7.38075162e-16] [ 3.08969930e-01 -5.39309018e-03 -9.51056516e-01] [ 9.50911666e-01 -1.65982249e-02  3.09016994e-01]]

参考:https://www.cnblogs.com/singlex/p/RotateMatrix2Euler.html 参考:https://zhuanlan.zhihu.com/p/259999988 参考:https://blog.csdn.net/qq_15642411/article/details/83583695


本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。
如若转载,请注明出处:/AI/1085.html