旋转矩阵与欧拉⾓的相互转换及代码
这篇博客将会分享旋转矩阵和欧拉⾓的相互转换。
三维旋转矩阵有三个⾃由度，旋转能够使⽤多种⽅法表⽰（旋转矩阵，欧拉⾓，四元数，轴⾓，李群与李代数），⽐如⼀个3x3的矩阵，⽐如，甚⾄可以将旋转表⽰为三个数字，即绕三个轴x,y,z的旋转⾓度。在原始的欧拉⾓公式中，旋转是通过围绕Z轴，Y轴，X轴分别旋转得到的。它分别对应于偏航，俯仰和横滚。
当定义⼀个旋转时，可能还会引起歧义，⽐如对于⼀个给定的点(x,y,z)，可以把这个点视为⾏向量（x,y,z）或者列向量量，如果使⽤⾏向量，那么就必须后乘旋转矩阵（旋转矩阵在后），如果使⽤列向量，就要左乘旋转矩阵（旋转矩阵在前）。
在MATLAB中，rotm2euler.m能够实现旋转矩阵到欧拉⾓的转换。下⽂中将要叙述的转换代码参考于MATLAB中的rotm2euler.m实现。不同于MATLAB实现的是，它的旋转顺序是Z-Y-X，⽽下⾯的实现是X-Y-Z。
在计算将旋转矩阵转换成欧拉⾓之前，先介绍⼀下欧拉⾓转换为旋转矩阵的⽅法。
欧拉⾓转换为旋转矩阵
brown是什么意思假如已知旋转⾓，绕着X-Y-Z三个轴的⾓度分别为
那么三个旋转矩阵可以表⽰如下：如果旋转顺序为Z-Y-X的顺序，那么旋转矩阵可表⽰如下：
关于上⾯的旋转矩阵，以z轴旋转为例：
第⼀种物理意义，坐标系的旋转。 其应⽤场景有SLAM，机械臂运动学等。
如上图所⽰， P点不变，坐标系 O-XYZ旋转 α ，得到新的坐标系O-X’Y’Z’，在新坐标系下，P点的坐标变为P’ ，则有：
第⼆种物理意义，向量的旋转，其应⽤场景有机器⼈的姿态估计等。
[x ,y ,z ]T θθθx y z
如上图所⽰，坐标系 O-XYZ不变， P’点旋转α ，得到新的点P （我也想反过来，但⽤的教材上的图，没办法），则有：
短话剧剧本
对应于上述旋转顺序的C++代码如下：
C++
// Calculates rotation matrix given euler angles.
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
// Calculate rotation about x axis
Mat R_x =(Mat_<double>(3,3)<<
1,0,0,
0,cos(theta[0]),-sin(theta[0]),
好看的美剧推荐0,sin(theta[0]),cos(theta[0])
);
// Calculate rotation about y axis
Mat R_y =(Mat_<double>(3,3)<<
cos(theta[1]),0,sin(theta[1]),
0,1,0,
-sin(theta[1]),0,cos(theta[1])
);
// Calculate rotation about z axis
Mat R_z =(Mat_<double>(3,3)<<
cos(theta[2]),-sin(theta[2]),0,
sin(theta[2]),cos(theta[2]),0,
0,0,1);
// Combined rotation matrix
Mat R = R_z * R_y * R_x;
return R;
}
Python
# Calculates Rotation Matrix given euler angles.
def eulerAnglesToRotationMatrix(theta):
R_x = np.array([[1,0,0],
[0, s(theta[0]),-math.sin(theta[0])],
[0, math.sin(theta[0]), s(theta[0])]])
R_y = np.array([[s(theta[1]),0, math.sin(theta[1])],
[0,1,0],
[-math.sin(theta[1]),0, s(theta[1])]])
usually是什么意思
R_z = np.array([[s(theta[2]),-math.sin(theta[2]),0],
[math.sin(theta[2]), s(theta[2]),0],
[0,0,1]])
R = np.dot(R_z, np.dot( R_y, R_x ))
return R
在OpenCV中将旋转矩阵转换为欧拉⾓
将旋转矩阵转换为欧拉⾓就不是那么容易，不同的旋转顺序对应着不同的旋转⾓度。使⽤上⾯的代码，即使欧拉⾓看起来⾮常不同，您也可以验证与欧拉⾓[0.1920、2.3736、1.1170]（或[[11，136，64]度）和[-2.9496，0.7679，-2.0246]（或[-169，44，-116]度）相对应的旋转矩阵实际上是相同的。下⾯的代码显⽰了在给定旋转矩阵的情况下找到欧拉⾓的⽅法。以下代码的输出应与MATLAB的rotm2euler的输出完全匹配，但x和z的顺序会互换。
C++
bool isRotationMatrix(Mat &R)
{
Mat Rt;
transpo(R, Rt);
Mat shouldBeIdentity = Rt * R;
Mat I = Mat::eye(3,3, pe());
return norm(I, shouldBeIdentity)<1e-6;
}
// Calculates rotation matrix to euler angles
// The result is the same as MATLAB except the order
// of the euler angles ( x and z are swapped ).
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
asrt(isRotationMatrix(R));
float sy =sqrt(R.at<double>(0,0)* R.at<double>(0,0)+ R.at<double>(1,0)* R.at<double>(1,0)); bool singular = sy <1e-6;// If
float x, y, z;
房屋租赁纳税发票
if(!singular)
{
x =atan2(R.at<double>(2,1), R.at<double>(2,2));
y =atan2(-R.at<double>(2,0), sy);
z =atan2(R.at<double>(1,0), R.at<double>(0,0));
}
el
{
x =atan2(-R.at<double>(1,2), R.at<double>(1,1));
y =atan2(-R.at<double>(2,0), sy);
z =0;
}
resolution>baiu
return Vec3f(x, y, z);
}
Python
def isRotationMatrix(R):
Rt = np.transpo(R)
shouldBeIdentity = np.dot(Rt, R)
I = np.identity(3, dtype = R.dtype)
n = (I - shouldBeIdentity)
return n <1e-6
wjz# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order # of the euler angles ( x and z are swapped ).
hogan
def rotationMatrixToEulerAngles(R):
asrt(isRotationMatrix(R))
sy = math.sqrt(R[0,0]* R[0,0]+ R[1,0]* R[1,0])
singular = sy <1e-6
if not singular :
x = math.atan2(R[2,1], R[2,2])
y = math.atan2(-R[2,0], sy)
z = math.atan2(R[1,0], R[0,0])
el:
x = math.atan2(-R[1,2], R[1,1])
the holidayy = math.atan2(-R[2,0], sy)
z =0
return np.array([x, y, z])
