Eigen库 欧拉角、旋转矩阵、四元数之间的相互转换

1. #include <iostream>  
2.   
3. #include <Eigen/Core>  
4. #include <Eigen/Geometry>  
5. #define PI (3.1415926535897932346f)  
6.   
7. int main(int argc, char **argv)   
8. {  
9.     using ::std::cout;  
10.     using ::std::endl;  
11.     double yaw = PI/3,pitching = PI/4,droll = PI/6;  
12.       
13.       
14.       //EulerAngles to RotationMatrix  
15.     ::Eigen::Vector3d ea0(yaw,pitching,droll);  
16.     ::Eigen::Matrix3d R;  
17.     R = ::Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ())  
18.         * ::Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY())  
19.         * ::Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX());  
20.   
21.     cout << R << endl << endl;  
22.   
23.     //RotationMatrix to Quaterniond  
24.     ::Eigen::Quaterniond q;   
25.     q = R;      
26.     cout << q.x() << endl << endl;  
27.     cout << q.y() << endl << endl;  
28.     cout << q.z() << endl << endl;  
29.     cout << q.w() << endl << endl;  
30.       
31.     //Quaterniond to RotationMatrix  
32.     ::Eigen::Matrix3d Rx = q.toRotationMatrix();  
33.     cout << Rx << endl << endl;  
34.       
35.       
36.     //RotationMatrix to EulerAngles  
37.     ::Eigen::Vector3d ea1 = Rx.eulerAngles(2,1,0);       
38.     cout << ea1/PI*180 << endl << endl;  
39.       
40.       
41.     std::cin.ignore();  
42.     return 0;  
43. }