https://blog.csdn.net/u011092188/article/details/77430988
Eigen: C++開源矩陣計算工具——Eigen的簡單用法
Eigen庫是一個開源的C++線性代數庫，它提供了快速的有關矩陣的線性代數運算，還包括解方程等功能。Eigen是一個用純頭文件搭建起來的庫，這意味這你只要能找到它的頭文件，就能使用它。Eigen頭文件的默認位置是“/usr/include/eigen3”.
由於Eigen庫相較於OpenCV中的Mat等庫而言更加高效，許多上層的軟件庫也使用Eigen進行矩陣運算，比如SLAM中常用的g2o,Sophus等。此外Eigen庫還被被用於Caffe，Tensorflow等許多深度學習相關的框架中。
剛體運動中的旋轉通常可以由旋轉矩陣，旋轉向量和四元數等多種方式表示（具體的轉換公式請參見這篇博客），在Eigen庫中也有其對應的實現。本文主要介紹剛體運動時旋轉矩陣，旋轉向量和四元數的初始化以及相互轉換在Eigen中的實現方式。
Eigen庫中各種形式的表示如下：
旋轉矩陣（3X3）:Eigen::Matrix3d
旋轉向量（3X1）:Eigen::AngleAxisd
四元數（4X1）:Eigen::Quaterniond
平移向量（3X1）:Eigen::Vector3d
變換矩陣（4X4）:Eigen::Isometry3d
1
2
3
4
5
以下是具體的實現代碼eigen_geometry.cpp：
#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv) {
//下面三個變量作爲下面演示的中間變量
AngleAxisd t_V(M_PI / 4, Vector3d(0, 0, 1));
Matrix3d t_R = t_V.matrix();
Quaterniond t_Q(t_V);
//對旋轉向量（軸角）賦值的三大種方法
//1.使用旋轉的角度和旋轉軸向量（此向量爲單位向量）來初始化角軸
AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）爲旋轉軸，旋轉45度
cout << "Rotation_vector1" << endl << V1.matrix() << endl;
//2.使用旋轉矩陣轉旋轉向量的方式
//2.1 使用旋轉向量的fromRotationMatrix()函數來對旋轉向量賦值（注意此方法爲旋轉向量獨有,四元數沒有）
AngleAxisd V2;
V2.fromRotationMatrix(t_R);
cout << "Rotation_vector2" << endl << V2.matrix() << endl;
//2.2 直接使用旋轉矩陣來對旋轉向量賦值
AngleAxisd V3;
V3 = t_R;
cout << "Rotation_vector3" << endl << V3.matrix() << endl;
//2.3 使用旋轉矩陣來對旋轉向量進行初始化
AngleAxisd V4(t_R);
cout << "Rotation_vector4" << endl << V4.matrix() << endl;
//3. 使用四元數來對旋轉向量進行賦值
//3.1 直接使用四元數來對旋轉向量賦值
AngleAxisd V5;
V5 = t_Q;
cout << "Rotation_vector5" << endl << V5.matrix() << endl;
//3.2 使用四元數來對旋轉向量進行初始化
AngleAxisd V6(t_Q);
cout << "Rotation_vector6" << endl << V6.matrix() << endl;
//------------------------------------------------------
//對四元數賦值的三大種方法（注意Eigen庫中的四元數前三維是虛部,最後一維是實部）
//1.使用旋轉的角度和旋轉軸向量（此向量爲單位向量）來初始化四元數,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
Quaterniond Q1(cos((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 1 * sin((M_PI / 4) / 2));//以（0,0,1）爲旋轉軸，旋轉45度
//第一種輸出四元數的方式
cout << "Quaternion1" << endl << Q1.coeffs() << endl;
//第二種輸出四元數的方式
cout << Q1.x() << endl << endl;
cout << Q1.y() << endl << endl;
cout << Q1.z() << endl << endl;
cout << Q1.w() << endl << endl;
//2. 使用旋轉矩陣轉四元數的方式
//2.1 直接使用旋轉矩陣來對旋轉向量賦值
Quaterniond Q2;
Q2 = t_R;
cout << "Quaternion2" << endl << Q2.coeffs() << endl;
//2.2 使用旋轉矩陣來對四元數進行初始化
Quaterniond Q3(t_R);
cout << "Quaternion3" << endl << Q3.coeffs() << endl;
//3. 使用旋轉向量對四元數來進行賦值
//3.1 直接使用旋轉向量對四元數來賦值
Quaterniond Q4;
Q4 = t_V;
cout << "Quaternion4" << endl << Q4.coeffs() << endl;
//3.2 使用旋轉向量來對四元數進行初始化
Quaterniond Q5(t_V);
cout << "Quaternion5" << endl << Q5.coeffs() << endl;
//----------------------------------------------------
//對旋轉矩陣賦值的三大種方法
//1.使用旋轉矩陣的函數來初始化旋轉矩陣
Matrix3d R1=Matrix3d::Identity();
cout << "Rotation_matrix1" << endl << R1 << endl;
//2. 使用旋轉向量轉旋轉矩陣來對旋轉矩陣賦值
//2.1 使用旋轉向量的成員函數matrix()來對旋轉矩陣賦值
Matrix3d R2;
R2 = t_V.matrix();
cout << "Rotation_matrix2" << endl << R2 << endl;
//2.2 使用旋轉向量的成員函數toRotationMatrix()來對旋轉矩陣賦值
Matrix3d R3;
R3 = t_V.toRotationMatrix();
cout << "Rotation_matrix3" << endl << R3 << endl;
//3. 使用四元數轉旋轉矩陣來對旋轉矩陣賦值
//3.1 使用四元數的成員函數matrix()來對旋轉矩陣賦值
Matrix3d R4;
R4 = t_Q.matrix();
cout << "Rotation_matrix4" << endl << R4 << endl;
//3.2 使用四元數的成員函數toRotationMatrix()來對旋轉矩陣賦值
Matrix3d R5;
R5 = t_Q.toRotationMatrix();
cout << "Rotation_matrix5" << endl << R5 << 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
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
124
125
126
127
128
129
上述代碼對應的CMakeLists.txt爲：
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
project(useGeometry)
include_directories("/usr/include/eigen3")
add_executable(eigen_geometry eigen_geometry.cpp)
1
2
3
4
旋轉矩陣（R），旋轉向量（V）和四元數（Q）在Eigen中轉換關係的總結：
旋轉矩陣（R），旋轉向量（V）和四元數（Q）分別通過自身初始化自己的方式，也就是第一分部分代碼對旋轉矩陣（R），旋轉向量（V）和四元數（Q）賦值的第一種方式。
R通過自身初始化的方法：
//1.使用旋轉矩陣的函數來初始化旋轉矩陣
Matrix3d R1=Matrix3d::Identity();
cout << "Rotation_matrix1" << endl << R1 << endl;
1
2
3
4
V通過自身初始化的方法：
//1.使用旋轉的角度和旋轉軸向量（此向量爲單位向量）來初始化角軸
AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）爲旋轉軸，旋轉45度
cout << "Rotation_vector1" << endl << V1.matrix() << endl;
1
2
3
4
Q通過自身初始化的方法：
//1.使用旋轉的角度和旋轉軸向量（此向量爲單位向量）來初始化四元數,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
Quaterniond Q1(cos((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 0 * sin((M_PI / 4) / 2), 1 * sin((M_PI / 4) / 2));//以（0,0,1）爲旋轉軸，旋轉45度
cout << "Quaternion1" << endl << Q1.coeffs() << endl;
1
2
3
4
5
6
相關博客：
* C++矩陣庫 Eigen 快速入門
* Eigen: C++開源矩陣計算工具——Eigen的簡單用法