矩陣的使用可參考系列博客：點擊此處

原文鏈接：基于eigen實現歐拉角(RPY), 旋轉矩陣, 旋轉向量, 四元數之間的變換。

也可以參考另一篇博客：eigen 中四元數、歐拉角、旋轉矩陣、旋轉向量。

---

在機器人學中經常會涉及到歐拉角,旋轉矩陣,旋轉向量,四元數之間的轉換,因此基于eigen對變換關系進行實現,以便參考:
注意，代碼中Eigen::AngleAxisd中使用的是必須是弧度radian，不是角度angle。所以，不要直接輸入角度angle，需要先把角度angle轉化為弧度radian。

#include <iostream>#include <Eigen/Core> #include <Eigen/Geometry>using namespace std;#define PI (3.1415926535897932346f)int main(int argc, char **argv) {/**** 1. 旋轉向量 ****/cout << endl << "********** AngleAxis **********" << endl;//1.0 初始化旋轉向量,沿Z軸旋轉45度的旋轉向量Eigen::AngleAxisd rotation_vector1 (M_PI/4, Eigen::Vector3d(0, 0, 1)); //1.1 旋轉向量轉換為旋轉矩陣//旋轉向量用matrix()轉換成旋轉矩陣Eigen::Matrix3d rotation_matrix1 = Eigen::Matrix3d::Identity(); //單位矩陣初始化rotation_matrix1 = rotation_vector1.matrix();cout << "rotation matrix1 =\n" << rotation_matrix1 << endl; //或者由羅德里格公式進行轉換rotation_matrix1 = rotation_vector1.toRotationMatrix();cout << "rotation matrix1 =\n" << rotation_matrix1 << endl; /*1.2 旋轉向量轉換為歐拉角*///將旋轉向量轉換為旋轉矩陣,再由旋轉矩陣轉換為歐拉角,詳見旋轉矩陣轉換為歐拉角Eigen::Vector3d eulerAngle1 = rotation_vector1.matrix().eulerAngles(2,1,0);cout << "eulerAngle1, z y x: " << eulerAngle1 << endl;/*1.3 旋轉向量轉四元數*/Eigen::Quaterniond quaternion1(rotation_vector1);//或者Eigen::Quaterniond quaternion1_1;quaternion1_1 = rotation_vector1;cout << "quaternion1 x: " << quaternion1.x() << endl;cout << "quaternion1 y: " << quaternion1.y() << endl;cout << "quaternion1 z: " << quaternion1.z() << endl;cout << "quaternion1 w: " << quaternion1.w() << endl;cout << "quaternion1_1 x: " << quaternion1_1.x() << endl;cout << "quaternion1_1 y: " << quaternion1_1.y() << endl;cout << "quaternion1_1 z: " << quaternion1_1.z() << endl;cout << "quaternion1_1 w: " << quaternion1_1.w() << endl;/**** 2. 旋轉矩陣 *****/cout << endl << "********** RotationMatrix **********" << endl;//2.0 旋轉矩陣初始化Eigen::Matrix3d rotation_matrix2;rotation_matrix2 << 0.707107, -0.707107, 0, 0.707107, 0.707107, 0, 0, 0, 1; ;//或直接單位矩陣初始化Eigen::Matrix3d rotation_matrix2_1 = Eigen::Matrix3d::Identity();//2.1 旋轉矩陣轉換為歐拉角//ZYX順序，即先繞x軸roll,再繞y軸pitch,最后繞z軸yaw,0表示X軸,1表示Y軸,2表示Z軸Eigen::Vector3d euler_angles = rotation_matrix2.eulerAngles(2, 1, 0); cout << "yaw(z) pitch(y) roll(x) = " << euler_angles.transpose() << endl;//2.2 旋轉矩陣轉換為旋轉向量Eigen::AngleAxisd rotation_vector2;rotation_vector2.fromRotationMatrix(rotation_matrix2);//或者Eigen::AngleAxisd rotation_vector2_1(rotation_matrix2);cout << "rotation_vector2 " << "angle is: " << rotation_vector2.angle() * (180 / M_PI) << " axis is: " << rotation_vector2.axis().transpose() << endl;cout << "rotation_vector2_1 " << "angle is: " << rotation_vector2_1.angle() * (180 / M_PI) << " axis is: " << rotation_vector2_1.axis().transpose() << endl;//2.3 旋轉矩陣轉換為四元數Eigen::Quaterniond quaternion2(rotation_matrix2);//或者Eigen::Quaterniond quaternion2_1;quaternion2_1 = rotation_matrix2;cout << "quaternion2 x: " << quaternion2.x() << endl;cout << "quaternion2 y: " << quaternion2.y() << endl;cout << "quaternion2 z: " << quaternion2.z() << endl;cout << "quaternion2 w: " << quaternion2.w() << endl;cout << "quaternion2_1 x: " << quaternion2_1.x() << endl;cout << "quaternion2_1 y: " << quaternion2_1.y() << endl;cout << "quaternion2_1 z: " << quaternion2_1.z() << endl;cout << "quaternion2_1 w: " << quaternion2_1.w() << endl;/**** 3. 歐拉角 ****/cout << endl << "********** EulerAngle **********" << endl;//3.0 初始化歐拉角(Z-Y-X，即RPY, 先繞x軸roll,再繞y軸pitch,最后繞z軸yaw)Eigen::Vector3d ea(0.785398, -0, 0);//3.1 歐拉角轉換為旋轉矩陣Eigen::Matrix3d rotation_matrix3;rotation_matrix3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX());cout << "rotation matrix3 =\n" << rotation_matrix3 << endl; //3.2 歐拉角轉換為四元數,Eigen::Quaterniond quaternion3;quaternion3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX());cout << "quaternion3 x: " << quaternion3.x() << endl;cout << "quaternion3 y: " << quaternion3.y() << endl;cout << "quaternion3 z: " << quaternion3.z() << endl;cout << "quaternion3 w: " << quaternion3.w() << endl;//3.3 歐拉角轉換為旋轉向量Eigen::AngleAxisd rotation_vector3;rotation_vector3 = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(ea[2], Eigen::Vector3d::UnitX()); cout << "rotation_vector3 " << "angle is: " << rotation_vector3.angle() * (180 / M_PI) << " axis is: " << rotation_vector3.axis().transpose() << endl;/**** 4.四元數 ****/cout << endl << "********** Quaternion **********" << endl;//4.0 初始化四元素,注意eigen Quaterniond類四元數初始化參數順序為w,x,y,zEigen::Quaterniond quaternion4(0.92388, 0, 0, 0.382683);//4.1 四元數轉換為旋轉向量Eigen::AngleAxisd rotation_vector4(quaternion4);//或者Eigen::AngleAxisd rotation_vector4_1;rotation_vector4_1 = quaternion4;cout << "rotation_vector4 " << "angle is: " << rotation_vector4.angle() * (180 / M_PI) << " axis is: " << rotation_vector4.axis().transpose() << endl;cout << "rotation_vector4_1 " << "angle is: " << rotation_vector4_1.angle() * (180 / M_PI) << " axis is: " << rotation_vector4_1.axis().transpose() << endl;//4.2 四元數轉換為旋轉矩陣Eigen::Matrix3d rotation_matrix4;rotation_matrix4 = quaternion4.matrix();Eigen::Matrix3d rotation_matrix4_1;rotation_matrix4_1 = quaternion4.toRotationMatrix();cout << "rotation matrix4 =\n" << rotation_matrix4 << endl;cout << "rotation matrix4_1 =\n" << rotation_matrix4_1 << endl; //4.4 四元數轉歐拉角(Z-Y-X，即RPY)Eigen::Vector3d eulerAngle4 = quaternion4.matrix().eulerAngles(2,1,0);cout << "yaw(z) pitch(y) roll(x) = " << eulerAngle4.transpose() << endl;return 0; }

終端輸出:

總結

以上是生活随笔為你收集整理的【自动驾驶】30.c++实现基于eigen实现欧拉角(RPY), 旋转矩阵, 旋转向量, 四元数之间的变换（附代码）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。