四元数、欧拉角、旋转矩阵之间互相转换C++源码

转自四元数、欧拉角、旋转矩阵之间互相转换C++源码
还可参考
Creating a rotation matrix with pitch, yaw, roll using Eigen
Finding roll, pitch yaw from 3X3 rotation matrix with Eigen

1、源码

[html] view plain copy

print ?

1. #include <iostream>  
2. #include <Eigen/Eigen>  
3. #include <stdlib.h>  
4. #include <Eigen/Geometry>  
5. #include <Eigen/Core>  
6. #include <vector>  
7. #include <math.h>  
8.   
9. using namespace std;  
10. using namespace Eigen;  
11.   
12. Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw)  
13. {  
14.     Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());  
15.     Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());  
16.     Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());  
17.   
18.     Eigen::Quaterniond q = rollAngle  yawAngle  pitchAngle;  
19.     cout << “Euler2Quaternion result is:” <<endl;  
20.     cout << ”x = ” << q.x() <<endl;  
21.     cout << ”y = ” << q.y() <<endl;  
22.     cout << ”z = ” << q.z() <<endl;  
23.     cout << ”w = ” << q.w() <<endl<<endl;  
24.     return q;  
25. }  
26.   
27. Eigen::Vector3d Quaterniond2Euler(const double x,const double y,const double z,const double w)  
28. {  
29.     Eigen::Quaterniond q;  
30.     q.x() = x;  
31.     q.y() = y;  
32.     q.z() = z;  
33.     q.w() = w;  
34.   
35.     Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(2, 1, 0);  
36.     cout << “Quaterniond2Euler result is:” <<endl;  
37.     cout << ”x = ”<< euler[2] << endl ;  
38.     cout << ”y = ”<< euler[1] << endl ;  
39.     cout << ”z = ”<< euler[0] << endl << endl;  
40. }  
41.   
42. Eigen::Matrix3d Quaternion2RotationMatrix(const double x,const double y,const double z,const double w)  
43. {  
44.     Eigen::Quaterniond q;  
45.     q.x() = x;  
46.     q.y() = y;  
47.     q.z() = z;  
48.     q.w() = w;  
49.   
50.     Eigen::Matrix3d R = q.normalized().toRotationMatrix();  
51.     cout << “Quaternion2RotationMatrix result is:” <<endl;  
52.     cout << ”R = ” << endl << R << endl<< endl;  
53.     return R;  
54. }  
55.   
56.   
57. Eigen::Quaterniond rotationMatrix2Quaterniond(Eigen::Matrix3d R)  
58. {  
59.     Eigen::Quaterniond q = Eigen::Quaterniond(R);  
60.     q.normalize();  
61.     cout << “RotationMatrix2Quaterniond result is:” <<endl;  
62.     cout << ”x = ” << q.x() <<endl;  
63.     cout << ”y = ” << q.y() <<endl;  
64.     cout << ”z = ” << q.z() <<endl;  
65.     cout << ”w = ” << q.w() <<endl<<endl;  
66.     return q;  
67. }  
68.   
69. Eigen::Matrix3d euler2RotationMatrix(const double roll, const double pitch, const double yaw)  
70. {  
71.     Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());  
72.     Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());  
73.     Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());  
74.   
75.     Eigen::Quaterniond q = rollAngle  yawAngle  pitchAngle;  
76.     Eigen::Matrix3d R = q.matrix();  
77.     cout << “Euler2RotationMatrix result is:” <<endl;  
78.     cout << ”R = ” << endl << R << endl<<endl;  
79.     return R;  
80. }  
81.   
82. Eigen::Vector3d RotationMatrix2euler(Eigen::Matrix3d R)  
83. {  
84.     Eigen::Matrix3d m;  
85.     m = R;  
86.     Eigen::Vector3d euler = m.eulerAngles(0, 1, 2);  
87.     cout << “RotationMatrix2euler result is:” << endl;  
88.     cout << ”x = ”<< euler[2] << endl ;  
89.     cout << ”y = ”<< euler[1] << endl ;  
90.     cout << ”z = ”<< euler[0] << endl << endl;  
91.     return euler;  
92. }  
93.   
94.   
95. int main(int argc, char **argv)  
96. {  
97.   
98. //this is euler2Quaternion transform function,please input your euler angle//  
99.   euler2Quaternion(0,0,0);  
100.   
101. //this is Quaternion2Euler transform function,please input your euler angle//  
102.   Quaterniond2Euler(0,0,0,1);  
103.   
104. //this is Quaternion2RotationMatrix transform function,please input your Quaternion parameter//  
105.   Quaternion2RotationMatrix(0,0,0,1);  
106.   
107. //this is rotationMatrix2Euler transform function,please input your RotationMatrix parameter like following//  
108.   Eigen::Vector3d x_axiz,y_axiz,z_axiz;  
109.   x_axiz << 1,0,0;  
110.   y_axiz << 0,1,0;  
111.   z_axiz << 0,0,1;  
112.   Eigen::Matrix3d R;  
113.   R << x_axiz,y_axiz,z_axiz;  
114.   rotationMatrix2Quaterniond(R);  
115.   
116. //this is euler2RotationMatrix transform function,please input your euler angle for the function parameter//  
117.   euler2RotationMatrix(0,0,0);  
118.   
119. //this is RotationMatrix2euler transform function,please input your euler angle for the function parameter//  
120.   RotationMatrix2euler(R);  
121.   
122.   cout << “All transform is done!” << endl;  
123. }  

2、测试结果