其他相关的内容网上很多,这里就简单记录一下不同欧拉角分解顺序时,对应的角度怎么计算
#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;
关于将欧拉角反变换为旋转矩阵: