旋转矩阵与欧拉角

其他相关的内容网上很多，这里就简单记录一下不同欧拉角分解顺序时，对应的角度怎么计算

#include <opencv2/opencv.hpp>
#include <iostream>

using namespace cv;
using namespace std;
#define PI acos(-1)

void getEulerAngles(Mat& rotCamerMatrix, Vec3d& eulerAngles)
{
	//{
	//    //顺序为Z-Y-X,测试过Z*Y*X可还原
	//    eulerAngles[0] = atan2(rotCamerMatrix.at<double>(2, 1), rotCamerMatrix.at<double>(2, 2)) * 180 / PI;
	//    eulerAngles[1] = atan2(-rotCamerMatrix.at<double>(2, 0), sqrt(rotCamerMatrix.at<double>(2, 1) * rotCamerMatrix.at<double>(2, 1) + rotCamerMatrix.at<double>(2, 2) * rotCamerMatrix.at<double>(2, 2))) * 180 / PI;
	//    eulerAngles[2] = atan2(rotCamerMatrix.at<double>(1, 0), rotCamerMatrix.at<double>(0, 0)) * 180 / PI;
	//}

	{
		//顺序为ZXY，测试过Z*X*Y可还原
		eulerAngles[0] = atan2(rotCamerMatrix.at<double>(2, 1), sqrt(rotCamerMatrix.at<double>(0, 1) * rotCamerMatrix.at<double>(0, 1) + rotCamerMatrix.at<double>(1, 1) * rotCamerMatrix.at<double>(1, 1))) * 180 / PI;
		eulerAngles[1] = atan2(-rotCamerMatrix.at<double>(2, 0), rotCamerMatrix.at<double>(2, 2)) * 180 / PI;
		eulerAngles[2] = atan2(-rotCamerMatrix.at<double>(0, 1), rotCamerMatrix.at<double>(1, 1)) * 180 / PI;
	}
}

void getEulerMatrix(Mat rotMatrix, Vec3d& eulerAngles, Mat& x_mat, Mat& y_mat, Mat& z_mat)
{
	getEulerAngles(rotMatrix, eulerAngles);
	double angleY = eulerAngles[1];//偏航
	double angleX = eulerAngles[0];//俯仰
	double angleZ = eulerAngles[2];//滚动
	cout << "angleY = " << angleY << endl;
	cout << "angleX = " << angleX << endl;
	cout << "angleZ = " << angleZ << endl << endl;

	y_mat = Mat::zeros(Size(3, 3), CV_64F);
	y_mat.at<double>(0, 0) = cos(angleY / 180 * PI);
	y_mat.at<double>(0, 2) = sin(angleY / 180 * PI);
	y_mat.at<double>(1, 1) = 1.0;
	y_mat.at<double>(2, 0) = -sin(angleY / 180 * PI);
	y_mat.at<double>(2, 2) = cos(angleY / 180 * PI);

	x_mat = Mat::zeros(Size(3, 3), CV_64F);
	x_mat.at<double>(0, 0) = 1.0;
	x_mat.at<double>(1, 1) = cos(angleX / 180 * PI);
	x_mat.at<double>(1, 2) = -sin(angleX / 180 * PI);
	x_mat.at<double>(2, 1) = sin(angleX / 180 * PI);
	x_mat.at<double>(2, 2) = cos(angleX / 180 * PI);

	z_mat = Mat::zeros(Size(3, 3), CV_64F);
	z_mat.at<double>(0, 0) = cos(angleZ / 180 * PI);
	z_mat.at<double>(0, 1) = -sin(angleZ / 180 * PI);
	z_mat.at<double>(1, 0) = sin(angleZ / 180 * PI);
	z_mat.at<double>(1, 1) = cos(angleZ / 180 * PI);
	z_mat.at<double>(2, 2) = 1.0;
	cout << "rotMatrix \n" << rotMatrix << endl << endl;
	cout << "Z*X*Y  \n" << z_mat * x_mat * y_mat << endl << endl;
}

int main()
{
	//使用罗德里格斯函数生成一个旋转矩阵
	//因为旋转矩阵不能随意设置，但是旋转向量可以随便写个值
	//利用Rodrigues函数来生成一个符合约束的旋转矩阵
	Vec3d rvec=Vec3d(1,2,3);
	Mat R;
	Rodrigues(rvec, R);
	cout << "旋转矩阵\n" << R << endl;

	Vec3d angle;
	Mat x, y, z;
	getEulerMatrix(R, angle, x, y, z);

	return 0;
}

上述示例中将给定的旋转矩阵按照ZXY的方式进行欧拉角分解，分解出对应的角度后验证一下：

将xyz各个角度转换为 xyz对应的单独的旋转矩阵，然后一次按照Z*X*Y的顺序乘起来，与原旋转矩阵比较，假如两者完全相同，则说明分解正确。

如下所示

 

 怎么通过上图中的关系计算出每个轴对应的角度呢，以ZXY顺序为例：

Z1X2Y3，表示的是Z轴的旋转角度为1，X轴的旋转角度为2，Y轴的旋转角度为3

//顺序为ZXY，测试过Z*X*Y可还原
eulerAngles[0] = atan2(rotCamerMatrix.at<double>(2, 1), sqrt(rotCamerMatrix.at<double>(0, 1) * rotCamerMatrix.at<double>(0, 1) + rotCamerMatrix.at<double>(1, 1) * rotCamerMatrix.at<double>(1, 1))) * 180 / PI;
eulerAngles[1] = atan2(-rotCamerMatrix.at<double>(2, 0), rotCamerMatrix.at<double>(2, 2)) * 180 / PI;
eulerAngles[2] = atan2(-rotCamerMatrix.at<double>(0, 1), rotCamerMatrix.at<double>(1, 1)) * 180 / PI;

关于将欧拉角反变换为旋转矩阵：

万向锁

假设我们按照z-y-x的顺序对物体进行旋转，先绕z轴旋转α度，再绕y轴旋转90度，再绕x轴旋转β度，这个时候就会出现万向锁。这个时候最后绕x轴旋转β角度，最终得到的效果和绕z轴旋转是一样的，这样就退化成只有两个轴的旋转有作用，另一个轴的自由度丢失了。

可见，最终的结果只与绕y轴和绕x轴旋转有关，因此无论怎么改变α和β的值，最终的旋转矩阵至于两个轴有关系，另一个轴失去了意义。