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

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

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

Eigen: C++开源矩阵计算工具——Eigen的简单用法

---

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

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

上述代码对应的CMakeLists.txt为：

CMAKE_MINIMUM_REQUIRED(VERSION 2.8)project(useGeometry)include_directories("/usr/include/eigen3")add_executable(eigen_geometry eigen_geometry.cpp)

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

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

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

//1.使用旋转矩阵的函数来初始化旋转矩阵Matrix3d R1=Matrix3d::Identity();cout << "Rotation_matrix1" << endl << R1 << endl;

• V通过自身初始化的方法：

//1.使用旋转的角度和旋转轴向量（此向量为单位向量）来初始化角轴AngleAxisd V1(M_PI / 4, Vector3d(0, 0, 1));//以（0,0,1）为旋转轴，旋转45度cout << "Rotation_vector1" << endl << V1.matrix() << endl;

• 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;

---

相关博客：
* C++矩阵库 Eigen 快速入门
* Eigen: C++开源矩阵计算工具——Eigen的简单用法

---

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

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