Eigen：：關於旋轉(旋轉向量\旋轉矩陣\四元數\歐拉角)的初始化和轉換以及應用
code:
#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv) {
//下面三個變量作爲下面演示的中間變量
AngleAxisd t_V(M_PI / 4, Vector3d(0, 0, 1)); //旋轉向量:繞z軸旋轉45度
Matrix3d t_R = t_V.matrix(); //旋轉向量--->旋轉矩陣
Quaterniond t_Q(t_V); //旋轉向量--->四元數
/***************************旋轉向量的初始化***************************/
/*******************************
* 對旋轉向量（軸角）賦值的三大種方法
* 1.使用角度和矢量
* 2.使用旋轉矩陣
* 3.使用四元數
*******************************/
///1.使用旋轉的角度和旋轉軸向量（此向量爲單位向量）來初始化角軸
AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）爲旋轉軸，旋轉45度
cout << "Rotation_vector1" << endl << V1.matrix() << endl;
///2.使用旋轉矩陣轉旋轉向量的方式
//2.1 使用旋轉向量的fromRotationMatrix()函數來對旋轉向量賦值（注意此方法爲旋轉向量獨有,四元數沒有）
AngleAxisd V2;
V2.fromRotationMatrix(t_R);
cout << "Rotation_vector2" << endl << V2.matrix() << endl;
//2.2 直接使用旋轉矩陣來對旋轉向量賦值
AngleAxisd V3;
V3 = t_R;
cout << "Rotation_vector3" << endl << V3.matrix() << endl;
//2.3 使用旋轉矩陣來對旋轉向量進行初始化
AngleAxisd V4(t_R);
cout << "Rotation_vector4" << endl << V4.matrix() << endl;
///3. 使用四元數來對旋轉向量進行賦值
//3.1 直接使用四元數來對旋轉向量賦值
AngleAxisd V5;
V5 = t_Q;
cout << "Rotation_vector5" << endl << V5.matrix() << endl;
//3.2 使用四元數來對旋轉向量進行初始化
AngleAxisd V6(t_Q);
cout << "Rotation_vector6" << endl << V6.matrix() << endl;
/***************************四元數的初始化***************************/
/*******************************
* 對四元數初始化的三大種方法
* 1.使用四元數的值Quaterniond(Vector4d(x,y,z,w)) 或者 Quaterniond(w,x,y,z,w)
* 2.使用旋轉矩陣
* 3.使用旋轉向量
*******************************/
///1.使用旋轉的角度和旋轉軸向量（此向量爲單位向量）來初始化四元數,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
Quaterniond Q1(cos((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 1 * sin((M_PI / 4) / 2));
//以（0,0,1）爲旋轉軸，旋轉45度
//第一種輸出四元數的方式
cout << "Quaternion1" << endl << Q1.coeffs() << endl;
//第二種輸出四元數的方式
cout << Q1.x() << endl << endl;
cout << Q1.y() << endl << endl;
cout << Q1.z() << endl << endl;
cout << Q1.w() << endl << endl;
///2. 使用旋轉矩陣初始化四元數的方式
//2.1 直接使用旋轉矩陣來對旋轉向量賦值
Quaterniond Q2;
Q2 = t_R;
cout << "Quaternion2" << endl << Q2.coeffs() << endl;
//2.2 使用旋轉矩陣來對四元數進行初始化
Quaterniond Q3(t_R);
cout << "Quaternion3" << endl << Q3.coeffs() << endl;
///3. 使用旋轉向量對四元數來進行賦值
//3.1 直接使用旋轉向量對四元數來賦值
Quaterniond Q4;
Q4 = t_V;
cout << "Quaternion4" << endl << Q4.coeffs() << endl;
//3.2 使用旋轉向量來對四元數進行初始化
Quaterniond Q5(t_V);
cout << "Quaternion5" << endl << Q5.coeffs() << endl;
/***************************旋轉矩陣的初始化***************************/
/*******************************
* 對旋轉矩陣初始化的三大種方法
* 1.直接矩陣賦值
* 2.使用旋轉向量
* 3.使用四元數
*******************************/
///1.使用旋轉矩陣的函數來初始化旋轉矩陣
Matrix3d R1=Matrix3d::Identity();
cout << "Rotation_matrix1" << endl << R1 << endl;
///2. 使用旋轉向量轉旋轉矩陣來對旋轉矩陣賦值
//2.1 使用旋轉向量的成員函數matrix()來對旋轉矩陣賦值
Matrix3d R2;
R2 = t_V.matrix();
cout << "Rotation_matrix2" << endl << R2 << endl;
//2.2 使用旋轉向量的成員函數toRotationMatrix()來對旋轉矩陣賦值
Matrix3d R3;
R3 = t_V.toRotationMatrix();
cout << "Rotation_matrix3" << endl << R3 << endl;
//3. 使用四元數轉旋轉矩陣來對旋轉矩陣賦值
//3.1 使用四元數的成員函數matrix()來對旋轉矩陣賦值
Matrix3d R4;
R4 = t_Q.matrix();
cout << "Rotation_matrix4" << endl << R4 << endl;
//3.2 使用四元數的成員函數toRotationMatrix()來對旋轉矩陣賦值
Matrix3d R5;
R5 = t_Q.toRotationMatrix();
cout << "Rotation_matrix5" << endl << R5 << endl;
/***************************旋轉向量\旋轉矩陣\四元數====>歐拉角***************************/
///1.旋轉向量===>歐拉角
Vector3d eulerFromAngleAxisd=t_V.matrix().eulerAngles(2,1,0); //(z,y,x)--(2,1,0) (z,x,y)--(2,0,1)
cout << "euler from 旋轉向量:\n"<<(180)/(M_PI)*eulerFromAngleAxisd.transpose()<<endl;
///2.旋轉矩陣===>歐拉角
Vector3d eulerFromRotationMatrix=t_R.eulerAngles(2,1,0);
cout << "euler from 旋轉矩陣:\n"<<(180)/(M_PI)*eulerFromRotationMatrix.transpose()<<endl;
///3.四元數===>歐拉角
Vector3d eulerFromQuaternion = t_Q.matrix().eulerAngles(2,1,0);
cout << "euler from 四元數:\n"<<(180)/(M_PI)*eulerFromQuaternion.transpose()<<endl;
/***************************應用舉例***************************/
/*******************************
* 利用3次旋轉構造一個姿態R
* 1.繞z軸旋轉45度
* 2.繞y軸旋轉70度
* 3.繞x軸旋轉30度
*******************************/
Vector3d rollpitchyaw((30.0/180.0)*M_PI,(70.0/180.0)*M_PI,(45.0/180.0)*M_PI);
AngleAxisd v_z((45.0/180.0)*M_PI , Vector3d(0, 0, 1));
AngleAxisd v_y((70.0/180.0)*M_PI , Vector3d(0, 1, 0));
AngleAxisd v_x((30.0/180.0)*M_PI , Vector3d(1, 0, 0));
//歐拉角爲ZYX-321順序的調用框架---------------------------------------
/*******************************
* 利用3次旋轉構造一個姿態R
* 1.繞z軸旋轉45度
* 2.繞y軸旋轉70度
* 3.繞x軸旋轉30度
*******************************/
Matrix3d R_n2b_zyx=v_x.matrix()*v_y.matrix()*v_z.matrix();
cout << "R_ZYX :\n"<<R_n2b_zyx<<endl;
AngleAxisd total;
total=v_x*v_y*v_z;
Vector3d zyx_Euler_fromR=R_n2b_zyx.eulerAngles(0,1,2);//Eigen中使用右乘的順序,因此ZYX對應的是012,實際上這個編號跟乘法的順序一致就可以了(從左向又右看的順序)
cout<< "zyx euler from Rotation [輸出順序爲:x,y,z]:\n"<<(180)/(M_PI)*zyx_Euler_fromR.transpose()<<endl;
//------------------------------------------------------------
//歐拉角爲ZXY-312順序的調用框架---------------------------------------
/*******************************
* 利用3次旋轉構造一個姿態R
* 1.繞z軸旋轉45度
* 2.繞x軸旋轉30度
* 3.繞y軸旋轉70度
*******************************/
Matrix3d R_n2b_zxy=v_y.matrix()*v_x.matrix()*v_z.matrix();
cout << "R_ZXY :\n"<<R_n2b_zxy<<endl;
Vector3d zxy_Euler_fromR=R_n2b_zxy.eulerAngles(1,0,2);//Eigen中使用右乘的順序,因此ZYX對應的是012
cout<< "zxy euler from Rotation [輸出順序爲:y,x,z]:\n"<<(180)/(M_PI)*zxy_Euler_fromR.transpose()<<endl;
//------------------------------------------------------------
///關於輸出順序: 就是做旋轉矩陣的乘法的順序
//(ZYX-321):v_x.matrix()*v_y.matrix()*v_z.matrix() 對應 pitch,roll,yaw的輸出順序
//(ZXY-312):v_y.matrix()*v_x.matrix()*v_z.matrix() 對應 roll,pitch,yaw的輸出順序
return 0;
}
參考內容:
https://blog.csdn.net/weixin_38213410/article/details/89892336
https://blog.csdn.net/yang__jing/article/details/82316093
Eigen:關於轉換成歐拉角的函數的說明:
https://eigen.tuxfamily.org/dox/group__Geometry__Module.html#ga17994d2e81b723295f5bc3b1f862ed3b