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

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

阅读量：2383 次

发布时间：2019-05-10

本文共 4713 字，大约阅读时间需要 15 分钟。

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

以下是具体的实现代码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;

}

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)

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

旋转矩阵（R），旋转向量（V）和四元数（Q）分别通过自身初始化自己的方式，也就是第一分部分代码对旋转矩阵（R），旋转向量（V）和四元数（Q）赋值的第一种方式。

R通过自身初始化的方法：