Eigen库使用教程之旋转矩阵，旋转向量和四元数的初始化和相互转换的实现

Eigen: C++开源矩阵计算工具——Eigen的简单用法本系列文章为原创，转载请注明出处。
作者：Tom Bai
邮箱： [e-mail]

若您觉得本博文对您有帮助，请您为我点赞并关注我，以鼓励我写出更优秀的博文。谢谢！

---

Eigen库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。Eigen是一个用纯头文件搭建起来的库，这意味这你只要能找到它的头文件，就能使用它。Eigen头文件的默认位置是“/usr/include/eigen3”.

由于Eigen库相较于OpenCV中的Mat等库而言更加高效，许多上层的软件库也使用Eigen进行矩阵运算，比如SLAM中常用的g2o,Sophus等。此外Eigen库还被被用于Caffe，Tensorflow等许多深度学习相关的框架中。

1. 刚体运动中的旋转通常可以由旋转矩阵，旋转向量和四元数等多种方式表示（具体的转换公式请参见这篇博客），在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

1. 旋转矩阵（R），旋转向量（V）和四元数（Q）在Eigen中转换关系的总结：

1. 旋转矩阵（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的简单用法

---

本系列文章为原创，转载请注明出处。
作者：Tom Bai
邮箱： [e-mail]

若您觉得本博文对您有帮助，请您为我点赞并关注我，以鼓励我写出更优秀的博文。谢谢！