SLAM开荒笔记之Eigen库

Eigen库简介

SLAM中，经常会需要计算位姿、进行KF系列的滤波操作等，这就要求高效地执行矩阵运算， Eigen 库就是 C++ 中一个很好用的矩阵运算库，矩阵乘法、位姿变换矩阵的各种形式转换、转置、LU分解、SVD分解等都可以通过该库高效的实现。
下面是本人对该库的学习记录。

如何在C++中调用该库

Eigen库本身只依赖C++标准库，所以不需要另外安装其他依赖。Eigen库在Ubuntu下的安装也比较简单，输入命令行：

sudo apt-get install libeigen3-dev

即可完成安装，安装后如果想查询安装位置，可通过：locate eigen3查询。
在.cpp代码中，要调用该库只需要包含相应的头文件即可，一个比较简单的做法是

#include<Eigen/Eigen>  /* 包括Eigen库的所有模块 */

由于Eigen库没有.so或.a文件，所以不需要进行库链接（即target_link_libraries())，在相应的 CMakeLists.txt 文件中，仅仅需要添加该库的路径，有两种等价的写法
写法1：include_directories( "/usr/include/eigen3" )
写法2：

find_package( Eigen3 REQUIRED )
include_directories( ${Eigen3_INCLUDE_DIRS} )

位姿变换中常用数据类型声明

以绕$z$轴旋转$pi/4$为例。

• 初始化一个四元数Eigen::Quaterniond q(w, a, b, c);
  作用：声明一个四元数$q=w+ai+bj+ck$
  代码实现：

Eigen::Quaterniond q(0, 0, 0.383, 0.924);//初始化把w放前面
cout << "四元数输出方式1： q = \n" << q.coeffs() << endl; /* coeffs的顺序为（a,b,c,w), w为实部 */
cout << "四元数输出方式2：\nw = " << q.w() <<  "\nx = " << q.x()
     << "\ny = " << q.y() << "\nz = " << q.z() << endl;

代码运行结果：

四元数输出方式1： q = 
    0
0.383
0.924
    0
四元数输出方式2：
w = 0
x = 0
y = 0.383
z = 0.924

• 初始化一个旋转矩阵

Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
rotation_matrix << 0.707, -0.707, 0,
                   0.707,  0.707, 0,
		               0,      0, 1;
cout <<  "rotation matrix = \n" <<  rotation_matrix << endl;

代码运行结果：

rotation matrix = 
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1

• 初始化一个旋转向量（轴角表示法）

Eigen::AngleAxisd rotation_vector(M_PI/4, Eigen::Vector3d(0, 0, 1));
cout << "rotation_vector axis = \n" << rotation_vector.axis() 
     << "\nrotation_vector angle = " << rotation_vector.angle() << endl;

代码运行结果：

rotation_vector axis = 
0
0
1
rotation_vector angle = 0.785398

• 初始化一个欧拉角

Eigen::Vector3d euler_angles = Eigen::Vector3d(M_PI/4, 0, 0); /* zyx顺序，即roll pitch yaw 顺序 */
cout << "欧拉角（zyx） = " << euler_angles.transpose() <<endl;

代码运行结果：

欧拉角（zyx） = 0.785398        0        0

• 初始化一个平移向量

Eigen::Vector3d t(1, 2, 3);
cout << "平移向量 = " << t.transpose() <<endl;

代码运行结果

平移向量 = 1 2 3

• 初始化一个齐次欧式变换矩阵

Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(rotation_matrix);
T.pretranslate(t);
cout << T.matrix()<<endl;

代码运行结果：

 0.707 -0.707      0      1
 0.707  0.707      0      2
     0      0      1      3
     0      0      0      1

• 注意事项
  所有和角度相关的量，输入和输出都是使用的弧度制，$\pi$的宏定义为M_PI。

常用函数

• 旋转向量、旋转矩阵、欧拉角转四元数

Eigen::Quaterniond q1=Eigen::Quaterniond(rotation_vector);  /* 旋转向量能直接初始化四元数 */
Eigen::Quaterniond q2=Eigen::Quaterniond(rotation_matrix);  /* 旋转矩阵能直接初始化四元数 */
//Eigen::Quaterniond q3=Eigen::Quaterniond(euler_angles);   /* 欧拉角不能直接初始化四元数 */
/* 要想用欧拉角初始化四元数， 可以先将欧拉角转化为轴角表示，进而转化为四元数 */
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(euler_angles(0), Eigen::Vector3d::UnitZ()));
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(euler_angles(1), Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(euler_angles(2), Eigen::Vector3d::UnitX()));
 
Eigen::Quaterniond q3 = yawAngle*pitchAngle*rollAngle;

cout << "q = "  <<  q.coeffs().transpose() << "\n"
     << "q1 = " << q1.coeffs().transpose() << "\n"
     << "q2 = " << q2.coeffs().transpose() << "\n"
	 << "q3 = " << q3.coeffs().transpose() << endl;

代码运行结果

q =     0 0.383 0.924     0
q1 =        0        0 0.382683  0.92388
q2 =        0        0 0.382638 0.923851
q3 =        0        0 0.382683  0.92388

• 四元数、旋转向量、欧拉角转旋转矩阵

Eigen::Matrix3d rotation_matrix1 = q.matrix();   /* 四元数能通过matrix方法直接初始化旋转矩阵 */
Eigen::Matrix3d rotation_matrix2 = rotation_vector.matrix();   /* 旋转向量能通过matrix方法直接初始化旋转矩阵 */
//Eigen::Matrix3d rotation_matrix2 = rotation_vector.toRotationMatrix(); /* 两种方法可选 */
/* 要想将欧拉角转换为旋转矩阵， 可以先将欧拉角转化为轴角表示，进而转化为旋转矩阵 */
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(euler_angles(0), Eigen::Vector3d::UnitZ()));
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(euler_angles(1), Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(euler_angles(2), Eigen::Vector3d::UnitX()));
/* Eigen库不支持隐式将轴角赋值给旋转矩阵，只能显式赋值 */
Eigen::Matrix3d rotation_matrix3;  /* 轴角表示不能直接初始化旋转矩阵，如果不用matrix方法，就只能通过赋值运算转换 */
rotation_matrix3 = yawAngle*pitchAngle*rollAngle;
    
cout << "rotation matrix = \n"  <<  rotation_matrix << "\n"
     << "rotation matrix1 = \n" << rotation_matrix1 << "\n"
     << "rotation matrix2 = \n" << rotation_matrix2 << "\n"
	 << "rotation matrix3 = \n" << rotation_matrix3 << endl;

代码运行结果

rotation matrix = 
 0.707 -0.707      0
 0.707  0.707      0
     0      0      1
rotation matrix1 = 
 -1.00093         0         0
        0 -0.707552  0.707784
        0  0.707784  0.706622
rotation matrix2 = 
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
rotation matrix3 = 
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1

• 四元数、旋转矩阵、欧拉角转旋转向量

Eigen::AngleAxisd rotation_vector1(q); 
Eigen::AngleAxisd rotation_vector2(rotation_matrix);
    
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(euler_angles(0), Eigen::Vector3d::UnitZ()));
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(euler_angles(1), Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(euler_angles(2), Eigen::Vector3d::UnitX()));
  
Eigen::AngleAxisd rotation_vector3;
rotation_vector3 = yawAngle*pitchAngle*rollAngle;

cout << "rotation_vector axis = "  <<  rotation_vector.axis().transpose() << " rotation_vector angle = " << rotation_vector.angle() << "\n"
     << "rotation_vector1 axis = "  <<  rotation_vector1.axis().transpose() << " rotation_vector1 angle = " << rotation_vector1.angle() << "\n"
     << "rotation_vector2 axis = "  <<  rotation_vector2.axis().transpose() << " rotation_vector2 angle = " << rotation_vector2.angle() << "\n"
	 << "rotation_vector3 axis = "  <<  rotation_vector3.axis().transpose() << " 

代码运行效果

rotation_vector axis = 0 0 1 rotation_vector angle = 0.785398
rotation_vector1 axis =        0 0.382911 0.923785 rotation_vector1 angle = 3.14159
rotation_vector2 axis = 0 0 1 rotation_vector2 angle = 0.785336
rotation_vector3 axis = 0 0 1 rotation_vector3 angle = 0.785398

• 四元数、旋转矩阵、旋转向量转欧拉角

Eigen::Vector3d euler_angles1 = q.matrix().eulerAngles(2,1,0); /* 2-z轴， 1-y轴， 0-x轴 8/
Eigen::Vector3d euler_angles2 = rotation_vector.matrix().eulerAngles(2,1,0);
Eigen::Vector3d euler_angles3 = rotation_matrix.eulerAngles(2,1,0);
	
cout << "欧拉角（zyx）euler_angles = " << euler_angles.transpose() << "\n"
     << "欧拉角（zyx）euler_angles1 = " << euler_angles.transpose() << "\n"
	 << "欧拉角（zyx）euler_angles2 = " << euler_angles.transpose() << "\n"
	 << "欧拉角（zyx）euler_angles3 = " << euler_angles.transpose() << endl;

代码运行结果

欧拉角（zyx）euler_angles = 0.785398        0        0
欧拉角（zyx）euler_angles1 = 0.785398        0        0
欧拉角（zyx）euler_angles2 = 0.785398        0        0
欧拉角（zyx）euler_angles3 = 0.785398        0        0

• 其他常用矩阵操作函数

M.transpose();  /* 矩阵M的转置 */
M.inverse();  /* 矩阵M的逆 */
M.determinant();  /* 矩阵M的行列式 */
M.sum(); /* 矩阵M各元素和 */
12*M; /* 矩阵数乘 */

/* 求矩阵M（要求实对称矩阵）特征值和特征向量 */
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( M );
    cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;
Eigen::Matrix3d::Identity();  /* 单位三维矩阵 */
Eigen::Matrix3d::Random();  /* 三维随机数矩阵 */

头文件选择

Eigen的主要模块有基本模块Core，其他功能模块Geometry、LU、Clolesky、Householder、SVD、QR、Eigenvalues、Sparse，两个整合模块Dense、Eigen。一般情况下，#include <Eigen/Dense>就足够了。

/* Eigen 头文件选择 */
#include <Eigen/Eigen>  /* 包含了Eigen库的所有模块 */
#include <Eigen/Dense>  /* 稠密矩阵的代数运算（逆，特征值等），包含了Core/Geometry/LU/Cholesky/SVD/QR/Eigenvalues模块 */
#include <Eigen/Sparese>  /* 稀疏矩阵相关的内容，如存储和基本线性运算 */
#include <Eigen/Core>  /* 基本的Matrix和Array类，基本的线性代数运算和数组操作 */
#include <Eigen/Geometry>  /* SLAM中常用的位姿变换(包括四元数、轴角等) */
#include <Eigen/LU>  /* 矩阵求逆，求行列式， 矩阵LU分解 */
#include <Eigen/Cholesky>  /* LLT和LDLT Cholesky分解 */
#include  <Eigen/Householder>  /* Householder变换，用于线性代数运算 */
#include <Eigen/SVD>  /* SVD分解 */
#include <Eigen/QR>  /* QR分解 */
#include <Eigen/Eigenvalues>  /* 求方阵特征值、特征向量 */

参考文献

Eigen中四元数、欧拉角、旋转矩阵、旋转向量之间的转换
Eigen教程1----Eigen的介绍