旋转矩阵转欧拉角
- 1 关于欧拉角
- 2 转换公式推导
- 2.1 由欧拉角构造旋转矩阵
- 2.2 由旋转矩阵推算欧拉角
- 2.2.1 一般情况
- 2.2.2 约束滚转角
- 3 转换代码（C++)
- 3.1 欧拉角-->旋转矩阵
- 3.2 旋转矩阵-->欧拉角
- 3.2.1 一般情况
- 3.2.2 约束滚转自由度
在我的应用场景中有一个角度始终为0，添加这个约束后就不用考虑欧拉角奇异性问题。借此机会自己推导了一下公式，梳理一下欧拉角和旋转矩阵之间的变换关系。
1 关于欧拉角
需要注意的是，由于欧拉角和旋转矩阵之间的转换关系跟很多因素有关，如各轴旋转角定义、欧拉角顺规、手性定义、参考坐标系等，因此讨论之前需要先明确定义。
本文讨论的内容约束如下：右手系、ZYX顺规（Z-Yaw，X-Roll，Y-Pitch）、内在旋转、主动旋转，也即参考文章【4】中的NED_Z-Y-X_Euler_Angles。
2 转换公式推导
2.1 由欧拉角构造旋转矩阵
欧拉角构造旋转矩阵直接将三个基础旋转矩阵按顺序相乘即可：
2.2 由旋转矩阵推算欧拉角
2.2.1 一般情况
从上面构造出的旋转矩阵可以很容易地推算出欧拉角：
Y轴俯仰角(pitch):
X轴滚转角(roll):
Z轴偏航角(yaw):
其中
当俯仰角 时
，即
，旋转矩阵退化成如下形式：
此时 和
的计算公式中
就失效了，即出现了Gimbal Lock。
要处理这种情况，根据情况给 或
，然后再根据退化后的旋转矩阵计算另一个角度即可。
2.2.2 约束滚转角
在我的应用场景中，滚转角 ，因此欧拉角退化为两个自由度，此时，
，旋转矩阵退化为：
X轴滚转角(roll):
Y轴俯仰角(pitch):
Z轴偏航角(yaw):
3 转换代码（C++)
参考文章：Rotation Matrix To Euler Angles
3.1 欧拉角–>旋转矩阵
// 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;
}3.2 旋转矩阵–>欧拉角
3.2.1 一般情况
// 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//由于欧拉角存在奇异性，当pitch为90°时会出现解不稳定的情况因此需要进行特殊处理
{
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);
}3.2.2 约束滚转自由度
// 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;
}
Vec3f rotationMatrixToEulerAngles_RollEquel0(Mat &R)
{
assert(isRotationMatrix(R));
double x,y,z;
x = 0;
y = atan2(-R.at<double>(2,0), R.at<double>(2,2));
z = atan2(-R.at<double>(0,1), R.at<double>(1,1));
return Vec3f(x, y, z);
}
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
