1 #include <iostream> 2 #include <cmath> 3 using namespace std; 4 5 #include <Eigen/Core> 6 // Eigen 幾何模塊 7 #include <Eigen/Geometry> 8 9 10 int main ( int argc, char** argv ) 11 { 12 13 //旋轉矩陣R 14 //3X3的旋轉矩陣能夠用Matrix3d，也Matrix3f 15 Eigen::Matrix3d R=Eigen::Matrix3d::Identity();//將其賦值爲單位矩陣 16 // Eigen::Matrix3d R; 17 // R.setIdentity(); //也是將旋轉矩陣賦值爲單位矩陣，與上面效果同樣 18 cout<<"R= \n"<<R<<endl; 19 20 //旋轉向量v（軸角） 21 //旋轉向量使用AngleAxisd(f),底層不直接是Matrix，但由於重載了運算符，運算能夠看成矩陣 22 Eigen::AngleAxisd v(M_PI/4,Eigen::Vector3d(0,0,1));//沿z軸旋轉了45度 23 cout << "rotation vector: Angle is: " << v.angle() * (180 / M_PI)<<endl//旋轉角 24 << " Axis is: " << v.axis().transpose() << endl<<endl;//旋轉軸 25 //將旋轉向量轉換爲旋轉矩陣 26 //方式一：用matrix() 27 // R=v.matrix(); 28 //方式二：用toRotationMatrix() 29 R=v.toRotationMatrix(); 30 cout<<"R=\n"<<R<<endl<<endl; 31 //將旋轉向量轉化爲四元數q 32 Eigen::Quaterniond q = Eigen::Quaterniond(v); 33 cout<<"q=\n"<<q.coeffs()<<endl<<endl;//coeffs的順序:(x,y,z,w) 34 cout<<"q=\n"<<q.x()<<endl<<q.y()<<endl<<q.z()<<endl<<q.w()<<endl<<endl;//四元數的另外一種輸出方式 35 36 37 //將旋轉矩陣轉化爲四元數 38 q = Eigen::Quaterniond(R); 39 cout<<"q=\n"<<q.coeffs()<<endl<<endl; 40 //將旋轉矩陣轉化爲歐拉角 41 Eigen::Vector3d euler_angles=R.eulerAngles(2,1,0); 42 cout<<"yaw(Z) pitch(Y) roll(X)=\n"<<euler_angles.transpose()<<endl<<endl; 43 44 45 //將四元數轉化爲旋轉矩陣 46 R=Eigen::Matrix3d(q); 47 cout<<"R=\n"<<R<<endl<<endl; 48 49 //將四元數轉化爲旋轉向量 50 v=Eigen::AngleAxisd(q); 51 cout<<v.matrix()<<endl<<endl; 52 return 0; 53 }
運行結果：R=
1 0 0
0 1 0
0 0 1
rotation vector: Angle is: 45
Axis is: 0 0 1
R=
0.707107 -0.707107 0
0.707107 0.707107 0
0 0 1
q=
0
0
0.382683
0.92388
q=
0
0
0.382683
0.92388
q=
0
0
0.382683
0.92388
yaw(Z) pitch(Y) roll(X)=
0.785398 -0 0
R=
0.707107 -0.707107 0
0.707107 0.707107 0
0 0 1
0.707107 -0.707107 0
0.707107 0.707107 0
0 0 1
ios