这篇博客将会分享旋转矩阵和欧拉角的相互转换。
三维旋转矩阵有三个自由度，旋转能够使用多种方法表示（旋转矩阵，欧拉角，四元数，轴角，李群与李代数），比如一个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。
在计算将旋转矩阵转换成欧拉角之前，先介绍一下欧拉角转换为旋转矩阵的方法。
欧拉角转换为旋转矩阵
假如已知旋转角，绕着X-Y-Z三个轴的角度分别为 那么三个旋转矩阵可以表示如下：
如果旋转顺序为Z-Y-X的顺序，那么旋转矩阵可表示如下：
关于上面的旋转矩阵，以z轴旋转为例：
第一种物理意义，座标系的旋转。 其应用场景有SLAM，机械臂运动学等。
如上图所示， P点不变，座标系 O-XYZ旋转 α ,得到新的座标系O-X’Y’Z’，在新座标系下，P点的座标变为P’ ，则有：
第二种物理意义，向量的旋转，其应用场景有机器人的姿态估计等。
如上图所示，座标系 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, math.cos(theta[0]), -math.sin(theta[0])],
[0, math.sin(theta[0]), math.cos(theta[0]) ]])
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],
[0, 1, 0],
[-math.sin(theta[1]), 0, math.cos(theta[1])]])
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
[math.sin(theta[2]), math.cos(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++
// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{
Mat Rt;
transpose(R, Rt);
Mat shouldBeIdentity = Rt * R;
Mat I = Mat::eye(3,3, shouldBeIdentity.type());
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)
{
assert(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));
}
else
{
x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
y = atan2(-R.at<double>(2,0), sy);
z = 0;
}
return Vec3f(x, y, z);
}
Python
# Checks if a matrix is a valid rotation matrix.
def isRotationMatrix(R) :
Rt = np.transpose(R)
shouldBeIdentity = np.dot(Rt, R)
I = np.identity(3, dtype = R.dtype)
n = np.linalg.norm(I - shouldBeIdentity)
return n < 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 ).
def rotationMatrixToEulerAngles(R) :
assert(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])
else :
x = math.atan2(-R[1,2], R[1,1])
y = math.atan2(-R[2,0], sy)
z = 0
return np.array([x, y, z])