网站导航
> 文章列表 > 旋转矩阵与欧拉角

旋转矩阵与欧拉角

网友: 文章列表 2024-03-22 00:48:44

旋转矩阵与欧拉角

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

#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;

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

 

网络标签：

相关问题