SLAM算法与工程实践系列文章

下面是SLAM算法与工程实践系列文章的总链接，本人发表这个系列的文章链接均收录于此

SLAM算法与工程实践系列文章链接

---

下面是专栏地址：

SLAM算法与工程实践系列专栏

---

文章目录

• SLAM算法与工程实践系列文章
• • SLAM算法与工程实践系列文章链接
  • SLAM算法与工程实践系列专栏
• 前言
• SLAM算法与工程实践——SLAM基本库的安装与使用（1）：Eigen库
• Eigen
• • 安装
  • 查询版本
  • 基本使用
  • • eigenMatrix.cpp
    • • 包含头文件
      • Eigen::Matrix
      • 初始化
      • 访问矩阵元素
      • 矩阵运算
      • 特征值
      • 解方程
      • 矩阵分解
    • eigenGeometry.cpp
    • • 旋转向量
      • 欧拉角
      • 四元数
    • visualizeGeometry.cpp
    • 特征值计算
    • 出现的错误
  • 常见用法

---

前言

这个系列的文章是分享SLAM相关技术算法的学习和工程实践

---

SLAM算法与工程实践——SLAM基本库的安装与使用（1）：Eigen库

Eigen

Eigen库官网：https://eigen.tuxfamily.org/index.php?title=Main_Page

Eigen 3 官方文档：https://eigen.tuxfamily.org/dox/

安装

Eigen3是一个纯头文件的库，这个特点让使用者省去了很多安装和环境配置的麻烦

直接安装：

sudo apt-get install libeigen3-dev

或者下载源码解压缩安装包

git clone https://github.com/eigenteam/eigen-git-mirror.git
cd eigen-git-mirror
mkdir build
cd build
cmake ..
sudo make install

#安装后 头文件安装在/usr/local/include/eigen3/
#移动头文件
sudo cp -r /usr/local/include/eigen3/Eigen /usr/local/include

备注：在很多程序中 include 时经常使用 #include <Eigen/Dense> 而不是使用 #include <eigen3/Eigen/Dense> 所以要做下处理

查询版本

参考：

查看Eigen、CMake、ceres、opencv版本

找到eigen本地目录下的Macros.h头文件查看对应的版本。

执行如下命令：

sudo nano /usr/include/eigen3/Eigen/src/Core/util/Macros.h

可以看到Eigen的版本为3.3.7

基本使用

头文件

一般情况下，只需要：

#include <Eigen/Core>
#include <Eigen/Dense>

Eign中对各种形式的表达方式总结如下。请注意每种类型都有单精度和双精度两种数据类型，而且和之前一样，不能由编译器自动转换。下面以双精度为例，你可以把最后的 d 改成 f ，即得到单精度的数据结构。

• 旋转矩阵（3×3)：Eigen:Matrix.3d。
• 旋转向量(3×1)：Eigen:AngleAxisd。
• 欧拉角（3×1)：Eigen:Vector3d。
• 四元数(4×1)：Eigen:Quaterniond
• 欧氏变换矩阵(4×4)：Eigen:Isometry3d。
• 仿射变换(4×4)：Eigen:Affine3d。
• 射影变换(4×4)：Eigen:Projective.3d。

参考代码中对应的CMakeLists即可编译此程序。在下面的程序中，演示了如何使用Eigen中的旋转矩阵、旋转向量、欧拉角和四元数。我们用这几种旋转方式旋转一个向量v,发现结果是一样的。

同时，也演示了如何在程序中转换这几种表达方式。想进一步了解Eigen的几何模块的读者可以参考(http://eigen.tuxfamily.org/dox/group__TutorialGeometry.html)

注意：

程序代码通常和数学表示有一些细微的差别。例如，通过运算符重载，四元数和三维向量可以直接计算乘法，但在数学上则需要先把向量转成虚四元数，再利用四元数乘法进行计算，同样的情况也适用于变换矩阵乘三维向量的情况。总体而言，程序中的用法会比数学公式更灵活。

eigenMatrix.cpp

包含头文件

#include <ctime>	// 用来计时
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

一般多用到这两个头文件，Dense 里面其实已经包含了 Core 中的内容，只写 #include <Eigen/Dense> 即可

Eigen 中所有向量和矩阵都是 Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列

声明一个2*3的float矩阵

Matrix<float, 2, 3> matrix_23;

Eigen::Matrix

详细解读见官网文档：https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html

Eigen 通过 typedef 提供了许多内置类型，不过底层仍是 Eigen::Matrix

例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量

Vector3d v_3d;

这是一样的

Matrix<float, 3, 1> vd_3d;

将鼠标移动到 Vector3d 处，可以看到

typedef Eigen::Matrix<double, 3, 1> Eigen::Vector3d

Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>

Matrix3d matrix_33 = Matrix3d::Zero(); //初始化为零

如果不确定矩阵大小，可以使用动态大小的矩阵

Matrix<double, Dynamic, Dynamic> matrix_dynamic;

更简单的

MatrixXd matrix_x;

MatrixXd 表示动态大小的矩阵，其定义为

typedef Eigen::Matrix<double, -1, -1> Eigen::MatrixXd

这种类型还有很多，我们不一一列举

初始化

下面是对Eigen阵的操作

输入数据（初始化），这里默认按行输入

matrix_23 << 1, 2, 3, 4, 5, 6;

输出

cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;

访问矩阵元素

用()访问矩阵中的元素

cout << "print matrix 2x3: " << endl;
for (int i = 0; i < 2; i++) {
	for (int j = 0; j < 3; j++) cout << matrix_23(i, j) << "\t";
	cout << endl;
}

矩阵运算

矩阵和向量相乘（实际上仍是矩阵和矩阵）

v_3d << 3, 2, 1;
vd_3d << 4, 5, 6;

但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的

Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;

应该显式转换

Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << endl;

Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;

注意：这里输出时转置是为了节省空间，因为计算结果为列向量，将其转置后为行向量，显示比较方便

>

同样你不能搞错矩阵的维度
试着取消下面的注释，看看Eigen会报什么错

// Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;

一些矩阵运算
四则运算就不演示了，直接用±*/即可。

matrix_33 = Matrix3d::Random();      // 随机数矩阵
cout << "random matrix: \n" << matrix_33 << endl;
cout << "transpose: \n" << matrix_33.transpose() << endl;      // 转置
cout << "sum: " << matrix_33.sum() << endl;            // 各元素和
cout << "trace: " << matrix_33.trace() << endl;          // 迹
cout << "times 10: \n" << 10 * matrix_33 << endl;               // 数乘
cout << "inverse: \n" << matrix_33.inverse() << endl;        // 逆
cout << "det: " << matrix_33.determinant() << endl;    // 行列式

结果如下

特征值

实对称矩阵可以保证对角化成功

matrix_33 = Matrix3d::Random();      // 随机数矩阵
SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

这里 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> 的意义为：

class Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>
Computes eigenvalues and eigenvectors of selfadjoint matrices

模板参数:
_MatrixType – the type of the matrix of which we are computing the eigendecomposition; this is expected to be an instantiation of the Matrix class template. A matrix

结果如下;

解方程

我们求解 matrix_NN * x = v_Nd 这个方程，即求解 $Ax=b$

N的大小在前边的宏里定义，它由随机数生成

直接求逆自然是最直接的，但是求逆运算量大

#define MATRIX_SIZE 50

Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN
      = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
  matrix_NN = matrix_NN * matrix_NN.transpose();  // 保证半正定
  Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);

clock_t time_stt = clock(); // 计时
  // 直接求逆
Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
cout << "time of normal inverse is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
cout << "x = " << x.transpose() << endl;

结果为：

矩阵分解

矩阵分解详见：https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html

通常用矩阵分解来求，例如QR分解，速度会快很多

Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN
      = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
  matrix_NN = matrix_NN * matrix_NN.transpose();  // 保证半正定
  Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);

time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout << "time of Qr decomposition is "
     << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
cout << "x = " << x.transpose() << endl;

结果为：

在QR分解中

#include <iostream>
#include <Eigen/Dense>
 
int main()
{
   Eigen::Matrix3f A;
   Eigen::Vector3f b;
   A << 1,2,3,  4,5,6,  7,8,10;
   b << 3, 3, 4;
   std::cout << "Here is the matrix A:\n" << A << std::endl;
   std::cout << "Here is the vector b:\n" << b << std::endl;
   Eigen::Vector3f x = A.colPivHouseholderQr().solve(b);
   std::cout << "The solution is:\n" << x << std::endl;
}

输出结果：

Here is the matrix A:
 1  2  3
 4  5  6
 7  8 10
Here is the vector b:
3
3
4
The solution is:
-2
 1
 1

在本例中，colPivHouseholderQr() 方法返回类 ColPivHouse holderQR 的对象。由于这里的矩阵是 Matrix3f 类型的，所以这一行可能被替换为：

ColPivHouseholderQR<Matrix3f> dec(A);
Vector3f x = dec.solve(b);

对于正定矩阵，还可以用cholesky分解来解方程

time_stt = clock();
x = matrix_NN.ldlt().solve(v_Nd);
cout << "time of ldlt decomposition is "
     << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
cout << "x = " << x.transpose() << endl;

结果为：

此处，ColPivHouseholderQR是一个带有列主的QR分解。对于本教程来说，这是一个很好的折衷方案，因为它适用于所有矩阵，同时速度很快。

以下是一些其他分解的表格，您可以根据矩阵、您试图解决的问题以及您想要进行的权衡进行选择：

Decomposition	Method	Requirements on the matrix	Speed (small-to-medium)	Speed (large)	Accuracy
PartialPivLU	partialPivLu()	Invertible	++	++	+
FullPivLU	fullPivLu()	None	-	- -	+++
HouseholderQR	householderQr()	None	++	++	+
ColPivHouseholderQR	colPivHouseholderQr()	None	+	-	+++
FullPivHouseholderQR	fullPivHouseholderQr()	None	-	- -	+++
CompleteOrthogonalDecomposition	completeOrthogonalDecomposition()	None	+	-	+++
LLT	llt()	Positive definite	+++	+++	+
LDLT	ldlt()	Positive or negative semidefinite	+++	+	++
BDCSVD	bdcSvd()	None	-	-	+++
JacobiSVD	jacobiSvd()	None	-	- - -	+++

要获得不同分解的真实相对速度的概述，请查看此benchmark .

方阵是对称的，对于过约束矩阵，报告的时间包括计算对称协方差矩阵的成本 $A^{T}A$ 对于前四个基于 Cholesky 和 LU 的求解器，用*****符号表示（表的右上角部分）。计时以毫秒为单位，因素与LLT分解有关， LLT分解速度最快，但也是最不通用和鲁棒的。

solver/size	8x8	100x100	1000x1000	4000x4000	10000x8	10000x100	10000x1000	10000x4000
LLT	0.05	0.42	5.83	374.55	6.79 *	30.15 *	236.34 *	3847.17 *
LDLT	0.07 (x1.3)	0.65 (x1.5)	26.86 (x4.6)	2361.18 (x6.3)	6.81 (x1) *	31.91 (x1.1) *	252.61 (x1.1) *	5807.66 (x1.5) *
PartialPivLU	0.08 (x1.5)	0.69 (x1.6)	15.63 (x2.7)	709.32 (x1.9)	6.81 (x1) *	31.32 (x1) *	241.68 (x1) *	4270.48 (x1.1) *
FullPivLU	0.1 (x1.9)	4.48 (x10.6)	281.33 (x48.2)	-	6.83 (x1) *	32.67 (x1.1) *	498.25 (x2.1) *	-
HouseholderQR	0.19 (x3.5)	2.18 (x5.2)	23.42 (x4)	1337.52 (x3.6)	34.26 (x5)	129.01 (x4.3)	377.37 (x1.6)	4839.1 (x1.3)
ColPivHouseholderQR	0.23 (x4.3)	2.23 (x5.3)	103.34 (x17.7)	9987.16 (x26.7)	36.05 (x5.3)	163.18 (x5.4)	2354.08 (x10)	37860.5 (x9.8)
CompleteOrthogonalDecomposition	0.23 (x4.3)	2.22 (x5.2)	99.44 (x17.1)	10555.3 (x28.2)	35.75 (x5.3)	169.39 (x5.6)	2150.56 (x9.1)	36981.8 (x9.6)
FullPivHouseholderQR	0.23 (x4.3)	4.64 (x11)	289.1 (x49.6)	-	69.38 (x10.2)	446.73 (x14.8)	4852.12 (x20.5)	-
JacobiSVD	1.01 (x18.6)	71.43 (x168.4)	-	-	113.81 (x16.7)	1179.66 (x39.1)	-	-
BDCSVD	1.07 (x19.7)	21.83 (x51.5)	331.77 (x56.9)	18587.9 (x49.6)	110.53 (x16.3)	397.67 (x13.2)	2975 (x12.6)	48593.2 (x12.6)

*****: 此分解不支持对过度约束问题的直接最小二乘求解，并且报告的时间包括形成对称协方差矩阵的成本 $A^{T}A$.

eigenGeometry.cpp

旋转向量

Eigen/Geometry 模块提供了各种旋转和平移的表示

3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f

Matrix3d rotation_matrix = Matrix3d::Identity();

旋转向量使用 AngleAxis，详情见：https://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html

AngleAxisf for float
AngleAxisd for double

注意：

此类的目的不是用来存储旋转变换，而是为了更容易地创建其他旋转(Quaternion, rotation Matrix)和变换对象。

设置 AngleAxis 对象时，必须将其初始化为弧度制的角度和归一化的轴矢量。

如果轴向量未归一化，则角度轴对象表示无效旋转！

其构造为：

Eigen::AngleAxis< Scalar_ >::AngleAxis	(	const Scalar & 	angle,
const MatrixBase< Derived > & 	axis 
)	

由其衍生出的AngleAxisd 的定义如下

typedef Eigen::AngleAxis<double> Eigen::AngleAxisd

它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）

AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度
cout.precision(3);	// 设置输出精度
cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵

注意：这里使用M_PI时要包含头文件#include <cmath>

也可以直接赋值

rotation_matrix = rotation_vector.toRotationMatrix();

结果为：

用 AngleAxis 可以进行坐标变换

Vector3d v(1, 0, 0);
Vector3d v_rotated = rotation_vector * v;
cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;

或者用旋转矩阵

v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;

结果为：

欧拉角

可以将旋转矩阵直接转换成欧拉角

Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll顺序
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

结果为：

欧氏变换矩阵使用 Eigen::Isometry，其定义为

typedef Eigen::Transform<double, 3, 1> Eigen::Isometry3d

详见：https://eigen.tuxfamily.org/dox/classEigen_1_1Hyperplane.html#afb4d86eb3d2bb8311681067df71499de

使用

Isometry3d T = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
T.rotate(rotation_vector);                                     // 按照rotation_vector进行旋转
T.pretranslate(Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)
cout << "Transform matrix = \n" << T.matrix() << endl;

结果为：

用变换矩阵进行坐标变换

Vector3d v_transformed = T * v;                              // 相当于R*v+t
cout << "v tranformed = " << v_transformed.transpose() << endl;

注意：这里已经对乘号做了重载，所以T为四维，乘以3维的v，可以得到答案

结果为：

对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

四元数

详见：

https://eigen.tuxfamily.org/dox/classEigen_1_1Quaternion.html

https://eigen.tuxfamily.org/dox/classEigen_1_1QuaternionBase.html

可以直接把AngleAxis赋值给四元数，反之亦然

Quaterniond q = Quaterniond(rotation_vector);
cout << "quaternion from rotation vector = " << q.coeffs().transpose()
       << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部

也可以把旋转矩阵赋给它

q = Quaterniond(rotation_matrix);
cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;

结果为：

这里的coeffs()的返回类型为Vector4d 即4维向量

inline Eigen::Vector4d &Eigen::Quaterniond::coeffs()

使用四元数旋转一个向量，使用重载的乘法即可

v_rotated = q * v; // 注意数学上是qvq^{-1}，这里做了符号重载而已
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;

结果为：

用常规向量乘法表示，则应该如下计算

cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

结果为：

注意：

Eigen库中的四元素存储排列为：前三位为虚部，第四维为实部。

但是初始化时，仍然为第一维为实部，后面三维为虚部，即

四元数Eigen::Quaterniond 的正确初始化顺序为Eigen::Quaterniond(w,x,y,z)

而 coeffs的顺序是（x,y,z,w）,w 为实部，前三者为虚部

Warning

Note the order of the arguments: the real w coefficient first, while internally the coefficients are stored in the following order: [x, y, z, w]

书本上的定义为：第一维为实部，后面三维为虚部

visualizeGeometry.cpp

特征值计算

参考：

Eigen矩阵运算库快速上手

Eigen::SelfAdjointEigenSolver

Eigen::SelfAdjointEigenSolver类计算自伴随矩阵的特征值和特征向量，头文件是#include <Eigen/Eigenvalues>。对于标量 $\lambda$ 和向量 $v$ ，使得 $Av=\lambda V$。SelfAdjointEigenSolver类功能就是计算自伴随矩阵的特征值和特征向量。

自伴随矩阵主对角线上的元素都是实数的，其特征值也是实数。如果 $D$ 是特征值在对角线上的对角矩阵， $V$ 是以特征向量为列的矩阵，则 $a=VD{V}^{-1}$ （对于自伴矩阵，矩阵 $V$ 总是可逆的），这称为特征分解。

特征值及对应的特征向量计算，在矩阵分析中占有重要位置。基于Eigen的特征值计算如下：

Eigen::MatrixXd m = Eigen::MatrixXd::Random(3,3);
//构造一个实对称矩阵，SelfAdjointEigenSolver模板类，专门计算特征值和特征向量
Eigen::MatrixXd mTm = m.transpose() * m;//构成中心对其的协方差矩阵
 
//计算
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigen_solver(mTm);
 
//取出特征值和特征向量
Eigen::VectorXd eigenvalues = eigen_solver.eigenvalues();
Eigen::MatrixXd eigenvectors = eigen_solver.eigenvectors();
 
Eigen::VectorXd v0 = eigenvectors.col(0);// 因为特征值一般按从小到大排列,所以col(0)就是最小特征值对应的特征向量

出现的错误

将角轴转换为旋转矩阵时，提示

不存在从 "Eigen::Matrix<double, 3, 3, 0, 3, 3> () const" 转换到 "Eigen::Matrix<double, 3, 3, 0, 3, 3>" 的适当构造函数C/C++(415)

toRotationMatrix() 方法要加冒号

Eigen::Matrix3d fai1_SO3 =  Eigen::AngleAxisd(fai1.norm(),fai1.normalized()).toRotationMatrix();
  Eigen::Matrix3d fai2_SO3=  Eigen::AngleAxisd(theta_fai2,a).toRotationMatrix();

常见用法

参考：

Eigen::MatrixXd和VectorXd的用法注意

Eigen高阶操作总结 — 子矩阵、块操作

Eigen学习（五）块操作

1、行优先和列优先

矩阵默认是列优先,向量只能是列优先.注意:在Eigen中行优先的矩阵会在其名字中包含有row，否则就是列优先。

2、<<输入是一行一行输入,不管该矩阵是否是行优先还是列优先.

在Eigen中重载了"<<"操作符，通过该操作符即可以一个一个元素的进行赋值，也可以一块一块的赋值。另外也可以使用下标进行复

3\索引:MatrixXd矩阵只能用(),VectorXd不仅能用()还能用[]

在矩阵的访问中，行索引总是作为第一个参数，需注意Eigen中遵循大家的习惯让矩阵、数组、向量的下标都是从0开始。矩阵元素的访问可以通过（）操作符完成，例如m(2,3)即是获取矩阵m的第2行第3列元素（注意行列数从0开始）

4、重置矩阵大小

当前矩阵的行数、列数、大小可以通过rows()，cols()和size()来获取，对于动态矩阵可以通过resize()函数来动态修改矩阵的大小.

需注意：

(1) 固定大小的矩阵是不能使用resize（）来修改矩阵的大小；

(2) resize（）函数会析构掉原来的数据，因此调用resize（）函数之后将不能保证元素的值不改变。
(3) 使用“=”操作符操作动态矩阵时，如果左右边的矩阵大小不等，则左边的动态矩阵的大小会被修改为右边的大小。

5、MatrixXd和Vector2d的构造 注意!

矩阵的构造函数中只提供行列数、元素类型的构造参数，而不提供元素值的构造，对于比较小的、固定长度的向量提供初始化元素的定义，

6、矩阵的块操作：有三种使用方法：

matrix.block(i,j, p, q) : 表示返回从矩阵(i, j)开始，每行取p个元素，每列取q个元素；

matrix.block<p,q>(i, j) :<p, q>可理解为一个p行q列的子矩阵，该定义表示从原矩阵中第(i, j)开始，获取一个p行q列的子矩阵；