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