Eigen库函数实现欧拉角与旋转矩阵转换

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
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
int main()
{
/*+++++++++++++++++++++++++++
一、向量矩阵基本操作
++++++++++++++++**/
//1、向量
Eigen::Vector3d v3d;
//向量
Eigen::VectorXd v(6);
//需要指定大小(6)
v3d << 1,2,3;
cout << "向量 v3d = "<< endl << v3d << endl << endl;
v << 1,2,3,4,5,6;
cout << "向量 v = "<< endl << v << endl << endl;
//2、矩阵基本操作
Eigen::Matrix<float,2,3> m_f23;
//行、列
Eigen::MatrixXd m_d24(2,4);
Eigen::Matrix3d m_d33;
m_f23 << 1,2,3,
4,5,6;
cout << "矩阵 m_f23 = "<< endl << m_f23 << endl << endl;
m_d24 << 1,2,3,4,
5,6,7,8;
cout << "矩阵 m_d24 = "<< endl << m_d24 << endl << endl;
m_d33 << 1,2,3,
4,5,6,
7,8,9;
cout << "矩阵 m_d33 = "<< endl << m_d33 << endl << endl;
//3、矩阵利用函数进行赋值
Eigen::Matrix4d m_d44 = Eigen::Matrix4d::Zero();
cout << "Zero 矩阵 m_d44 = " << endl << m_d44 << endl << endl;
m_d44 = Eigen::Matrix4d::Random();
cout << "矩阵 m_d44 = "<< endl << m_d44 << endl << endl;
Eigen::MatrixXd m_dnn(5,5);
m_dnn = Eigen::MatrixXd::Random(5, 5);
cout << "Random 矩阵 m_dnn = "<< endl << m_dnn << endl << endl;
//4、获取指定元素的值
for(int i = 0 ;i < 5; i++){
for(int j = 0; j < 5; j++){
cout << m_dnn(i,j) << "," ;
}
cout << endl << endl;
}
//5、获取矩阵的子矩阵（block）
Eigen::MatrixXf m_block(5,5);
m_block <<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;
Eigen::MatrixXf m_sun1 = m_block.block<2,2>(1,1);//从1行1列开始，获取2*2的子矩阵矩阵
cout << "block 矩阵 m_block.block<2,2>(1,1) = "<< endl << m_sun1 << endl << endl;
Eigen::MatrixXf m_sun2 = m_block.block(1,1,2,2);//同上
cout << "block 矩阵 m_block.block(1,1,2,2) = "<< endl << m_sun2 << endl << endl;
cout << "矩阵 m_block m_block.row(0) = "<< endl << m_block.row(0) << endl << endl;
cout << "矩阵 m_block m_block.col(0) = "<< endl << m_block.col(0) << endl << endl;
//6、矩阵组合操作
Eigen::Matrix3f R;
R << 1,2,3,4,5,6,7,8,9;
Eigen::Matrix<float,3,1> T;
T << 11,11,11;
Eigen::Matrix<float,1,4> B;
B << 22,22,22,22;
Eigen::Matrix<float,4,4> H;
H << R,T,B;
cout << "其次矩阵 H = "<< endl << H << endl << endl;
Eigen::Matrix<float,2,4> newH;
newH << H.block(0,0,1,4),B;
cout << "其次矩阵 newH = "<< endl << newH << endl << endl;
/*------------------
end
-----------------**/
/*+++++++++++++++++++++++++++
二、矩阵进行运算
++++++++++++++++**/
// 1、矩阵相乘
Eigen::Matrix3d m1,m2;
m1 << 1,0,0,
0,1,0,
0,0,2;
m2 << 1,2,3,
4,5,6,
7,8,9;
cout << "m1 * m2 = "<< endl << m1 * m2 << endl << endl;
//2、矩阵转置
cout << "转置
m2.transpose() = "<< endl << m2.transpose() << endl << endl;
//3、矩阵求和
cout << "求和
m2.sum() = "<< endl << m2.sum() << endl << endl;
//4、矩阵求逆
cout << "求逆
m2.inverse() = "<< endl << m2.inverse() << endl << endl;
//5、矩阵迹
cout << "迹
m2.trace() = "<< endl << m2.trace() << endl << endl;
//6、矩阵数乘
cout << "数乘
10*m2 = "<< endl << 10 * m2 << endl << endl;
//7、矩阵行列式
cout << "行列式
m2.determinant() = "<< endl << m2.determinant() << endl << endl;
return 0;
}

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
//这里theta也是弧度制
static Eigen::Matrix3d eul2rotm(Eigen::Vector3d &theta,string m_sSeq)
{
Eigen::Matrix3d rotX;
// 计算旋转矩阵的X分量
rotX <<
1,
0,
0,
0,
cos(theta[0]),
-sin(theta[0]),
0,
sin(theta[0]),
cos(theta[0]);
Eigen::Matrix3d rotY;
// 计算旋转矩阵的Y分量
rotY <<
cos(theta[1]),
0, sin(theta[1]),
0,
1,
0,
-sin(theta[1]),
0, cos(theta[1]);
Eigen::Matrix3d rotZ;
// 计算旋转矩阵的Z分量
rotZ <<
cos(theta[2]), -sin(theta[2]), 0,
sin(theta[2]),
cos(theta[2]), 0,
0,
0,
1;
Eigen::Matrix3d R;
if (m_sSeq == "zyx")
{
R = rotX * rotY * rotZ;
}
else if (m_sSeq == "yzx")
{
R = rotX * rotZ * rotY;
}
else if (m_sSeq == "zxy")
{
R = rotY * rotX * rotZ;
}
else if (m_sSeq == "xzy")
{
R = rotY * rotZ * rotX;
}
else if (m_sSeq == "yxz")
{
R = rotZ * rotX * rotY;
}
else if (m_sSeq == "xyz")
{
R = rotZ * rotY * rotX;
}
return R;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
static Eigen::Vector3d rotm2eul(Eigen::Matrix3d &R)
{
double sy = sqrt(R(0,0) * R(0,0) + R(1,0) * R(1,0));
bool singular = sy < 1e-6;
double x, y, z;
if (!singular)
{
x = atan2( R(2,1), R(2,2));
y = atan2(-R(2,0), sy);
z = atan2( R(1,0), R(0,0));
}
else
{
x = atan2(-R(1,2), R(1,1));
y = atan2(-R(2,0), sy);
z = 0;
}
return {x, y, z};
}